AP_GPS: Allow backends to report their lag confidences

This commit is contained in:
Michael du Breuil 2017-05-24 10:42:03 -07:00 committed by Francisco Ferreira
parent 7659e9c31f
commit 5f8633e322
6 changed files with 34 additions and 19 deletions

View File

@ -228,6 +228,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Units: ms // @Units: ms
// @Range: 0 250 // @Range: 0 250
// @User: Advanced // @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("DELAY_MS", 18, AP_GPS, _delay_ms[0], 0), AP_GROUPINFO("DELAY_MS", 18, AP_GPS, _delay_ms[0], 0),
// @Param: DELAY_MS2 // @Param: DELAY_MS2
@ -236,6 +237,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Units: ms // @Units: ms
// @Range: 0 250 // @Range: 0 250
// @User: Advanced // @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("DELAY_MS2", 19, AP_GPS, _delay_ms[1], 0), AP_GROUPINFO("DELAY_MS2", 19, AP_GPS, _delay_ms[1], 0),
// @Param: BLEND_MASK // @Param: BLEND_MASK
@ -1052,15 +1054,17 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
// the user is always right !! // the user is always right !!
return true; 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 so return a default delay of 1 measurement interval // no GPS was detected in this instance so return the worst possible lag term
lag_sec = 0.001f * (float)get_rate_ms(instance); if (_type[instance] == GPS_TYPE_NONE) {
// check lack of GPS is consistent with user expectation lag_sec = 0.0f;
return state[instance].status == NO_GPS; return true;
} else {
lag_sec = GPS_WORST_LAG_SEC;
}
return _type[instance] == GPS_TYPE_AUTO;
} 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
lag_sec = drivers[instance]->get_lag(); return drivers[instance]->get_lag(lag_sec);
// check for a valid GPS configuration
return drivers[instance]->is_configured();
} }
} }

View File

@ -33,6 +33,7 @@
#define GPS_RTK_INJECT_TO_ALL 127 #define GPS_RTK_INJECT_TO_ALL 127
#define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms #define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms
#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
#define GPS_WORST_LAG_SEC 0.22f // worst lag value any GPS driver is expected to return, expressed in seconds
// the number of GPS leap seconds // the number of GPS leap seconds
#define GPS_LEAPSECONDS_MILLIS 18000ULL #define GPS_LEAPSECONDS_MILLIS 18000ULL

View File

@ -43,8 +43,8 @@ public:
void broadcast_configuration_failure_reason(void) const override; void broadcast_configuration_failure_reason(void) const override;
// return velocity lag // get the velocity lag, returns true if the driver is confident in the returned value
float get_lag(void) const override { return 0.08f; } ; bool get_lag(float &lag_sec) const override { lag_sec = 0.08f; return true; } ;
private: private:

View File

@ -59,7 +59,7 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
_ublox_port(255), _ublox_port(255),
_have_version(false), _have_version(false),
_unconfigured_messages(CONFIG_ALL), _unconfigured_messages(CONFIG_ALL),
_hardware_generation(0), _hardware_generation(UBLOX_UNKNOWN_HARDWARE_GENERATION),
_new_position(0), _new_position(0),
_new_speed(0), _new_speed(0),
_disable_counter(0), _disable_counter(0),
@ -104,7 +104,7 @@ AP_GPS_UBLOX::_request_next_config(void)
break; break;
case STEP_POLL_SVINFO: case STEP_POLL_SVINFO:
// not required once we know what generation we are on // not required once we know what generation we are on
if(_hardware_generation == 0) { if(_hardware_generation == UBLOX_UNKNOWN_HARDWARE_GENERATION) {
if (!_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0)) { if (!_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0)) {
_next_message--; _next_message--;
} }
@ -1307,19 +1307,27 @@ AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const {
/* /*
return velocity lag in seconds return velocity lag in seconds
*/ */
float AP_GPS_UBLOX::get_lag(void) const bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
{ {
switch (_hardware_generation) { switch (_hardware_generation) {
case UBLOX_UNKNOWN_HARDWARE_GENERATION:
lag_sec = 0.22f;
// always bail out in this case, it's used to indicate we have yet to receive a valid
// hardware generation, however the user may have inhibited us detecting the generation
// so if we aren't allowed to do configuration, we will accept this as the default delay
return gps._auto_config != 1;
case UBLOX_5: case UBLOX_5:
case UBLOX_6: case UBLOX_6:
default: default:
return 0.22f; lag_sec = 0.22f;
break;
case UBLOX_7: case UBLOX_7:
case UBLOX_M8: case UBLOX_M8:
// based on flight logs the 7 and 8 series seem to produce about 120ms lag // based on flight logs the 7 and 8 series seem to produce about 120ms lag
return 0.12f; lag_sec = 0.12f;
break; break;
}; };
return true;
} }
void AP_GPS_UBLOX::Write_DataFlash_Log_Startup_messages() const void AP_GPS_UBLOX::Write_DataFlash_Log_Startup_messages() const

View File

@ -116,8 +116,8 @@ public:
void broadcast_configuration_failure_reason(void) const override; void broadcast_configuration_failure_reason(void) const override;
void Write_DataFlash_Log_Startup_messages() const override; void Write_DataFlash_Log_Startup_messages() const override;
// return velocity lag // get the velocity lag, returns true if the driver is confident in the returned value
float get_lag(void) const override; bool get_lag(float &lag_sec) const override;
const char *name() const override { return "u-blox"; } const char *name() const override { return "u-blox"; }
@ -477,7 +477,9 @@ private:
UBLOX_5, UBLOX_5,
UBLOX_6, UBLOX_6,
UBLOX_7, UBLOX_7,
UBLOX_M8 UBLOX_M8,
UBLOX_UNKNOWN_HARDWARE_GENERATION = 0xff // not in the ublox spec used for
// flagging state in the driver
}; };
enum config_step { enum config_step {

View File

@ -53,8 +53,8 @@ public:
virtual void handle_msg(const mavlink_message_t *msg) { return ; } virtual void handle_msg(const mavlink_message_t *msg) { return ; }
virtual void handle_gnss_msg(const AP_GPS::GPS_State &msg) { return ; } virtual void handle_gnss_msg(const AP_GPS::GPS_State &msg) { return ; }
// driver specific lag // driver specific lag, returns true if the driver is confident in the provided lag
virtual float get_lag(void) const { return 0.2f; } virtual bool get_lag(float &lag) const { lag = 0.2f; return true; }
virtual const char *name() const = 0; virtual const char *name() const = 0;