aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: improved delay health threshold

Browse Source 
allow for missing frames without showing unhealthy on GCS
This commit is contained in:
Andrew Tridgell 2020-04-05 08:53:27 +10:00
parent 3397ed766d
commit 36098c3221
2 changed files with 35 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_GPS/AP_GPS.cpp
View File

@ -767,6 +767,25 @@ void AP_GPS::update_instance(uint8_t instance)
} }
#endif #endif
if (data_should_be_logged) {
// keep count of delayed frames and average frame delay for health reporting
const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer
GPS_timing &t = timing[instance];
if (t.delta_time_ms > gps_max_delta_ms) {
t.delayed_count++;
} else {
t.delayed_count = 0;
}
if (t.delta_time_ms < 2000) {
if (t.average_delta_ms <= 0) {
t.average_delta_ms = t.delta_time_ms;
} else {
t.average_delta_ms = 0.98 * t.average_delta_ms + 0.02 * t.delta_time_ms;
}
}
}
#ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_BUILD_AP_PERIPH
if (data_should_be_logged && if (data_should_be_logged &&
(should_log() || AP::ahrs().have_ekf_logging())) { (should_log() || AP::ahrs().have_ekf_logging())) {
@ -1686,18 +1705,22 @@ bool AP_GPS::is_healthy(uint8_t instance) const
return false; return false;
} }
const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer /*
allow two lost frames before declaring the GPS unhealthy, but
bool last_msg_valid = last_message_delta_time_ms(instance) < gps_max_delta_ms; require the average frame rate to be close to 5Hz
*/
const uint8_t delay_threshold = 2;
const float delay_avg_max = 245;
const GPS_timing &t = timing[instance];
bool delay_ok = (t.delayed_count < delay_threshold) && t.average_delta_ms < delay_avg_max;
#if defined(GPS_BLENDED_INSTANCE) #if defined(GPS_BLENDED_INSTANCE)
if (instance == GPS_BLENDED_INSTANCE) { if (instance == GPS_BLENDED_INSTANCE) {
return last_msg_valid && blend_health_check(); return delay_ok && blend_health_check();
} }
#endif #endif
return last_msg_valid && return delay_ok && drivers[instance] != nullptr &&
drivers[instance] != nullptr &&
drivers[instance]->is_healthy(); drivers[instance]->is_healthy();
} }

6
libraries/AP_GPS/AP_GPS.h
View File

@ -520,6 +520,12 @@ private:
// delta time between the last pair of GPS updates in system milliseconds // delta time between the last pair of GPS updates in system milliseconds
uint16_t delta_time_ms; uint16_t delta_time_ms;
// count of delayed frames
uint8_t delayed_count;
// the average time delta
float average_delta_ms;
}; };
// Note allowance for an additional instance to contain blended data // Note allowance for an additional instance to contain blended data
GPS_timing timing[GPS_MAX_INSTANCES]; GPS_timing timing[GPS_MAX_INSTANCES];