Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_GPS: support GPSx_GNSS_MODE for F9P #26922

Merged
merged 3 commits into from
Jun 9, 2024
Merged

AP_GPS: support GPSx_GNSS_MODE for F9P #26922

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5a7597e
AP_GPS: support GPSx_GNSS_MODE for F9P
andyp1per Apr 29, 2024
9051f45
AP_HAL_ChibiOS: enable LEDs on Here4AP
andyp1per May 22, 2024
86fbaad
AP_GPS: dynamically allocate config_GNSS
andyp1per May 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
206 changes: 202 additions & 4 deletions libraries/AP_GPS/AP_GPS_UBLOX.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -334,6 +334,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 +461,43 @@ 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 +1071,7 @@ AP_GPS_UBLOX::_parse_gps(void)
_unconfigured_messages &= ~CONFIG_TP5;
break;
}

break;
case CLASS_MON:
switch(_buffer.ack.msgID) {
Expand All @@ -1046,6 +1089,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 +1116,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 +1167,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 +1343,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 +1375,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 +1408,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 +1429,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 +1938,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 +2012,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 +2172,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 +2297,92 @@ 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

if (config_GNSS == nullptr) {
config_GNSS = (config_list*)calloc(UBLOX_MAX_GNSS_CONFIG_BLOCKS*3, sizeof(config_list));
}

if (config_GNSS == nullptr) {
return 0;
}

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
Loading