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

AP_GPS: fixed the check for disabling CONFIG_RATE_SOL config

Browse Source 
we should stop looking for SOL as soon as we know we have PVT, instead
of waiting for all others to be cleared

may help with
https://discuss.ardupilot.org/t/fc-tries-to-config-gps-constantly-when-trying-to-arm-in-loiter-mode/129930
This commit is contained in:
Andrew Tridgell 2025-02-18 11:03:02 +11:00
parent 6ad05dc2c3
commit 69c540a0de
2 changed files with 24 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -278,7 +278,7 @@ AP_GPS_UBLOX::_request_next_config(void)
return; return;
} }
if (_unconfigured_messages == CONFIG_RATE_SOL && havePvtMsg) { if ((_unconfigured_messages & CONFIG_RATE_SOL) != 0 && havePvtMsg) {
/* /*
we don't need SOL if we have PVT and TIMEGPS. This is needed we don't need SOL if we have PVT and TIMEGPS. This is needed
as F9P doesn't support the SOL message as F9P doesn't support the SOL message
@ -453,7 +453,8 @@ AP_GPS_UBLOX::_request_next_config(void)
break; break;
case STEP_F9: { case STEP_F9: {
if (_hardware_generation == UBLOX_F9) { if (_hardware_generation == UBLOX_F9 ||
_hardware_generation == UBLOX_M10) {
uint8_t cfg_count = populate_F9_gnss(); uint8_t cfg_count = populate_F9_gnss();
// special handling of F9 config // special handling of F9 config
if (cfg_count > 0) { if (cfg_count > 0) {
@ -470,7 +471,8 @@ AP_GPS_UBLOX::_request_next_config(void)
} }
case STEP_F9_VALIDATE: { case STEP_F9_VALIDATE: {
if (_hardware_generation == UBLOX_F9) { if (_hardware_generation == UBLOX_F9 ||
_hardware_generation == UBLOX_M10) {
// GNSS takes 0.5s to reset // GNSS takes 0.5s to reset
if (AP_HAL::millis() - _f9_config_time < 500) { if (AP_HAL::millis() - _f9_config_time < 500) {
_next_message--; _next_message--;
@ -662,6 +664,17 @@ AP_GPS_UBLOX::read(void)
} }
} }
#if 0
// this can be modified to force a particular config state for
// state machine config debugging
static bool done_force_config_error;
if (!_unconfigured_messages && !done_force_config_error) {
done_force_config_error = true;
_unconfigured_messages = CONFIG_F9 | CONFIG_RATE_SOL;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Forcing config state 0x%04x", unsigned(_unconfigured_messages));
}
#endif
if(!_unconfigured_messages && gps._save_config && !_cfg_saved && if(!_unconfigured_messages && gps._save_config && !_cfg_saved &&
_num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 && _num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 &&
!hal.util->get_soft_armed()) { !hal.util->get_soft_armed()) {
@ -1088,6 +1101,7 @@ AP_GPS_UBLOX::_parse_gps(void)
if (active_config.fetch_index == -1) { if (active_config.fetch_index == -1) {
CFG_Debug("NACK starting %u", unsigned(active_config.count)); CFG_Debug("NACK starting %u", unsigned(active_config.count));
active_config.fetch_index = 0; active_config.fetch_index = 0;
use_single_valget = true;
} else { } else {
// the device does not support the config key we asked for, // the device does not support the config key we asked for,
// consider the bit as done // consider the bit as done
@ -1096,7 +1110,8 @@ AP_GPS_UBLOX::_parse_gps(void)
int(active_config.fetch_index), int(active_config.fetch_index),
unsigned(active_config.list[active_config.fetch_index].key), unsigned(active_config.list[active_config.fetch_index].key),
unsigned(active_config.done_mask)); unsigned(active_config.done_mask));
if (active_config.done_mask == (1U<<active_config.count)-1) { if (active_config.done_mask == (1U<<active_config.count)-1 ||
active_config.fetch_index >= active_config.count) {
// all done! // all done!
_unconfigured_messages &= ~active_config.unconfig_bit; _unconfigured_messages &= ~active_config.unconfig_bit;
} }
@ -1441,6 +1456,9 @@ AP_GPS_UBLOX::_parse_gps(void)
// M10 does not support CONFIG_GNSS // M10 does not support CONFIG_GNSS
_unconfigured_messages &= ~CONFIG_GNSS; _unconfigured_messages &= ~CONFIG_GNSS;
check_L1L5 = true; check_L1L5 = true;
// M10 does not support multi-valued VALGET
use_single_valget = true;
} }
if (check_L1L5) { if (check_L1L5) {
// check if L1L5 in extension // check if L1L5 in extension
@ -1989,7 +2007,7 @@ AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint
// -1) then if we get a NACK for VALGET we switch to fetching one // -1) then if we get a NACK for VALGET we switch to fetching one
// value at a time. This copes with the M10S which can only fetch // value at a time. This copes with the M10S which can only fetch
// one value at a time // one value at a time
active_config.fetch_index = -1; active_config.fetch_index = use_single_valget? 0 :-1;
uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)]; uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)];
struct ubx_cfg_valget msg {}; struct ubx_cfg_valget msg {};

1
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -887,6 +887,7 @@ private:
int8_t fetch_index; int8_t fetch_index;
int8_t set_index; int8_t set_index;
} active_config; } active_config;
bool use_single_valget;
#if GPS_MOVING_BASELINE #if GPS_MOVING_BASELINE
// config for moving baseline base // config for moving baseline base