AP_GPS: use references for state variable

Author: peterbarker

libraries/AP_GPS/AP_GPS_Blended.cpp

@@ -35,16 +35,17 @@ bool AP_GPS_Blended::_calc_weights(void)
     uint32_t max_rate_ms = 0; // largest update interval of a GPS receiver
     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         // Find largest and smallest times
-        if (gps.state[i].last_gps_time_ms > max_ms) {
-            max_ms = gps.state[i].last_gps_time_ms;
+        const auto &state_i = gps.state[i];
+        if (state_i.last_gps_time_ms > max_ms) {
+            max_ms = state_i.last_gps_time_ms;
         }
-        if ((gps.state[i].last_gps_time_ms < min_ms) && (gps.state[i].last_gps_time_ms > 0)) {
-            min_ms = gps.state[i].last_gps_time_ms;
+        if ((state_i.last_gps_time_ms < min_ms) && (state_i.last_gps_time_ms > 0)) {
+            min_ms = state_i.last_gps_time_ms;
         }
         max_rate_ms = MAX(gps.get_rate_ms(i), max_rate_ms);
-        if (isinf(gps.state[i].speed_accuracy) ||
-            isinf(gps.state[i].horizontal_accuracy) ||
-            isinf(gps.state[i].vertical_accuracy)) {
+        if (isinf(state_i.speed_accuracy) ||
+            isinf(state_i.horizontal_accuracy) ||
+            isinf(state_i.vertical_accuracy)) {
             return false;
         }
     }
@@ -60,9 +61,10 @@ bool AP_GPS_Blended::_calc_weights(void)
     float speed_accuracy_sum_sq = 0.0f;
     if (gps._blend_mask & BLEND_MASK_USE_SPD_ACC) {
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_3D) {
-                if (gps.state[i].have_speed_accuracy && gps.state[i].speed_accuracy > 0.0f) {
-                    speed_accuracy_sum_sq += sq(gps.state[i].speed_accuracy);
+            const auto &state_i = gps.state[i];
+            if (state_i.status >= AP_GPS::GPS_OK_FIX_3D) {
+                if (state_i.have_speed_accuracy && state_i.speed_accuracy > 0.0f) {
+                    speed_accuracy_sum_sq += sq(state_i.speed_accuracy);
                 } else {
                     // not all receivers support this metric so set it to zero and don't use it
                     speed_accuracy_sum_sq = 0.0f;
@@ -76,9 +78,10 @@ bool AP_GPS_Blended::_calc_weights(void)
     float horizontal_accuracy_sum_sq = 0.0f;
     if (gps._blend_mask & BLEND_MASK_USE_HPOS_ACC) {
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_2D) {
-                if (gps.state[i].have_horizontal_accuracy && gps.state[i].horizontal_accuracy > 0.0f) {
-                    horizontal_accuracy_sum_sq += sq(gps.state[i].horizontal_accuracy);
+            const auto &state_i = gps.state[i];
+            if (state_i.status >= AP_GPS::GPS_OK_FIX_2D) {
+                if (state_i.have_horizontal_accuracy && state_i.horizontal_accuracy > 0.0f) {
+                    horizontal_accuracy_sum_sq += sq(state_i.horizontal_accuracy);
                 } else {
                     // not all receivers support this metric so set it to zero and don't use it
                     horizontal_accuracy_sum_sq = 0.0f;
@@ -92,9 +95,10 @@ bool AP_GPS_Blended::_calc_weights(void)
     float vertical_accuracy_sum_sq = 0.0f;
     if (gps._blend_mask & BLEND_MASK_USE_VPOS_ACC) {
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_3D) {
-                if (gps.state[i].have_vertical_accuracy && gps.state[i].vertical_accuracy > 0.0f) {
-                    vertical_accuracy_sum_sq += sq(gps.state[i].vertical_accuracy);
+            const auto &state_i = gps.state[i];
+            if (state_i.status >= AP_GPS::GPS_OK_FIX_3D) {
+                if (state_i.have_vertical_accuracy && state_i.vertical_accuracy > 0.0f) {
+                    vertical_accuracy_sum_sq += sq(state_i.vertical_accuracy);
                 } else {
                     // not all receivers support this metric so set it to zero and don't use it
                     vertical_accuracy_sum_sq = 0.0f;
@@ -119,8 +123,9 @@ bool AP_GPS_Blended::_calc_weights(void)
         // calculate the weights using the inverse of the variances
         float sum_of_hpos_weights = 0.0f;
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_2D && gps.state[i].horizontal_accuracy >= 0.001f) {
-                hpos_blend_weights[i] = horizontal_accuracy_sum_sq / sq(gps.state[i].horizontal_accuracy);
+            const auto &state_i = gps.state[i];
+            if (state_i.status >= AP_GPS::GPS_OK_FIX_2D && state_i.horizontal_accuracy >= 0.001f) {
+                hpos_blend_weights[i] = horizontal_accuracy_sum_sq / sq(state_i.horizontal_accuracy);
                 sum_of_hpos_weights += hpos_blend_weights[i];
             }
         }
@@ -139,8 +144,9 @@ bool AP_GPS_Blended::_calc_weights(void)
         // calculate the weights using the inverse of the variances
         float sum_of_vpos_weights = 0.0f;
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_3D && gps.state[i].vertical_accuracy >= 0.001f) {
-                vpos_blend_weights[i] = vertical_accuracy_sum_sq / sq(gps.state[i].vertical_accuracy);
+            const auto &state_i = gps.state[i];
+            if (state_i.status >= AP_GPS::GPS_OK_FIX_3D && state_i.vertical_accuracy >= 0.001f) {
+                vpos_blend_weights[i] = vertical_accuracy_sum_sq / sq(state_i.vertical_accuracy);
                 sum_of_vpos_weights += vpos_blend_weights[i];
             }
         }
@@ -159,8 +165,9 @@ bool AP_GPS_Blended::_calc_weights(void)
         // calculate the weights using the inverse of the variances
         float sum_of_spd_weights = 0.0f;
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_3D && gps.state[i].speed_accuracy >= 0.001f) {
-                spd_blend_weights[i] = speed_accuracy_sum_sq / sq(gps.state[i].speed_accuracy);
+            const auto &state_i = gps.state[i];
+            if (state_i.status >= AP_GPS::GPS_OK_FIX_3D && state_i.speed_accuracy >= 0.001f) {
+                spd_blend_weights[i] = speed_accuracy_sum_sq / sq(state_i.speed_accuracy);
                 sum_of_spd_weights += spd_blend_weights[i];
             }
         }
@@ -243,45 +250,46 @@ void AP_GPS_Blended::calc_state(void)
 
     // combine the states into a blended solution
     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
+        const auto &state_i = gps.state[i];
         // use the highest status
-        if (gps.state[i].status > state.status) {
-            state.status = gps.state[i].status;
+        if (state_i.status > state.status) {
+            state.status = state_i.status;
         }
 
         // calculate a blended average velocity
-        state.velocity += gps.state[i].velocity * _blend_weights[i];
+        state.velocity += state_i.velocity * _blend_weights[i];
 
         // report the best valid accuracies and DOP metrics
 
-        if (gps.state[i].have_horizontal_accuracy && gps.state[i].horizontal_accuracy > 0.0f && gps.state[i].horizontal_accuracy < state.horizontal_accuracy) {
+        if (state_i.have_horizontal_accuracy && state_i.horizontal_accuracy > 0.0f && state_i.horizontal_accuracy < state.horizontal_accuracy) {
             state.have_horizontal_accuracy = true;
-            state.horizontal_accuracy = gps.state[i].horizontal_accuracy;
+            state.horizontal_accuracy = state_i.horizontal_accuracy;
         }
 
-        if (gps.state[i].have_vertical_accuracy && gps.state[i].vertical_accuracy > 0.0f && gps.state[i].vertical_accuracy < state.vertical_accuracy) {
+        if (state_i.have_vertical_accuracy && state_i.vertical_accuracy > 0.0f && state_i.vertical_accuracy < state.vertical_accuracy) {
             state.have_vertical_accuracy = true;
-            state.vertical_accuracy = gps.state[i].vertical_accuracy;
+            state.vertical_accuracy = state_i.vertical_accuracy;
         }
 
-        if (gps.state[i].have_vertical_velocity) {
+        if (state_i.have_vertical_velocity) {
             state.have_vertical_velocity = true;
         }
 
-        if (gps.state[i].have_speed_accuracy && gps.state[i].speed_accuracy > 0.0f && gps.state[i].speed_accuracy < state.speed_accuracy) {
+        if (state_i.have_speed_accuracy && state_i.speed_accuracy > 0.0f && state_i.speed_accuracy < state.speed_accuracy) {
             state.have_speed_accuracy = true;
-            state.speed_accuracy = gps.state[i].speed_accuracy;
+            state.speed_accuracy = state_i.speed_accuracy;
         }
 
-        if (gps.state[i].hdop > 0 && gps.state[i].hdop < state.hdop) {
-            state.hdop = gps.state[i].hdop;
+        if (state_i.hdop > 0 && state_i.hdop < state.hdop) {
+            state.hdop = state_i.hdop;
         }
 
-        if (gps.state[i].vdop > 0 && gps.state[i].vdop < state.vdop) {
-            state.vdop = gps.state[i].vdop;
+        if (state_i.vdop > 0 && state_i.vdop < state.vdop) {
+            state.vdop = state_i.vdop;
         }
 
-        if (gps.state[i].num_sats > 0 && gps.state[i].num_sats > state.num_sats) {
-            state.num_sats = gps.state[i].num_sats;
+        if (state_i.num_sats > 0 && state_i.num_sats > state.num_sats) {
+            state.num_sats = state_i.num_sats;
         }
 
         // report a blended average GPS antenna position