Skip to content

Commit

Permalink
AP_GPS: support GPSx_GNSS_MODE for F9P
Browse files Browse the repository at this point in the history
  • Loading branch information
@andyp1per
andyp1per committed Apr 30, 2024
1 parent bcf6332 commit 5b50d69
Show file tree
Hide file tree
Showing 2 changed files with 87 additions and 5 deletions.
82 changes: 80 additions & 2 deletions libraries/AP_GPS/AP_GPS_UBLOX.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -273,6 +273,8 @@ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_dis[] {
{ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 0},
};

AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_GNSS[UBLOX_MAX_GNSS_CONFIG_BLOCKS*3] {};

void
AP_GPS_UBLOX::_request_next_config(void)
{
Expand Down Expand Up @@ -455,6 +457,73 @@ AP_GPS_UBLOX::_request_next_config(void)
#endif
break;

case STEP_F9: {
if (_hardware_generation == UBLOX_F9 && params.gnss_mode != 0) {

uint8_t cfg_count = 0;

// ZED-F9P defaults are
// GPS L1C/A+L2C
// SBAS L1C/A
// GALILEO E1+E5b
// BEIDOU B1+B2
// QZSS L1C/A+L2C
// GLONASS L1+L2
// IMES not supported
// GPS and QZSS should be enabled/disabled together, but we will leave them alone

uint8_t gnss_mode = params.gnss_mode;
gnss_mode |= 1U<<GNSS_GPS;
gnss_mode |= 1U<<GNSS_QZSS;
gnss_mode &= ~(1U<<GNSS_IMES);
params.gnss_mode.set_and_save(gnss_mode);

for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
bool ena = gnss_mode & (1U<<i);
switch (i) {
case GNSS_SBAS:
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_SBAS_ENA, ena };
// If a constellation is disabled, the signals get disabled as well
if (ena) {
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_SBAS_L1CA_ENA, ena };
}
break;
case GNSS_GALILEO:
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_ENA, ena };
if (ena) {
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E1_ENA, ena };
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E5B_ENA, ena };
}
break;
case GNSS_BEIDOU:
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_ENA, ena };
if (ena) {
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B1_ENA, ena };
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B2_ENA, ena };
}
break;
case GNSS_GLONASS:
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_ENA, ena };
if (ena) {
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_L1_ENA, ena };
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_L2_ENA, ena };
}
break;
case GNSS_IMES:
case GNSS_GPS:
case GNSS_QZSS:
// not supported or leave alone
break;
}
}
// special handling of F9 config
CFG_Debug("Sending F9 settings");
if (!_configure_config_set(config_GNSS, cfg_count, CONFIG_F9, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
_next_message--;
}
}
break;
}

case STEP_M10: {
if (_hardware_generation == UBLOX_M10) {
Expand Down Expand Up @@ -1046,6 +1115,7 @@ AP_GPS_UBLOX::_parse_gps(void)
case CLASS_CFG:
switch(_buffer.nack.msgID) {
case MSG_CFG_VALGET:
CFG_Debug("NACK 0x%x", (unsigned)_buffer.nack.msgID);
if (active_config.list != nullptr) {
/*
likely this device does not support fetching multiple keys at once, go one at a time
Expand Down Expand Up @@ -1290,9 +1360,12 @@ AP_GPS_UBLOX::_parse_gps(void)
// see if it is in active config list
int8_t cfg_idx = find_active_config_index(id);
if (cfg_idx >= 0) {
CFG_Debug("valset(0x%lx): %u", uint32_t(id), (*cfg_data) & 0x1);
const uint8_t key_size = config_key_size(id);
if (cfg_len < key_size ||
memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) {
if (cfg_len < key_size
// for keys of length 1 only the LSB is significant
|| (key_size == 1 && (active_config.list[cfg_idx].value & 0x1) != (*cfg_data & 0x1))
|| memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) {
_configure_valset(id, &active_config.list[cfg_idx].value, active_config.layers);
_unconfigured_messages |= active_config.unconfig_bit;
active_config.done_mask &= ~(1U << cfg_idx);
Expand All @@ -1318,6 +1391,8 @@ AP_GPS_UBLOX::_parse_gps(void)
unsigned(active_config.list[active_config.fetch_index].key));
}
}
} else {
CFG_Debug("valget no active config for 0x%lx", (uint32_t)id);
}

// step over the value
Expand Down Expand Up @@ -1364,6 +1439,7 @@ AP_GPS_UBLOX::_parse_gps(void)
_unconfigured_messages |= CONFIG_TMODE_MODE;
}
_hardware_generation = UBLOX_F9;
_unconfigured_messages |= CONFIG_F9;
}
if (strncmp(_version.swVersion, "EXT CORE 4", 10) == 0) {
// a M9
Expand Down Expand Up @@ -1866,6 +1942,7 @@ AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value, uint8_t layers
if (!supports_F9_config()) {
return false;
}

const uint8_t len = config_key_size(key);
struct ubx_cfg_valset msg {};
uint8_t buf[sizeof(msg)+len];
Expand Down Expand Up @@ -2057,6 +2134,7 @@ static const char *reasons[] = {"navigation rate",
"Time mode settings",
"RTK MB",
"TIM TM2",
"F9",
"M10",
"L5 Enable Disable"};

Expand Down
10 changes: 7 additions & 3 deletions libraries/AP_GPS/AP_GPS_UBLOX.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,9 +99,10 @@
#define CONFIG_TMODE_MODE (1<<16)
#define CONFIG_RTK_MOVBASE (1<<17)
#define CONFIG_TIM_TM2 (1<<18)
#define CONFIG_M10 (1<<19)
#define CONFIG_L5 (1<<20)
#define CONFIG_LAST (1<<21) // this must always be the last bit
#define CONFIG_F9 (1<<19)
#define CONFIG_M10 (1<<20)
#define CONFIG_L5 (1<<21)
#define CONFIG_LAST (1<<22) // this must always be the last bit

#define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)

Expand Down Expand Up @@ -742,6 +743,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
STEP_VERSION,
STEP_RTK_MOVBASE, // setup moving baseline
STEP_TIM_TM2,
STEP_F9,
STEP_M10,
STEP_L5,
STEP_LAST
Expand Down Expand Up @@ -882,6 +884,8 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
static const config_list config_M10[];
static const config_list config_L5_ovrd_ena[];
static const config_list config_L5_ovrd_dis[];
// scratch space for GNSS config
static config_list config_GNSS[UBLOX_MAX_GNSS_CONFIG_BLOCKS*3];
};

#endif

0 comments on commit 5b50d69

Please sign in to comment.