mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_GPS: Allow backends to report their lag confidences
This commit is contained in:
parent
7659e9c31f
commit
5f8633e322
@ -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();
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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
|
||||||
|
@ -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:
|
||||||
|
|
||||||
|
@ -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
|
||||||
|
@ -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 {
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user