AP_GPS: remove blended GPS from small boards

those boards which are not including all backends will lose blended after this
This commit is contained in:
Peter Barker 2024-03-21 14:43:23 +11:00 committed by Andrew Tridgell
parent c450de6e96
commit a0b2753766
5 changed files with 43 additions and 36 deletions

View File

@ -576,7 +576,7 @@ bool AP_Arming::gps_checks(bool report)
} }
for (uint8_t i = 0; i < gps.num_sensors(); i++) { for (uint8_t i = 0; i < gps.num_sensors(); i++) {
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
if ((i != GPS_BLENDED_INSTANCE) && if ((i != GPS_BLENDED_INSTANCE) &&
#else #else
if ( if (
@ -614,7 +614,7 @@ bool AP_Arming::gps_checks(bool report)
(double)distance_m); (double)distance_m);
return false; return false;
} }
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
if (!gps.blend_health_check()) { if (!gps.blend_health_check()) {
check_failed(ARMING_CHECK_GPS, report, "GPS blending unhealthy"); check_failed(ARMING_CHECK_GPS, report, "GPS blending unhealthy");
return false; return false;

View File

@ -214,7 +214,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// 19 was GPS_DELAY_MS2 // 19 was GPS_DELAY_MS2
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
// @Param: _BLEND_MASK // @Param: _BLEND_MASK
// @DisplayName: Multi GPS Blending Mask // @DisplayName: Multi GPS Blending Mask
// @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 2(Blend) // @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 2(Blend)
@ -400,7 +400,7 @@ void AP_GPS::convert_parameters()
// GPS solution is treated as an additional sensor. // GPS solution is treated as an additional sensor.
uint8_t AP_GPS::num_sensors(void) const uint8_t AP_GPS::num_sensors(void) const
{ {
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
if (_output_is_blended) { if (_output_is_blended) {
return num_instances+1; return num_instances+1;
} }
@ -1072,7 +1072,7 @@ void AP_GPS::update(void)
#if GPS_MAX_RECEIVERS > 1 #if GPS_MAX_RECEIVERS > 1
void AP_GPS::update_primary(void) void AP_GPS::update_primary(void)
{ {
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
/* /*
if blending is requested, attempt to calculate weighting for if blending is requested, attempt to calculate weighting for
each GPS each GPS
@ -1105,7 +1105,7 @@ void AP_GPS::update_primary(void)
primary_instance = GPS_BLENDED_INSTANCE; primary_instance = GPS_BLENDED_INSTANCE;
return; return;
} }
#endif // defined (GPS_BLENDED_INSTANCE) #endif // AP_GPS_BLENDED_ENABLED
// check the primary param is set to possible GPS // check the primary param is set to possible GPS
int8_t primary_param = _primary.get(); int8_t primary_param = _primary.get();
@ -1145,7 +1145,7 @@ void AP_GPS::update_primary(void)
} }
} }
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
// handling switching away from blended GPS // handling switching away from blended GPS
if (primary_instance == GPS_BLENDED_INSTANCE) { if (primary_instance == GPS_BLENDED_INSTANCE) {
primary_instance = 0; primary_instance = 0;
@ -1180,7 +1180,7 @@ void AP_GPS::update_primary(void)
_last_instance_swap_ms = now; _last_instance_swap_ms = now;
return; return;
} }
#endif // defined(GPS_BLENDED_INSTANCE) #endif // AP_GPS_BLENDED_ENABLED
// Use primary if 3D fix or better // Use primary if 3D fix or better
if (((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::USE_PRIMARY_IF_3D_FIX) && (state[primary_param].status >= GPS_OK_FIX_3D)) { if (((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::USE_PRIMARY_IF_3D_FIX) && (state[primary_param].status >= GPS_OK_FIX_3D)) {
@ -1690,7 +1690,7 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
return false; return false;
} }
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
// return lag of blended GPS // return lag of blended GPS
if (instance == GPS_BLENDED_INSTANCE) { if (instance == GPS_BLENDED_INSTANCE) {
lag_sec = _blended_lag_sec; lag_sec = _blended_lag_sec;
@ -1727,7 +1727,7 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const
return params[0].antenna_offset; return params[0].antenna_offset;
} }
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
if (instance == GPS_BLENDED_INSTANCE) { if (instance == GPS_BLENDED_INSTANCE) {
// return an offset for the blended GPS solution // return an offset for the blended GPS solution
return _blended_antenna_offset; return _blended_antenna_offset;
@ -1786,7 +1786,7 @@ bool AP_GPS::is_healthy(uint8_t instance) const
} }
#endif // HAL_BUILD_AP_PERIPH #endif // HAL_BUILD_AP_PERIPH
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
if (instance == GPS_BLENDED_INSTANCE) { if (instance == GPS_BLENDED_INSTANCE) {
return blend_health_check(); return blend_health_check();
} }

View File

@ -30,28 +30,6 @@
#include <SITL/SIM_GPS.h> #include <SITL/SIM_GPS.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
/**
maximum number of GPS instances available on this platform. If more
than 1 then redundant sensors may be available
*/
#ifndef GPS_MAX_RECEIVERS
#define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data
#endif
#if !defined(GPS_MAX_INSTANCES)
#if GPS_MAX_RECEIVERS > 1
#define GPS_MAX_INSTANCES (GPS_MAX_RECEIVERS + 1) // maximum number of GPS instances including the 'virtual' GPS created by blending receiver data
#else
#define GPS_MAX_INSTANCES 1
#endif // GPS_MAX_RECEIVERS > 1
#endif // GPS_MAX_INSTANCES
#if GPS_MAX_RECEIVERS <= 1 && GPS_MAX_INSTANCES > 1
#error "GPS_MAX_INSTANCES should be 1 for GPS_MAX_RECEIVERS <= 1"
#endif
#if GPS_MAX_INSTANCES > GPS_MAX_RECEIVERS
#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS // the virtual blended GPS is always the highest instance (2)
#endif
#define GPS_UNKNOWN_DOP UINT16_MAX // set unknown DOP's to maximum value, which is also correct for MAVLink #define GPS_UNKNOWN_DOP UINT16_MAX // set unknown DOP's to maximum value, which is also correct for MAVLink
// the number of GPS leap seconds - copied into SIM_GPS.cpp // the number of GPS leap seconds - copied into SIM_GPS.cpp
@ -761,7 +739,7 @@ private:
void inject_data(const uint8_t *data, uint16_t len); void inject_data(const uint8_t *data, uint16_t len);
void inject_data(uint8_t instance, const uint8_t *data, uint16_t len); void inject_data(uint8_t instance, const uint8_t *data, uint16_t len);
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
// GPS blending and switching // GPS blending and switching
Vector3f _blended_antenna_offset; // blended antenna offset Vector3f _blended_antenna_offset; // blended antenna offset
float _blended_lag_sec; // blended receiver lag in seconds float _blended_lag_sec; // blended receiver lag in seconds

View File

@ -1,6 +1,6 @@
#include "AP_GPS.h" #include "AP_GPS.h"
#if defined(GPS_BLENDED_INSTANCE) #if AP_GPS_BLENDED_ENABLED
// defines used to specify the mask position for use of different accuracy metrics in the blending algorithm // 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_HPOS_ACC 1
@ -377,4 +377,4 @@ void AP_GPS::calc_blended_state(void)
} }
#endif #endif
} }
#endif // GPS_BLENDED_INSTANCE #endif // AP_GPS_BLENDED_ENABLED

View File

@ -7,10 +7,39 @@
#define AP_GPS_ENABLED 1 #define AP_GPS_ENABLED 1
#endif #endif
#if AP_GPS_ENABLED
/**
maximum number of GPS instances available on this platform. If more
than 1 then redundant sensors may be available
*/
#ifndef GPS_MAX_RECEIVERS
#define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data
#endif
#if !defined(GPS_MAX_INSTANCES)
#if GPS_MAX_RECEIVERS > 1
#define GPS_MAX_INSTANCES (GPS_MAX_RECEIVERS + 1) // maximum number of GPS instances including the 'virtual' GPS created by blending receiver data
#else
#define GPS_MAX_INSTANCES 1
#endif // GPS_MAX_RECEIVERS > 1
#endif // GPS_MAX_INSTANCES
#if GPS_MAX_RECEIVERS <= 1 && GPS_MAX_INSTANCES > 1
#error "GPS_MAX_INSTANCES should be 1 for GPS_MAX_RECEIVERS <= 1"
#endif
#if GPS_MAX_INSTANCES > GPS_MAX_RECEIVERS
#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS // the virtual blended GPS is always the highest instance (2)
#endif
#endif
#ifndef AP_GPS_BACKEND_DEFAULT_ENABLED #ifndef AP_GPS_BACKEND_DEFAULT_ENABLED
#define AP_GPS_BACKEND_DEFAULT_ENABLED AP_GPS_ENABLED #define AP_GPS_BACKEND_DEFAULT_ENABLED AP_GPS_ENABLED
#endif #endif
#ifndef AP_GPS_BLENDED_ENABLED
#define AP_GPS_BLENDED_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED && defined(GPS_BLENDED_INSTANCE)
#endif
#ifndef AP_GPS_DRONECAN_ENABLED #ifndef AP_GPS_DRONECAN_ENABLED
#define AP_GPS_DRONECAN_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS #define AP_GPS_DRONECAN_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS
#endif #endif