Line data    Source code

       1             : #include "AP_GPS_config.h"
       2             : 
       3             : #if AP_GPS_BLENDED_ENABLED
       4             : 
       5             : #include "AP_GPS_Blended.h"
       6             : 
       7             : // defines used to specify the mask position for use of different accuracy metrics in the blending algorithm
       8             : #define BLEND_MASK_USE_HPOS_ACC     1
       9             : #define BLEND_MASK_USE_VPOS_ACC     2
      10             : #define BLEND_MASK_USE_SPD_ACC      4
      11             : 
      12             : #define BLEND_COUNTER_FAILURE_INCREMENT 10
      13             : 
      14             : /*
      15             :  calculate the weightings used to blend GPSs location and velocity data
      16             : */
      17       17961 : bool AP_GPS_Blended::_calc_weights(void)
      18             : {
      19             :     static_assert(GPS_MAX_RECEIVERS == 2, "GPS blending only currently works with 2 receivers");
      20             :     // Note that the early quit below relies upon exactly 2 instances
      21             :     // The time delta calculations below also rely upon every instance being currently detected and being parsed
      22             : 
      23             :     // exit immediately if not enough receivers to do blending
      24       17961 :     if (gps.state[0].status <= AP_GPS_FixType::NONE || gps.state[1].status <= AP_GPS_FixType::NONE) {
      25        1742 :         return false;
      26             :     }
      27             : 
      28             :     // Use the oldest non-zero time, but if time difference is excessive, use newest to prevent a disconnected receiver from blocking updates
      29       16219 :     uint32_t max_ms = 0; // newest non-zero system time of arrival of a GPS message
      30       16219 :     uint32_t min_ms = -1; // oldest non-zero system time of arrival of a GPS message
      31       16219 :     uint32_t max_rate_ms = 0; // largest update interval of a GPS receiver
      32       48657 :     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
      33             :         // Find largest and smallest times
      34       32438 :         if (gps.state[i].last_gps_time_ms > max_ms) {
      35       16219 :             max_ms = gps.state[i].last_gps_time_ms;
      36             :         }
      37       32438 :         if ((gps.state[i].last_gps_time_ms < min_ms) && (gps.state[i].last_gps_time_ms > 0)) {
      38       16219 :             min_ms = gps.state[i].last_gps_time_ms;
      39             :         }
      40       32438 :         max_rate_ms = MAX(gps.get_rate_ms(i), max_rate_ms);
      41       64876 :         if (isinf(gps.state[i].speed_accuracy) ||
      42       64876 :             isinf(gps.state[i].horizontal_accuracy) ||
      43       32438 :             isinf(gps.state[i].vertical_accuracy)) {
      44           0 :             return false;
      45             :         }
      46             :     }
      47       16219 :     if ((max_ms - min_ms) < (2 * max_rate_ms)) {
      48             :         // data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated
      49       16219 :         state.last_gps_time_ms = min_ms;
      50             :     } else {
      51             :         // receiver data has timed out so fail out of blending
      52           0 :         return false;
      53             :     }
      54             : 
      55             :     // calculate the sum squared speed accuracy across all GPS sensors
      56       16219 :     float speed_accuracy_sum_sq = 0.0f;
      57       16219 :     if (gps._blend_mask & BLEND_MASK_USE_SPD_ACC) {
      58       25569 :         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
      59       20894 :             if (gps.state[i].status >= AP_GPS_FixType::FIX_3D) {
      60       20894 :                 if (gps.state[i].have_speed_accuracy && gps.state[i].speed_accuracy > 0.0f) {
      61        9350 :                     speed_accuracy_sum_sq += sq(gps.state[i].speed_accuracy);
      62             :                 } else {
      63             :                     // not all receivers support this metric so set it to zero and don't use it
      64       11544 :                     speed_accuracy_sum_sq = 0.0f;
      65       11544 :                     break;
      66             :                 }
      67             :             }
      68             :         }
      69             :     }
      70             : 
      71             :     // calculate the sum squared horizontal position accuracy across all GPS sensors
      72       16219 :     float horizontal_accuracy_sum_sq = 0.0f;
      73       16219 :     if (gps._blend_mask & BLEND_MASK_USE_HPOS_ACC) {
      74       34632 :         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
      75       23088 :             if (gps.state[i].status >= AP_GPS_FixType::FIX_2D) {
      76       23088 :                 if (gps.state[i].have_horizontal_accuracy && gps.state[i].horizontal_accuracy > 0.0f) {
      77       23088 :                     horizontal_accuracy_sum_sq += sq(gps.state[i].horizontal_accuracy);
      78             :                 } else {
      79             :                     // not all receivers support this metric so set it to zero and don't use it
      80           0 :                     horizontal_accuracy_sum_sq = 0.0f;
      81           0 :                     break;
      82             :                 }
      83             :             }
      84             :         }
      85             :     }
      86             : 
      87             :     // calculate the sum squared vertical position accuracy across all GPS sensors
      88       16219 :     float vertical_accuracy_sum_sq = 0.0f;
      89       16219 :     if (gps._blend_mask & BLEND_MASK_USE_VPOS_ACC) {
      90           0 :         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
      91           0 :             if (gps.state[i].status >= AP_GPS_FixType::FIX_3D) {
      92           0 :                 if (gps.state[i].have_vertical_accuracy && gps.state[i].vertical_accuracy > 0.0f) {
      93           0 :                     vertical_accuracy_sum_sq += sq(gps.state[i].vertical_accuracy);
      94             :                 } else {
      95             :                     // not all receivers support this metric so set it to zero and don't use it
      96           0 :                     vertical_accuracy_sum_sq = 0.0f;
      97           0 :                     break;
      98             :                 }
      99             :             }
     100             :         }
     101             :     }
     102             :     // Check if we can do blending using reported accuracy
     103       16219 :     bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f || speed_accuracy_sum_sq > 0.0f);
     104             : 
     105             :     // if we can't do blending using reported accuracy, return false and hard switch logic will be used instead
     106       16219 :     if (!can_do_blending) {
     107           0 :         return false;
     108             :     }
     109             : 
     110       16219 :     float sum_of_all_weights = 0.0f;
     111             : 
     112             :     // calculate a weighting using the reported horizontal position
     113       16219 :     float hpos_blend_weights[GPS_MAX_RECEIVERS] = {};
     114       16219 :     if (horizontal_accuracy_sum_sq > 0.0f) {
     115             :         // calculate the weights using the inverse of the variances
     116       11544 :         float sum_of_hpos_weights = 0.0f;
     117       34632 :         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     118       23088 :             if (gps.state[i].status >= AP_GPS_FixType::FIX_2D && gps.state[i].horizontal_accuracy >= 0.001f) {
     119       23088 :                 hpos_blend_weights[i] = horizontal_accuracy_sum_sq / sq(gps.state[i].horizontal_accuracy);
     120       23088 :                 sum_of_hpos_weights += hpos_blend_weights[i];
     121             :             }
     122             :         }
     123             :         // normalise the weights
     124       11544 :         if (sum_of_hpos_weights > 0.0f) {
     125       34632 :             for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     126       23088 :                 hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights;
     127             :             }
     128       11544 :             sum_of_all_weights += 1.0f;
     129             :         }
     130             :     }
     131             : 
     132             :     // calculate a weighting using the reported vertical position accuracy
     133       16219 :     float vpos_blend_weights[GPS_MAX_RECEIVERS] = {};
     134       16219 :     if (vertical_accuracy_sum_sq > 0.0f) {
     135             :         // calculate the weights using the inverse of the variances
     136           0 :         float sum_of_vpos_weights = 0.0f;
     137           0 :         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     138           0 :             if (gps.state[i].status >= AP_GPS_FixType::FIX_3D && gps.state[i].vertical_accuracy >= 0.001f) {
     139           0 :                 vpos_blend_weights[i] = vertical_accuracy_sum_sq / sq(gps.state[i].vertical_accuracy);
     140           0 :                 sum_of_vpos_weights += vpos_blend_weights[i];
     141             :             }
     142             :         }
     143             :         // normalise the weights
     144           0 :         if (sum_of_vpos_weights > 0.0f) {
     145           0 :             for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     146           0 :                 vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights;
     147             :             }
     148           0 :             sum_of_all_weights += 1.0f;
     149             :         };
     150             :     }
     151             : 
     152             :     // calculate a weighting using the reported speed accuracy
     153       16219 :     float spd_blend_weights[GPS_MAX_RECEIVERS] = {};
     154       16219 :     if (speed_accuracy_sum_sq > 0.0f) {
     155             :         // calculate the weights using the inverse of the variances
     156        4675 :         float sum_of_spd_weights = 0.0f;
     157       14025 :         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     158        9350 :             if (gps.state[i].status >= AP_GPS_FixType::FIX_3D && gps.state[i].speed_accuracy >= 0.001f) {
     159        9350 :                 spd_blend_weights[i] = speed_accuracy_sum_sq / sq(gps.state[i].speed_accuracy);
     160        9350 :                 sum_of_spd_weights += spd_blend_weights[i];
     161             :             }
     162             :         }
     163             :         // normalise the weights
     164        4675 :         if (sum_of_spd_weights > 0.0f) {
     165       14025 :             for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     166        9350 :                 spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights;
     167             :             }
     168        4675 :             sum_of_all_weights += 1.0f;
     169             :         }
     170             :     }
     171             : 
     172       16219 :     if (!is_positive(sum_of_all_weights)) {
     173           0 :         return false;
     174             :     }
     175             : 
     176             :     // calculate an overall weight
     177       48657 :     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     178       32438 :         _blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights;
     179             :     }
     180             : 
     181       16219 :     return true;
     182             : }
     183             : 
     184       17961 : bool AP_GPS_Blended::calc_weights()
     185             : {
     186             :     // adjust blend health counter
     187       17961 :     if (!_calc_weights()) {
     188        1742 :         _blend_health_counter = MIN(_blend_health_counter+BLEND_COUNTER_FAILURE_INCREMENT, 100);
     189       16219 :     } else if (_blend_health_counter > 0) {
     190         400 :         _blend_health_counter--;
     191             :     }
     192             : 
     193             :     // we are never healthy if we do not have any weights:
     194       17961 :     bool non_zero_weight_found = false;
     195       21423 :     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     196       19692 :         if (_blend_weights[i] > 0) {
     197       16230 :             non_zero_weight_found = true;
     198       16230 :             break;
     199             :         }
     200             :     }
     201       17961 :     if (!non_zero_weight_found) {
     202        1731 :         return false;
     203             :     }
     204             : 
     205             :     // stop blending if unhealthy
     206       16230 :     return _blend_health_counter < 50;
     207             : }
     208             : 
     209             : /*
     210             :  calculate a blended GPS state
     211             : */
     212       16024 : void AP_GPS_Blended::calc_state(void)
     213             : {
     214             :     // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver
     215       16024 :     state.instance = GPS_BLENDED_INSTANCE;
     216       16024 :     state.status = AP_GPS_FixType::NONE;
     217       16024 :     state.time_week_ms = 0;
     218       16024 :     state.time_week = 0;
     219       16024 :     state.ground_speed = 0.0f;
     220       16024 :     state.ground_course = 0.0f;
     221       16024 :     state.hdop = GPS_UNKNOWN_DOP;
     222       16024 :     state.vdop = GPS_UNKNOWN_DOP;
     223       16024 :     state.num_sats = 0;
     224       16024 :     state.velocity.zero();
     225       16024 :     state.speed_accuracy = 1e6f;
     226       16024 :     state.horizontal_accuracy = 1e6f;
     227       16024 :     state.vertical_accuracy = 1e6f;
     228       16024 :     state.have_vertical_velocity = false;
     229       16024 :     state.have_speed_accuracy = false;
     230       16024 :     state.have_horizontal_accuracy = false;
     231       16024 :     state.have_vertical_accuracy = false;
     232       16024 :     state.location = {};
     233             : 
     234       16024 :     _blended_antenna_offset.zero();
     235       16024 :     _blended_lag_sec = 0;
     236             : 
     237             : #if HAL_LOGGING_ENABLED
     238       16024 :     const uint32_t last_blended_message_time_ms = timing.last_message_time_ms;
     239             : #endif
     240       16024 :     timing.last_fix_time_ms = 0;
     241       16024 :     timing.last_message_time_ms = 0;
     242             : 
     243       16024 :     if (gps.state[0].have_undulation) {
     244       16024 :         state.have_undulation = true;
     245       16024 :         state.undulation = gps.state[0].undulation;
     246           0 :     } else if (gps.state[1].have_undulation) {
     247           0 :         state.have_undulation = true;
     248           0 :         state.undulation = gps.state[1].undulation;
     249             :     } else {
     250           0 :         state.have_undulation = false;
     251             :     }
     252             : 
     253             :     // combine the states into a blended solution
     254       48072 :     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     255             :         // use the highest status
     256       32048 :         if (gps.state[i].status > state.status) {
     257       16024 :             state.status = gps.state[i].status;
     258             :         }
     259             : 
     260             :         // calculate a blended average velocity
     261       32048 :         state.velocity += gps.state[i].velocity * _blend_weights[i];
     262             : 
     263             :         // report the best valid accuracies and DOP metrics
     264             : 
     265       32048 :         if (gps.state[i].have_horizontal_accuracy && gps.state[i].horizontal_accuracy > 0.0f && gps.state[i].horizontal_accuracy < state.horizontal_accuracy) {
     266       16024 :             state.have_horizontal_accuracy = true;
     267       16024 :             state.horizontal_accuracy = gps.state[i].horizontal_accuracy;
     268             :         }
     269             : 
     270       32048 :         if (gps.state[i].have_vertical_accuracy && gps.state[i].vertical_accuracy > 0.0f && gps.state[i].vertical_accuracy < state.vertical_accuracy) {
     271       16024 :             state.have_vertical_accuracy = true;
     272       16024 :             state.vertical_accuracy = gps.state[i].vertical_accuracy;
     273             :         }
     274             : 
     275       32048 :         if (gps.state[i].have_vertical_velocity) {
     276       32048 :             state.have_vertical_velocity = true;
     277             :         }
     278             : 
     279       32048 :         if (gps.state[i].have_speed_accuracy && gps.state[i].speed_accuracy > 0.0f && gps.state[i].speed_accuracy < state.speed_accuracy) {
     280        4625 :             state.have_speed_accuracy = true;
     281        4625 :             state.speed_accuracy = gps.state[i].speed_accuracy;
     282             :         }
     283             : 
     284       32048 :         if (gps.state[i].hdop > 0 && gps.state[i].hdop < state.hdop) {
     285       16024 :             state.hdop = gps.state[i].hdop;
     286             :         }
     287             : 
     288       32048 :         if (gps.state[i].vdop > 0 && gps.state[i].vdop < state.vdop) {
     289       16024 :             state.vdop = gps.state[i].vdop;
     290             :         }
     291             : 
     292       32048 :         if (gps.state[i].num_sats > 0 && gps.state[i].num_sats > state.num_sats) {
     293       16024 :             state.num_sats = gps.state[i].num_sats;
     294             :         }
     295             : 
     296             :         // report a blended average GPS antenna position
     297       32048 :         Vector3f temp_antenna_offset = gps.params[i].antenna_offset;
     298       32048 :         temp_antenna_offset *= _blend_weights[i];
     299       32048 :         _blended_antenna_offset += temp_antenna_offset;
     300             : 
     301             :         // blend the timing data
     302       32048 :         if (gps.timing[i].last_fix_time_ms > timing.last_fix_time_ms) {
     303       16024 :             timing.last_fix_time_ms = gps.timing[i].last_fix_time_ms;
     304             :         }
     305       32048 :         if (gps.timing[i].last_message_time_ms > timing.last_message_time_ms) {
     306       16024 :             timing.last_message_time_ms = gps.timing[i].last_message_time_ms;
     307             :         }
     308             :     }
     309             : 
     310             :     /*
     311             :      * Calculate an instantaneous weighted/blended average location from the available GPS instances and store in the _output_state.
     312             :      * This will be statistically the most likely location, but will be not stable enough for direct use by the autopilot.
     313             :     */
     314             : 
     315             :     // Use the GPS with the highest weighting as the reference position
     316       16024 :     float best_weight = 0.0f;
     317       16024 :     uint8_t best_index = 0;
     318       48072 :     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     319       32048 :         if (_blend_weights[i] > best_weight) {
     320       16024 :             best_weight = _blend_weights[i];
     321       16024 :             best_index = i;
     322       16024 :             state.location = gps.state[i].location;
     323             :         }
     324             :     }
     325             : 
     326             :     // Calculate the weighted sum of horizontal and vertical position offsets relative to the reference position
     327       16024 :     Vector2f blended_NE_offset_m;
     328       16024 :     float blended_alt_offset_cm = 0.0f;
     329       16024 :     blended_NE_offset_m.zero();
     330       48072 :     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     331       32048 :         if (_blend_weights[i] > 0.0f && i != best_index) {
     332       16024 :             blended_NE_offset_m += state.location.get_distance_NE(gps.state[i].location) * _blend_weights[i];
     333       16024 :             blended_alt_offset_cm += (float)(gps.state[i].location.alt - state.location.alt) * _blend_weights[i];
     334             :         }
     335             :     }
     336             : 
     337             :     // Add the sum of weighted offsets to the reference location to obtain the blended location
     338       16024 :     state.location.offset(blended_NE_offset_m.x, blended_NE_offset_m.y);
     339       16024 :     state.location.offset_up_cm(blended_alt_offset_cm);
     340             : 
     341             :     // Calculate ground speed and course from blended velocity vector
     342       16024 :     state.ground_speed = state.velocity.xy().length();
     343       16024 :     state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
     344             : 
     345             :     // If the GPS week is the same then use a blended time_week_ms
     346             :     // If week is different, then use time stamp from GPS with largest weighting
     347             :     // detect inconsistent week data
     348       16024 :     uint8_t last_week_instance = 0;
     349       16024 :     bool weeks_consistent = true;
     350       48072 :     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     351       32048 :         if (last_week_instance == 0 && _blend_weights[i] > 0) {
     352             :             // this is our first valid sensor week data
     353       16024 :             last_week_instance = gps.state[i].time_week;
     354       16024 :         } else if (last_week_instance != 0 && _blend_weights[i] > 0 && last_week_instance != gps.state[i].time_week) {
     355             :             // there is valid sensor week data that is inconsistent
     356       16024 :             weeks_consistent = false;
     357             :         }
     358             :     }
     359             :     // calculate output
     360       16024 :     if (!weeks_consistent) {
     361             :         // use data from highest weighted sensor
     362       16024 :         state.time_week = gps.state[best_index].time_week;
     363       16024 :         state.time_week_ms = gps.state[best_index].time_week_ms;
     364             :     } else {
     365             :         // use week number from highest weighting GPS (they should all have the same week number)
     366           0 :         state.time_week = gps.state[best_index].time_week;
     367             :         // calculate a blended value for the number of ms lapsed in the week
     368           0 :         double temp_time_0 = 0.0;
     369           0 :         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     370           0 :             if (_blend_weights[i] > 0.0f) {
     371           0 :                 temp_time_0 += (double)gps.state[i].time_week_ms * (double)_blend_weights[i];
     372             :             }
     373             :         }
     374           0 :         state.time_week_ms = (uint32_t)temp_time_0;
     375             :     }
     376             : 
     377             :     // calculate a blended value for the timing data and lag
     378       16024 :     double temp_time_1 = 0.0;
     379       16024 :     double temp_time_2 = 0.0;
     380       48072 :     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
     381       32048 :         if (_blend_weights[i] > 0.0f) {
     382       32048 :             temp_time_1 += (double)gps.timing[i].last_fix_time_ms * (double) _blend_weights[i];
     383       32048 :             temp_time_2 += (double)gps.timing[i].last_message_time_ms * (double)_blend_weights[i];
     384       32048 :             float gps_lag_sec = 0;
     385       32048 :             gps.get_lag(i, gps_lag_sec);
     386       32048 :             _blended_lag_sec += gps_lag_sec * _blend_weights[i];
     387             :         }
     388             :     }
     389       16024 :     timing.last_fix_time_ms = (uint32_t)temp_time_1;
     390       16024 :     timing.last_message_time_ms = (uint32_t)temp_time_2;
     391             : 
     392             : #if HAL_LOGGING_ENABLED
     393       17628 :     if (timing.last_message_time_ms > last_blended_message_time_ms &&
     394        1604 :         should_log()) {
     395        1604 :         gps.Write_GPS(GPS_BLENDED_INSTANCE);
     396             :     }
     397             : #endif
     398             : 
     399             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     400             :     // sanity checks
     401       16024 :     if (state.status > AP_GPS_FixType::NONE && !state.location.initialised()) {
     402           0 :         AP_HAL::panic("status is >NO_FIX but location is zero");
     403             :     }
     404             : #endif  // CONFIG_HAL_BOARD == HAL_BOARD_SITL
     405       16024 : }
     406             : 
     407    41284477 : bool AP_GPS_Blended::get_lag(float &lag_sec) const
     408             : {
     409    41284477 :         lag_sec = _blended_lag_sec;
     410             :         // auto switching uses all GPS receivers, so all must be configured
     411             :         uint8_t inst; // we don't actually care what the number is, but must pass it
     412    41284477 :         return gps.first_unconfigured_gps(inst);
     413             : }
     414             : 
     415             : #endif  // AP_GPS_BLENDED_ENABLED