Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Support GPS_SBAS_MODE on SBF GPS units 

Also swaps to using an AP_Enum for the SBAS type, and fixes up the fact
that the prearm/failure reasons should be using the config step, rather
then the init blob index
This commit is contained in:
Michael du Breuil 2021-04-05 14:30:30 -07:00 committed by Andrew Tridgell
parent 06ef0a0993
commit 1288f4c81b
4 changed files with 53 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_GPS/AP_GPS.h
View File

@ -533,6 +533,12 @@ public:
bool get_error_codes(uint8_t instance, uint32_t &error_codes) const;
bool get_error_codes(uint32_t &error_codes) const { return get_error_codes(primary_instance, error_codes); }
enum class SBAS_Mode : int8_t {
Disabled = 0,
Enabled = 1,
DoNotChange = 2,
};
protected:
// configuration parameters
@ -543,7 +549,7 @@ protected:
AP_Int16 _sbp_logmask;
AP_Int8 _inject_to;
uint32_t _last_instance_swap_ms;
AP_Int8 _sbas_mode;
AP_Enum<SBAS_Mode> _sbas_mode;
AP_Int8 _min_elevation;
AP_Int8 _raw_data;
AP_Int8 _gnss_mode[GPS_MAX_RECEIVERS];

43
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -54,6 +54,7 @@ do { \
constexpr const char *AP_GPS_SBF::portIdentifiers[];
constexpr const char* AP_GPS_SBF::_initialisation_blob[];
constexpr const char* AP_GPS_SBF::sbas_on_blob[];
AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port) :
@ -117,10 +118,28 @@ AP_GPS_SBF::read(void)
}
break;
case Config_State::Blob:
if (asprintf(&config_string,"%s\n", (char *)_initialisation_blob[_init_blob_index]) == -1) {
if (asprintf(&config_string, "%s\n", _initialisation_blob[_init_blob_index]) == -1) {
config_string = nullptr;
}
break;
case Config_State::SBAS:
switch ((AP_GPS::SBAS_Mode)gps._sbas_mode) {
case AP_GPS::SBAS_Mode::Disabled:
if (asprintf(&config_string, "%s\n", sbas_off) == -1) {
config_string = nullptr;
}
break;
case AP_GPS::SBAS_Mode::Enabled:
if (asprintf(&config_string, "%s\n", sbas_on_blob[_init_blob_index]) == -1) {
config_string = nullptr;
}
break;
case AP_GPS::SBAS_Mode::DoNotChange:
config_string = nullptr;
config_step = Config_State::Complete;
break;
}
break;
case Config_State::Complete:
// should never reach here, why search for a config if we have fully configured already
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
@ -313,6 +332,14 @@ AP_GPS_SBF::parse(uint8_t temp)
case Config_State::Blob:
_init_blob_index++;
if (_init_blob_index >= ARRAY_SIZE(_initialisation_blob)) {
config_step = Config_State::SBAS;
_init_blob_index = 0;
}
break;
case Config_State::SBAS:
_init_blob_index++;
if ((gps._sbas_mode == AP_GPS::SBAS_Mode::Disabled)
||_init_blob_index >= ARRAY_SIZE(sbas_on_blob)) {
config_step = Config_State::Complete;
}
break;
@ -529,15 +556,19 @@ AP_GPS_SBF::process_message(void)
void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
{
if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE &&
_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF is not fully configured (%u/%u)", state.instance + 1,
_init_blob_index, (unsigned)ARRAY_SIZE(_initialisation_blob));
config_step != Config_State::Complete) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF is not fully configured (%u/%u/%u/%u)",
state.instance + 1,
(unsigned)config_step,
_init_blob_index,
(unsigned)ARRAY_SIZE(_initialisation_blob),
(unsigned)ARRAY_SIZE(sbas_on_blob));
}
}
bool AP_GPS_SBF::is_configured (void) const {
return (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE ||
_init_blob_index >= ARRAY_SIZE(_initialisation_blob));
return ((gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) ||
(config_step == Config_State::Complete));
}
bool AP_GPS_SBF::is_healthy (void) const {

7
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -70,6 +70,7 @@ private:
Baud_Rate,
SSO,
Blob,
SBAS,
Complete
};
Config_State config_step;
@ -83,6 +84,12 @@ private:
GPS_SBF_EXTRA_CONFIG
#endif
};
static constexpr const char* sbas_off = "sst, -SBAS";
static constexpr const char* sbas_on_blob[] = {
"snt,+GEOL1+GEOL5",
"sst,+SBAS",
"ssbc,auto,Operational,MixedSystems,auto",
};
uint32_t _config_last_ack_time;
const char* _port_enable = "\nSSSSSSSSSS\n";

4
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -255,7 +255,7 @@ AP_GPS_UBLOX::_request_next_config(void)
}
break;
case STEP_POLL_SBAS:
if (gps._sbas_mode != 2) {
if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
_send_message(CLASS_CFG, MSG_CFG_SBAS, nullptr, 0);
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
@ -1016,7 +1016,7 @@ AP_GPS_UBLOX::_parse_gps(void)
#endif
case MSG_CFG_SBAS:
if (gps._sbas_mode != 2) {
if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n",
(unsigned)_buffer.sbas.mode,
(unsigned)_buffer.sbas.usage,