From 5c6c6786f866671592fef796706955985da85d76 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 20 Mar 2024 22:01:56 +1100 Subject: [PATCH] AP_GPS: create real AP_GPS_Blended backend --- libraries/AP_Arming/AP_Arming.cpp | 6 - libraries/AP_GPS/AP_GPS.cpp | 45 +++--- libraries/AP_GPS/AP_GPS.h | 19 +-- libraries/AP_GPS/AP_GPS_Blended.cpp | 241 +++++++++++++++------------- libraries/AP_GPS/AP_GPS_Blended.h | 74 +++++++++ 5 files changed, 226 insertions(+), 159 deletions(-) create mode 100644 libraries/AP_GPS/AP_GPS_Blended.h diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 5b1bd0d2f95ec1..593da9840dd449 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -649,12 +649,6 @@ bool AP_Arming::gps_checks(bool report) (double)distance_m); return false; } -#if AP_GPS_BLENDED_ENABLED - if (!gps.blend_health_check()) { - check_failed(ARMING_CHECK_GPS, report, "GPS blending unhealthy"); - return false; - } -#endif // check AHRS and GPS are within 10m of each other if (gps.num_sensors() > 0) { diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 73829001c8771d..70c01c466059cd 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -29,6 +29,7 @@ #include #include "AP_GPS_NOVA.h" +#include "AP_GPS_Blended.h" #include "AP_GPS_ERB.h" #include "AP_GPS_GSOF.h" #include "AP_GPS_NMEA.h" @@ -66,8 +67,6 @@ #define GPS_BAUD_TIME_MS 1200 #define GPS_TIMEOUT_MS 4000u -#define BLEND_COUNTER_FAILURE_INCREMENT 10 - extern const AP_HAL::HAL &hal; // baudrates to try to detect GPSes with @@ -347,6 +346,11 @@ void AP_GPS::init() rate_ms.set(GPS_MAX_RATE_MS); } } + + // create the blended instance if appropriate: +#if AP_GPS_BLENDED_ENABLED + drivers[GPS_BLENDED_INSTANCE] = new AP_GPS_Blended(*this, params[GPS_BLENDED_INSTANCE], state[GPS_BLENDED_INSTANCE], timing[GPS_BLENDED_INSTANCE]); +#endif } void AP_GPS::convert_parameters() @@ -1084,25 +1088,15 @@ void AP_GPS::update_primary(void) */ const bool using_moving_base = is_rtk_base(0) || is_rtk_base(1); if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::BLEND && !using_moving_base) { - _output_is_blended = calc_blend_weights(); - // adjust blend health counter - if (!_output_is_blended) { - _blend_health_counter = MIN(_blend_health_counter+BLEND_COUNTER_FAILURE_INCREMENT, 100); - } else if (_blend_health_counter > 0) { - _blend_health_counter--; - } - // stop blending if unhealthy - if (_blend_health_counter >= 50) { - _output_is_blended = false; - } + _output_is_blended = ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->calc_weights(); } else { _output_is_blended = false; - _blend_health_counter = 0; + ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->zero_health_counter(); } if (_output_is_blended) { // Use the weighting to calculate blended GPS states - calc_blended_state(); + ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->calc_state(); // set primary to the virtual instance primary_instance = GPS_BLENDED_INSTANCE; return; @@ -1695,10 +1689,7 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const #if AP_GPS_BLENDED_ENABLED // return lag of blended GPS if (instance == GPS_BLENDED_INSTANCE) { - lag_sec = _blended_lag_sec; - // auto switching uses all GPS receivers, so all must be configured - uint8_t inst; // we don't actually care what the number is, but must pass it - return first_unconfigured_gps(inst); + return drivers[instance]->get_lag(lag_sec); } #endif @@ -1732,7 +1723,7 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const #if AP_GPS_BLENDED_ENABLED if (instance == GPS_BLENDED_INSTANCE) { // return an offset for the blended GPS solution - return _blended_antenna_offset; + return ((AP_GPS_Blended*)drivers[instance])->get_antenna_offset(); } #endif @@ -1788,12 +1779,6 @@ bool AP_GPS::is_healthy(uint8_t instance) const } #endif // HAL_BUILD_AP_PERIPH -#if AP_GPS_BLENDED_ENABLED - if (instance == GPS_BLENDED_INSTANCE) { - return blend_health_check(); - } -#endif - return drivers[instance] != nullptr && drivers[instance]->is_healthy(); } @@ -1827,6 +1812,14 @@ bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) { } } } + +#if defined(GPS_BLENDED_INSTANCE) + if (!drivers[GPS_BLENDED_INSTANCE]->is_healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, "GPS blending unhealthy"); + return false; + } +#endif + return true; } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index e45aef88c44c95..ea7c62ce29df51 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -52,6 +52,7 @@ class RTCM3_Parser; /// GPS driver main class class AP_GPS { + friend class AP_GPS_Blended; friend class AP_GPS_ERB; friend class AP_GPS_GSOF; friend class AP_GPS_MAV; @@ -512,9 +513,6 @@ class AP_GPS // pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned bool all_consistent(float &distance) const; - // pre-arm check of GPS blending. False if blending is unhealthy, True if healthy or blending is not being used - bool blend_health_check() const; - // handle sending of initialisation strings to the GPS - only used by backends void send_blob_start(uint8_t instance); void send_blob_start(uint8_t instance, const char *_blob, uint16_t size); @@ -606,7 +604,7 @@ class AP_GPS protected: // configuration parameters - Params params[GPS_MAX_RECEIVERS]; + Params params[GPS_MAX_INSTANCES]; AP_Int8 _navfilter; AP_Int8 _auto_switch; AP_Int16 _sbp_logmask; @@ -668,7 +666,7 @@ class AP_GPS // Note allowance for an additional instance to contain blended data GPS_timing timing[GPS_MAX_INSTANCES]; GPS_State state[GPS_MAX_INSTANCES]; - AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS]; + AP_GPS_Backend *drivers[GPS_MAX_INSTANCES]; AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS]; /// primary GPS instance @@ -756,18 +754,7 @@ class AP_GPS void inject_data(uint8_t instance, const uint8_t *data, uint16_t len); #if AP_GPS_BLENDED_ENABLED - // GPS blending and switching - Vector3f _blended_antenna_offset; // blended antenna offset - float _blended_lag_sec; // blended receiver lag in seconds - float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances. bool _output_is_blended; // true when a blended GPS solution being output - uint8_t _blend_health_counter; // 0 = perfectly health, 100 = very unhealthy - - // calculate the blend weight. Returns true if blend could be calculated, false if not - bool calc_blend_weights(void); - - // calculate the blended state - void calc_blended_state(void); #endif bool should_log() const; diff --git a/libraries/AP_GPS/AP_GPS_Blended.cpp b/libraries/AP_GPS/AP_GPS_Blended.cpp index 071b377817cc52..47deea9f4b3292 100644 --- a/libraries/AP_GPS/AP_GPS_Blended.cpp +++ b/libraries/AP_GPS/AP_GPS_Blended.cpp @@ -1,22 +1,20 @@ -#include "AP_GPS.h" +#include "AP_GPS_config.h" #if AP_GPS_BLENDED_ENABLED +#include "AP_GPS_Blended.h" + // defines used to specify the mask position for use of different accuracy metrics in the blending algorithm #define BLEND_MASK_USE_HPOS_ACC 1 #define BLEND_MASK_USE_VPOS_ACC 2 #define BLEND_MASK_USE_SPD_ACC 4 -// pre-arm check of GPS blending. True means healthy or that blending is not being used -bool AP_GPS::blend_health_check() const -{ - return (_blend_health_counter < 50); -} +#define BLEND_COUNTER_FAILURE_INCREMENT 10 /* calculate the weightings used to blend GPSs location and velocity data */ -bool AP_GPS::calc_blend_weights(void) +bool AP_GPS_Blended::_calc_weights(void) { // zero the blend weights memset(&_blend_weights, 0, sizeof(_blend_weights)); @@ -27,7 +25,7 @@ bool AP_GPS::calc_blend_weights(void) // The time delta calculations below also rely upon every instance being currently detected and being parsed // exit immediately if not enough receivers to do blending - if (state[0].status <= NO_FIX || state[1].status <= NO_FIX) { + if (gps.state[0].status <= AP_GPS::NO_FIX || gps.state[1].status <= AP_GPS::NO_FIX) { return false; } @@ -37,22 +35,22 @@ bool AP_GPS::calc_blend_weights(void) uint32_t max_rate_ms = 0; // largest update interval of a GPS receiver for (uint8_t i=0; i max_ms) { - max_ms = state[i].last_gps_time_ms; + if (gps.state[i].last_gps_time_ms > max_ms) { + max_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; + 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; } - max_rate_ms = MAX(get_rate_ms(i), max_rate_ms); - if (isinf(state[i].speed_accuracy) || - isinf(state[i].horizontal_accuracy) || - isinf(state[i].vertical_accuracy)) { + 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)) { return false; } } if ((max_ms - min_ms) < (2 * max_rate_ms)) { // data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated - state[GPS_BLENDED_INSTANCE].last_gps_time_ms = min_ms; + state.last_gps_time_ms = min_ms; } else { // receiver data has timed out so fail out of blending return false; @@ -60,11 +58,11 @@ bool AP_GPS::calc_blend_weights(void) // calculate the sum squared speed accuracy across all GPS sensors float speed_accuracy_sum_sq = 0.0f; - if (_blend_mask & BLEND_MASK_USE_SPD_ACC) { + if (gps._blend_mask & BLEND_MASK_USE_SPD_ACC) { for (uint8_t i=0; i= 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); + 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); } else { // not all receivers support this metric so set it to zero and don't use it speed_accuracy_sum_sq = 0.0f; @@ -76,11 +74,11 @@ bool AP_GPS::calc_blend_weights(void) // calculate the sum squared horizontal position accuracy across all GPS sensors float horizontal_accuracy_sum_sq = 0.0f; - if (_blend_mask & BLEND_MASK_USE_HPOS_ACC) { + if (gps._blend_mask & BLEND_MASK_USE_HPOS_ACC) { for (uint8_t i=0; i= 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); + 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); } else { // not all receivers support this metric so set it to zero and don't use it horizontal_accuracy_sum_sq = 0.0f; @@ -92,11 +90,11 @@ bool AP_GPS::calc_blend_weights(void) // calculate the sum squared vertical position accuracy across all GPS sensors float vertical_accuracy_sum_sq = 0.0f; - if (_blend_mask & BLEND_MASK_USE_VPOS_ACC) { + if (gps._blend_mask & BLEND_MASK_USE_VPOS_ACC) { for (uint8_t i=0; i= 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); + 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); } else { // not all receivers support this metric so set it to zero and don't use it vertical_accuracy_sum_sq = 0.0f; @@ -121,8 +119,8 @@ bool AP_GPS::calc_blend_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_OK_FIX_2D && state[i].horizontal_accuracy >= 0.001f) { - hpos_blend_weights[i] = horizontal_accuracy_sum_sq / sq(state[i].horizontal_accuracy); + 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); sum_of_hpos_weights += hpos_blend_weights[i]; } } @@ -141,8 +139,8 @@ bool AP_GPS::calc_blend_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_OK_FIX_3D && state[i].vertical_accuracy >= 0.001f) { - vpos_blend_weights[i] = vertical_accuracy_sum_sq / sq(state[i].vertical_accuracy); + 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); sum_of_vpos_weights += vpos_blend_weights[i]; } } @@ -161,8 +159,8 @@ bool AP_GPS::calc_blend_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_OK_FIX_3D && state[i].speed_accuracy >= 0.001f) { - spd_blend_weights[i] = speed_accuracy_sum_sq / sq(state[i].speed_accuracy); + 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); sum_of_spd_weights += spd_blend_weights[i]; } } @@ -187,104 +185,116 @@ bool AP_GPS::calc_blend_weights(void) return true; } +bool AP_GPS_Blended::calc_weights() +{ + // adjust blend health counter + if (!_calc_weights()) { + _blend_health_counter = MIN(_blend_health_counter+BLEND_COUNTER_FAILURE_INCREMENT, 100); + } else if (_blend_health_counter > 0) { + _blend_health_counter--; + } + // stop blending if unhealthy + return _blend_health_counter < 50; +} + /* calculate a blended GPS state */ -void AP_GPS::calc_blended_state(void) +void AP_GPS_Blended::calc_state(void) { // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver - state[GPS_BLENDED_INSTANCE].instance = GPS_BLENDED_INSTANCE; - state[GPS_BLENDED_INSTANCE].status = NO_FIX; - state[GPS_BLENDED_INSTANCE].time_week_ms = 0; - state[GPS_BLENDED_INSTANCE].time_week = 0; - state[GPS_BLENDED_INSTANCE].ground_speed = 0.0f; - state[GPS_BLENDED_INSTANCE].ground_course = 0.0f; - state[GPS_BLENDED_INSTANCE].hdop = GPS_UNKNOWN_DOP; - state[GPS_BLENDED_INSTANCE].vdop = GPS_UNKNOWN_DOP; - state[GPS_BLENDED_INSTANCE].num_sats = 0; - state[GPS_BLENDED_INSTANCE].velocity.zero(); - state[GPS_BLENDED_INSTANCE].speed_accuracy = 1e6f; - state[GPS_BLENDED_INSTANCE].horizontal_accuracy = 1e6f; - state[GPS_BLENDED_INSTANCE].vertical_accuracy = 1e6f; - state[GPS_BLENDED_INSTANCE].have_vertical_velocity = false; - state[GPS_BLENDED_INSTANCE].have_speed_accuracy = false; - state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = false; - state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = false; - state[GPS_BLENDED_INSTANCE].location = {}; + state.instance = GPS_BLENDED_INSTANCE; + state.status = AP_GPS::NO_FIX; + state.time_week_ms = 0; + state.time_week = 0; + state.ground_speed = 0.0f; + state.ground_course = 0.0f; + state.hdop = GPS_UNKNOWN_DOP; + state.vdop = GPS_UNKNOWN_DOP; + state.num_sats = 0; + state.velocity.zero(); + state.speed_accuracy = 1e6f; + state.horizontal_accuracy = 1e6f; + state.vertical_accuracy = 1e6f; + state.have_vertical_velocity = false; + state.have_speed_accuracy = false; + state.have_horizontal_accuracy = false; + state.have_vertical_accuracy = false; + state.location = {}; _blended_antenna_offset.zero(); _blended_lag_sec = 0; #if HAL_LOGGING_ENABLED - const uint32_t last_blended_message_time_ms = timing[GPS_BLENDED_INSTANCE].last_message_time_ms; + const uint32_t last_blended_message_time_ms = timing.last_message_time_ms; #endif - timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = 0; - timing[GPS_BLENDED_INSTANCE].last_message_time_ms = 0; - - if (state[0].have_undulation) { - state[GPS_BLENDED_INSTANCE].have_undulation = true; - state[GPS_BLENDED_INSTANCE].undulation = state[0].undulation; - } else if (state[1].have_undulation) { - state[GPS_BLENDED_INSTANCE].have_undulation = true; - state[GPS_BLENDED_INSTANCE].undulation = state[1].undulation; + timing.last_fix_time_ms = 0; + timing.last_message_time_ms = 0; + + if (gps.state[0].have_undulation) { + state.have_undulation = true; + state.undulation = gps.state[0].undulation; + } else if (gps.state[1].have_undulation) { + state.have_undulation = true; + state.undulation = gps.state[1].undulation; } else { - state[GPS_BLENDED_INSTANCE].have_undulation = false; + state.have_undulation = false; } // combine the states into a blended solution for (uint8_t i=0; i state[GPS_BLENDED_INSTANCE].status) { - state[GPS_BLENDED_INSTANCE].status = state[i].status; + if (gps.state[i].status > state.status) { + state.status = gps.state[i].status; } // calculate a blended average velocity - state[GPS_BLENDED_INSTANCE].velocity += state[i].velocity * _blend_weights[i]; + state.velocity += gps.state[i].velocity * _blend_weights[i]; // report the best valid accuracies and DOP metrics - if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f && state[i].horizontal_accuracy < state[GPS_BLENDED_INSTANCE].horizontal_accuracy) { - state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = true; - state[GPS_BLENDED_INSTANCE].horizontal_accuracy = state[i].horizontal_accuracy; + if (gps.state[i].have_horizontal_accuracy && gps.state[i].horizontal_accuracy > 0.0f && gps.state[i].horizontal_accuracy < state.horizontal_accuracy) { + state.have_horizontal_accuracy = true; + state.horizontal_accuracy = gps.state[i].horizontal_accuracy; } - if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f && state[i].vertical_accuracy < state[GPS_BLENDED_INSTANCE].vertical_accuracy) { - state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = true; - state[GPS_BLENDED_INSTANCE].vertical_accuracy = state[i].vertical_accuracy; + if (gps.state[i].have_vertical_accuracy && gps.state[i].vertical_accuracy > 0.0f && gps.state[i].vertical_accuracy < state.vertical_accuracy) { + state.have_vertical_accuracy = true; + state.vertical_accuracy = gps.state[i].vertical_accuracy; } - if (state[i].have_vertical_velocity) { - state[GPS_BLENDED_INSTANCE].have_vertical_velocity = true; + if (gps.state[i].have_vertical_velocity) { + state.have_vertical_velocity = true; } - if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f && state[i].speed_accuracy < state[GPS_BLENDED_INSTANCE].speed_accuracy) { - state[GPS_BLENDED_INSTANCE].have_speed_accuracy = true; - state[GPS_BLENDED_INSTANCE].speed_accuracy = state[i].speed_accuracy; + if (gps.state[i].have_speed_accuracy && gps.state[i].speed_accuracy > 0.0f && gps.state[i].speed_accuracy < state.speed_accuracy) { + state.have_speed_accuracy = true; + state.speed_accuracy = gps.state[i].speed_accuracy; } - if (state[i].hdop > 0 && state[i].hdop < state[GPS_BLENDED_INSTANCE].hdop) { - state[GPS_BLENDED_INSTANCE].hdop = state[i].hdop; + if (gps.state[i].hdop > 0 && gps.state[i].hdop < state.hdop) { + state.hdop = gps.state[i].hdop; } - if (state[i].vdop > 0 && state[i].vdop < state[GPS_BLENDED_INSTANCE].vdop) { - state[GPS_BLENDED_INSTANCE].vdop = state[i].vdop; + if (gps.state[i].vdop > 0 && gps.state[i].vdop < state.vdop) { + state.vdop = gps.state[i].vdop; } - if (state[i].num_sats > 0 && state[i].num_sats > state[GPS_BLENDED_INSTANCE].num_sats) { - state[GPS_BLENDED_INSTANCE].num_sats = state[i].num_sats; + if (gps.state[i].num_sats > 0 && gps.state[i].num_sats > state.num_sats) { + state.num_sats = gps.state[i].num_sats; } // report a blended average GPS antenna position - Vector3f temp_antenna_offset = params[i].antenna_offset; + Vector3f temp_antenna_offset = gps.params[i].antenna_offset; temp_antenna_offset *= _blend_weights[i]; _blended_antenna_offset += temp_antenna_offset; // blend the timing data - if (timing[i].last_fix_time_ms > timing[GPS_BLENDED_INSTANCE].last_fix_time_ms) { - timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = timing[i].last_fix_time_ms; + if (gps.timing[i].last_fix_time_ms > timing.last_fix_time_ms) { + timing.last_fix_time_ms = gps.timing[i].last_fix_time_ms; } - if (timing[i].last_message_time_ms > timing[GPS_BLENDED_INSTANCE].last_message_time_ms) { - timing[GPS_BLENDED_INSTANCE].last_message_time_ms = timing[i].last_message_time_ms; + if (gps.timing[i].last_message_time_ms > timing.last_message_time_ms) { + timing.last_message_time_ms = gps.timing[i].last_message_time_ms; } } @@ -300,7 +310,7 @@ void AP_GPS::calc_blended_state(void) if (_blend_weights[i] > best_weight) { best_weight = _blend_weights[i]; best_index = i; - state[GPS_BLENDED_INSTANCE].location = state[i].location; + state.location = gps.state[i].location; } } @@ -310,18 +320,18 @@ void AP_GPS::calc_blended_state(void) blended_NE_offset_m.zero(); for (uint8_t i=0; i 0.0f && i != best_index) { - blended_NE_offset_m += state[GPS_BLENDED_INSTANCE].location.get_distance_NE(state[i].location) * _blend_weights[i]; - blended_alt_offset_cm += (float)(state[i].location.alt - state[GPS_BLENDED_INSTANCE].location.alt) * _blend_weights[i]; + blended_NE_offset_m += state.location.get_distance_NE(gps.state[i].location) * _blend_weights[i]; + blended_alt_offset_cm += (float)(gps.state[i].location.alt - state.location.alt) * _blend_weights[i]; } } // Add the sum of weighted offsets to the reference location to obtain the blended location - state[GPS_BLENDED_INSTANCE].location.offset(blended_NE_offset_m.x, blended_NE_offset_m.y); - state[GPS_BLENDED_INSTANCE].location.alt += (int)blended_alt_offset_cm; + state.location.offset(blended_NE_offset_m.x, blended_NE_offset_m.y); + state.location.alt += (int)blended_alt_offset_cm; // Calculate ground speed and course from blended velocity vector - state[GPS_BLENDED_INSTANCE].ground_speed = state[GPS_BLENDED_INSTANCE].velocity.xy().length(); - state[GPS_BLENDED_INSTANCE].ground_course = wrap_360(degrees(atan2f(state[GPS_BLENDED_INSTANCE].velocity.y, state[GPS_BLENDED_INSTANCE].velocity.x))); + state.ground_speed = state.velocity.xy().length(); + state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); // If the GPS week is the same then use a blended time_week_ms // If week is different, then use time stamp from GPS with largest weighting @@ -331,8 +341,8 @@ void AP_GPS::calc_blended_state(void) for (uint8_t i=0; i 0) { // this is our first valid sensor week data - last_week_instance = state[i].time_week; - } else if (last_week_instance != 0 && _blend_weights[i] > 0 && last_week_instance != state[i].time_week) { + last_week_instance = gps.state[i].time_week; + } else if (last_week_instance != 0 && _blend_weights[i] > 0 && last_week_instance != gps.state[i].time_week) { // there is valid sensor week data that is inconsistent weeks_consistent = false; } @@ -340,19 +350,19 @@ void AP_GPS::calc_blended_state(void) // calculate output if (!weeks_consistent) { // use data from highest weighted sensor - state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week; - state[GPS_BLENDED_INSTANCE].time_week_ms = state[best_index].time_week_ms; + state.time_week = gps.state[best_index].time_week; + state.time_week_ms = gps.state[best_index].time_week_ms; } else { // use week number from highest weighting GPS (they should all have the same week number) - state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week; + state.time_week = gps.state[best_index].time_week; // calculate a blended value for the number of ms lapsed in the week double temp_time_0 = 0.0; for (uint8_t i=0; i 0.0f) { - temp_time_0 += (double)state[i].time_week_ms * (double)_blend_weights[i]; + temp_time_0 += (double)gps.state[i].time_week_ms * (double)_blend_weights[i]; } } - state[GPS_BLENDED_INSTANCE].time_week_ms = (uint32_t)temp_time_0; + state.time_week_ms = (uint32_t)temp_time_0; } // calculate a blended value for the timing data and lag @@ -360,21 +370,30 @@ void AP_GPS::calc_blended_state(void) double temp_time_2 = 0.0; for (uint8_t i=0; i 0.0f) { - temp_time_1 += (double)timing[i].last_fix_time_ms * (double) _blend_weights[i]; - temp_time_2 += (double)timing[i].last_message_time_ms * (double)_blend_weights[i]; + temp_time_1 += (double)gps.timing[i].last_fix_time_ms * (double) _blend_weights[i]; + temp_time_2 += (double)gps.timing[i].last_message_time_ms * (double)_blend_weights[i]; float gps_lag_sec = 0; - get_lag(i, gps_lag_sec); + gps.get_lag(i, gps_lag_sec); _blended_lag_sec += gps_lag_sec * _blend_weights[i]; } } - timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1; - timing[GPS_BLENDED_INSTANCE].last_message_time_ms = (uint32_t)temp_time_2; + timing.last_fix_time_ms = (uint32_t)temp_time_1; + timing.last_message_time_ms = (uint32_t)temp_time_2; #if HAL_LOGGING_ENABLED - if (timing[GPS_BLENDED_INSTANCE].last_message_time_ms > last_blended_message_time_ms && + if (timing.last_message_time_ms > last_blended_message_time_ms && should_log()) { - Write_GPS(GPS_BLENDED_INSTANCE); + gps.Write_GPS(GPS_BLENDED_INSTANCE); } #endif } + +bool AP_GPS_Blended::get_lag(float &lag_sec) const +{ + lag_sec = _blended_lag_sec; + // auto switching uses all GPS receivers, so all must be configured + uint8_t inst; // we don't actually care what the number is, but must pass it + return gps.first_unconfigured_gps(inst); +} + #endif // AP_GPS_BLENDED_ENABLED diff --git a/libraries/AP_GPS/AP_GPS_Blended.h b/libraries/AP_GPS/AP_GPS_Blended.h new file mode 100644 index 00000000000000..b68eb0f9660ed7 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_Blended.h @@ -0,0 +1,74 @@ +#pragma once + +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#include "AP_GPS_config.h" + +#if AP_GPS_BLENDED_ENABLED + +#include "AP_GPS.h" +#include "GPS_Backend.h" + +class AP_GPS_Blended : public AP_GPS_Backend +{ +public: + + AP_GPS_Blended(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, class AP_GPS::GPS_timing &_timing) : + AP_GPS_Backend(_gps, _params, _state, nullptr), + timing{_timing} + { } + + // pre-arm check of GPS blending. False if blending is unhealthy, + // True if healthy or blending is not being used + bool is_healthy() const override { + return (_blend_health_counter < 50); + } + + bool read() override { return true; } + + const char *name() const override { return "Blended"; } + + bool get_lag(float &lag_sec) const override; + const Vector3f &get_antenna_offset() const { + return _blended_antenna_offset; + } + + // calculate the blend weight. Returns true if blend could be + // calculated, false if not + bool calc_weights(void); + // calculate the blended state + void calc_state(void); + + void zero_health_counter() { + _blend_health_counter = 0; + } + +private: + + // GPS blending and switching + Vector3f _blended_antenna_offset; // blended antenna offset + float _blended_lag_sec; // blended receiver lag in seconds + float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances. + bool _output_is_blended; // true when a blended GPS solution being output + uint8_t _blend_health_counter; // 0 = perfectly health, 100 = very unhealthy + + AP_GPS::GPS_timing &timing; + bool _calc_weights(void); +}; + +#endif // AP_GPS_BLENDED_ENABLED