Skip to content

Commit

Permalink
AP_GPS: support GPSx_GNSS_MODE for F9P
Browse files Browse the repository at this point in the history 
support detecting F9P hardware variant
fix bug in extension buffer management
support NEO-F9P GNSS configuration
allow multiple configuration values to be set in one go
phase F9 configuration to account for GNSS reset
  • Loading branch information
@andyp1per
andyp1per committed May 22, 2024
1 parent 20a23d8 commit f465146
Show file tree
Hide file tree
Showing 2 changed files with 232 additions and 16 deletions.
199 changes: 195 additions & 4 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 @@ -334,6 +336,12 @@ AP_GPS_UBLOX::_request_next_config(void)
}
break;
case STEP_POLL_GNSS:
if (supports_F9_config()) {
if (last_configured_gnss != params.gnss_mode) {
_unconfigured_messages |= CONFIG_F9;
}
break;
}
if (!_send_message(CLASS_CFG, MSG_CFG_GNSS, nullptr, 0)) {
_next_message--;
}
Expand Down Expand Up @@ -455,7 +463,42 @@ AP_GPS_UBLOX::_request_next_config(void)
#endif
break;

case STEP_F9: {
if (_hardware_generation == UBLOX_F9) {
uint8_t cfg_count = populate_F9_gnss();
// special handling of F9 config
if (cfg_count > 0) {
CFG_Debug("Sending F9 settings, GNSS=%u", params.gnss_mode);
if (!_configure_list_valset(config_GNSS, cfg_count, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
_next_message--;
break;
}
_f9_config_time = AP_HAL::millis();
}
}
break;
}

case STEP_F9_VALIDATE: {
if (_hardware_generation == UBLOX_F9) {
// GNSS takes 0.5s to reset
if (AP_HAL::millis() - _f9_config_time < 500) {
_next_message--;
break;
}
_f9_config_time = 0;
uint8_t cfg_count = populate_F9_gnss();
// special handling of F9 config
if (cfg_count > 0) {
CFG_Debug("Validating F9 settings, GNSS=%u", params.gnss_mode);
// now validate all of the settings, this is a no-op if the first call succeeded
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) {
// special handling of M10 config
Expand Down Expand Up @@ -1029,6 +1072,7 @@ AP_GPS_UBLOX::_parse_gps(void)
_unconfigured_messages &= ~CONFIG_TP5;
break;
}

break;
case CLASS_MON:
switch(_buffer.ack.msgID) {
Expand All @@ -1046,6 +1090,7 @@ AP_GPS_UBLOX::_parse_gps(void)
case CLASS_CFG:
switch(_buffer.nack.msgID) {
case MSG_CFG_VALGET:
CFG_Debug("NACK VALGET 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 All @@ -1072,6 +1117,15 @@ AP_GPS_UBLOX::_parse_gps(void)
}
}
break;
case MSG_CFG_VALSET:
CFG_Debug("NACK VALSET 0x%x 0x%x", (unsigned)_buffer.nack.msgID,
unsigned(active_config.list[active_config.set_index].key));
if (is_gnss_key(active_config.list[active_config.set_index].key)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GPS %u: unable to configure band 0x%02x",
unsigned(state.instance + 1), unsigned(active_config.list[active_config.set_index].key));

}
break;
}
}
}
Expand Down Expand Up @@ -1114,7 +1168,7 @@ AP_GPS_UBLOX::_parse_gps(void)

#if UBLOX_GNSS_SETTINGS
case MSG_CFG_GNSS:
if (params.gnss_mode != 0) {
if (params.gnss_mode != 0 && !supports_F9_config()) {
struct ubx_cfg_gnss start_gnss = _buffer.gnss;
uint8_t gnssCount = 0;
Debug("Got GNSS Settings %u %u %u %u:\n",
Expand Down Expand Up @@ -1290,12 +1344,16 @@ 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);
active_config.set_index = cfg_idx;
_cfg_needs_save = true;
} else {
active_config.done_mask |= (1U << cfg_idx);
Expand All @@ -1318,6 +1376,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 @@ -1349,8 +1409,14 @@ AP_GPS_UBLOX::_parse_gps(void)
_have_version = true;
strncpy(_version.hwVersion, _buffer.mon_ver.hwVersion, sizeof(_version.hwVersion));
strncpy(_version.swVersion, _buffer.mon_ver.swVersion, sizeof(_version.swVersion));
void* mod = memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "MOD=", 4);
if (mod != nullptr) {
strncpy(_module, (char*)mod+4, UBLOX_MODULE_LEN-1);
}

GCS_SEND_TEXT(MAV_SEVERITY_INFO,
"u-blox %d HW: %s SW: %s",
"u-blox %s%s%d HW: %s SW: %s",
_module, mod != nullptr ? " " : "",
state.instance + 1,
_version.hwVersion,
_version.swVersion);
Expand All @@ -1364,6 +1430,13 @@ AP_GPS_UBLOX::_parse_gps(void)
_unconfigured_messages |= CONFIG_TMODE_MODE;
}
_hardware_generation = UBLOX_F9;
_unconfigured_messages |= CONFIG_F9;
_unconfigured_messages &= ~CONFIG_GNSS;
if (strncmp(_module, "ZED-F9P", UBLOX_MODULE_LEN) == 0) {
_hardware_variant = UBLOX_F9_ZED;
} else if (strncmp(_module, "NEO-F9P", UBLOX_MODULE_LEN) == 0) {
_hardware_variant = UBLOX_F9_NEO;
}
}
if (strncmp(_version.swVersion, "EXT CORE 4", 10) == 0) {
// a M9
Expand Down Expand Up @@ -1866,6 +1939,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 @@ -1939,6 +2013,48 @@ AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint
return _send_message(CLASS_CFG, MSG_CFG_VALGET, buf, sizeof(buf));
}

/*
* configure F9 based key/value pair for a complete configuration set
*
* this method sends all the key/value pairs in a block, but makes no attempt to check
* the results. Sending in a block is necessary for updates such as GNSS where certain
* combinations are invalid and setting one at a time will not produce the correct result
* if the result needs to be validated then a subsequent _configure_config_set() can be
* issued which will get all the values and reset those that are not properly updated.
*/
bool
AP_GPS_UBLOX::_configure_list_valset(const config_list *list, uint8_t count, uint8_t layers)
{
if (!supports_F9_config()) {
return false;
}

struct ubx_cfg_valset msg {};
uint8_t buf[sizeof(msg)+sizeof(config_list)*count];
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
return false;
}
msg.version = 1;
msg.layers = layers;
msg.transaction = 0;
uint32_t msg_len = sizeof(msg) - sizeof(msg.key);
memcpy(buf, &msg, msg_len);

uint8_t* payload = &buf[msg_len];
for (uint8_t i=0; i<count; i++) {
const uint8_t len = config_key_size(list[i].key);
memcpy(payload, &list[i].key, sizeof(ConfigKey));
payload += sizeof(ConfigKey);
msg_len += sizeof(ConfigKey);
memcpy(payload, &list[i].value, len);
payload += len;
msg_len += len;
}

auto ret = _send_message(CLASS_CFG, MSG_CFG_VALSET, buf, msg_len);
return ret;
}

/*
* save gps configurations to non-volatile memory sent until the call of
* this message
Expand Down Expand Up @@ -2057,6 +2173,7 @@ static const char *reasons[] = {"navigation rate",
"Time mode settings",
"RTK MB",
"TIM TM2",
"F9",
"M10",
"L5 Enable Disable"};

Expand Down Expand Up @@ -2181,10 +2298,84 @@ bool AP_GPS_UBLOX::is_healthy(void) const
return true;
}

// populate config_GNSS with F9 GNSS configuration
uint8_t AP_GPS_UBLOX::populate_F9_gnss(void)
{
uint8_t cfg_count = 0;
if (params.gnss_mode != 0 && (_unconfigured_messages & CONFIG_F9)) {
// ZED-F9P defaults are
// GPS L1C/A+L2C(ZED)
// SBAS L1C/A
// GALILEO E1+E5B(ZED)+E5A(NEO)
// BEIDOU B1+B2(ZED)+B2A(NEO)
// QZSS L1C/A+L2C(ZED)+L5(NEO)
// GLONASS L1+L2(ZED)
// IMES not supported
// GPS and QZSS should be enabled/disabled together, but we will leave them alone
// QZSS and SBAS can only be enabled if GPS is enabled

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 };
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 };
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E1_ENA, ena };
if (_hardware_variant == UBLOX_F9_ZED) {
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E5B_ENA, ena };
} else {
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E5A_ENA, ena };
}
break;
case GNSS_BEIDOU:
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_ENA, ena };
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B1_ENA, ena };
if (_hardware_variant == UBLOX_F9_ZED) {
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B2_ENA, ena };
} else {
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B2A_ENA, ena };
}
break;
case GNSS_GLONASS:
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_ENA, ena };
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_L1_ENA, ena };
if (_hardware_variant == UBLOX_F9_ZED) {
config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_L2_ENA, ena };
}
break;
// not supported or leave alone
case GNSS_IMES:
case GNSS_QZSS:
case GNSS_GPS:
break;
}
}
}

last_configured_gnss = params.gnss_mode;

return cfg_count;
}

// return true if GPS is capable of F9 config
bool AP_GPS_UBLOX::supports_F9_config(void) const
{
return _hardware_generation == UBLOX_F9 || _hardware_generation == UBLOX_M10;
}

// return true if GPS is capable of F9 config
bool AP_GPS_UBLOX::is_gnss_key(ConfigKey key) const
{
return (unsigned(key) & 0xFFFF0000) == 0x10310000;
}

#endif
Loading

0 comments on commit f465146

Please sign in to comment.