Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: add all_consistent and blend health for use as pre-arm check

Browse Source
This commit is contained in:
Randy Mackay 2017-03-09 09:47:40 +09:00
parent 72dfe1127f
commit 729ce34ce3
2 changed files with 37 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_GPS/AP_GPS.cpp
View File

@ -41,6 +41,7 @@
#define BLEND_MASK_USE_HPOS_ACC 1
#define BLEND_MASK_USE_VPOS_ACC 2
#define BLEND_MASK_USE_SPD_ACC 4
#define BLEND_COUNTER_FAILURE_INCREMENT 10
extern const AP_HAL::HAL &hal;
@ -632,6 +633,12 @@ AP_GPS::update(void)
// if blending is requested, attempt to calculate weighting for each GPS
if (_auto_switch == 2) {
_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--;
}
} else {
_output_is_blended = false;
}
@ -915,6 +922,29 @@ AP_GPS::broadcast_first_configuration_failure_reason(void) const {
}
}
// pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned
bool AP_GPS::all_consistent(float &distance) const
{
// return true immediately if only one valid receiver
if (num_instances <= 1 ||
drivers[0] == nullptr || _type[0] == GPS_TYPE_NONE ||
drivers[1] == nullptr || _type[1] == GPS_TYPE_NONE) {
distance = 0;
return true;
}
// calculate distance
distance = location_3d_diff_NED(state[0].location, state[1].location).length();
// success if distance is within 50m
return (distance < 50);
}
// pre-arm check of GPS blending. True means healthy or that blending is not being used
bool AP_GPS::blend_healthy() const
{
return (_blend_health_counter < 50);
}
void
AP_GPS::_broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index)
{

7
libraries/AP_GPS/AP_GPS.h
View File

@ -341,6 +341,12 @@ public:
return first_unconfigured_gps() == GPS_ALL_CONFIGURED;
}
// 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. True means healthy or that blending is not being used
bool blend_healthy() const;
// handle sending of initialisation strings to the GPS - only used by backends
void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
void send_blob_update(uint8_t instance);
@ -469,6 +475,7 @@ private:
uint32_t _last_time_updated[GPS_MAX_RECEIVERS]; // the last value of state.last_gps_time_ms read for that GPS instance - used to detect new data.
float _omega_lpf; // cutoff frequency in rad/sec of LPF applied to position offsets
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);