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

AP_GPS: Add validity boolean to GPS lag reporting

Browse Source
This commit is contained in:
priseborough 2017-05-22 11:07:26 +10:00 committed by Francisco Ferreira
parent bd0229b7bc
commit 5dcfc94371
2 changed files with 24 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_GPS/AP_GPS.cpp
View File

@ -1035,24 +1035,32 @@ void AP_GPS::Write_DataFlash_Log_Startup_messages()
/* /*
return the expected lag (in seconds) in the position and velocity readings from the gps return the expected lag (in seconds) in the position and velocity readings from the gps
return true if the GPS hardware configuration is known or the delay parameter has been set
*/ */
float AP_GPS::get_lag(uint8_t instance) const bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
{ {
// return lag of blended GPS // return lag of blended GPS
if (instance == GPS_MAX_RECEIVERS) { if (instance == GPS_BLENDED_INSTANCE) {
return _blended_lag_sec; lag_sec = _blended_lag_sec;
// auto switching uses all GPS receivers, so all must be configured
return all_configured();
} }
if (_delay_ms[instance] > 0) { if (_delay_ms[instance] > 0) {
// if the user has specified a non zero time delay, always return that value // if the user has specified a non zero time delay, always return that value
return 0.001f * (float)_delay_ms[instance]; lag_sec = 0.001f * (float)_delay_ms[instance];
// the user is always right !!
return true;
} else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) { } else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
// no GPS was detected in this instance // no GPS was detected in this instance so return a default delay of 1 measurement interval
// so return a default delay of 1 measurement interval lag_sec = 0.001f * (float)get_rate_ms(instance);
return 0.001f * (float)get_rate_ms(instance); // check lack of GPS is consistent with user expectation
return state[instance].status == NO_GPS;
} else { } else {
// the user has not specified a delay so we determine it from the GPS type // the user has not specified a delay so we determine it from the GPS type
return drivers[instance]->get_lag(); lag_sec = drivers[instance]->get_lag();
// check for a valid GPS configuration
return drivers[instance]->is_configured();
} }
} }
@ -1454,7 +1462,9 @@ void AP_GPS::calc_blended_state(void)
if (_blend_weights[i] > 0.0f) { if (_blend_weights[i] > 0.0f) {
temp_time_1 += (double)timing[i].last_fix_time_ms * (double) _blend_weights[i]; 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_2 += (double)timing[i].last_message_time_ms * (double)_blend_weights[i];
_blended_lag_sec += get_lag(i) * _blend_weights[i]; float gps_lag_sec = 0;
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_fix_time_ms = (uint32_t)temp_time_1;

7
libraries/AP_GPS/AP_GPS.h
View File

@ -306,8 +306,11 @@ public:
} }
// the expected lag (in seconds) in the position and velocity readings from the gps // the expected lag (in seconds) in the position and velocity readings from the gps
float get_lag(uint8_t instance) const; // return true if the GPS hardware configuration is known or the lag parameter has been set manually
float get_lag(void) const { return get_lag(primary_instance); } bool get_lag(uint8_t instance, float &lag_sec) const;
bool get_lag(float &lag_sec) const {
return get_lag(primary_instance, lag_sec);
}
// return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin // return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin
const Vector3f &get_antenna_offset(uint8_t instance) const; const Vector3f &get_antenna_offset(uint8_t instance) const;