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

AP_GPS: support GPSx_GNSS_MODE for F9P 

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
This commit is contained in:
Andy Piper 2024-04-29 15:46:53 +01:00 committed by Andrew Tridgell
parent c77f688032
commit a9dc7b440f
2 changed files with 232 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

199
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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)
{
@ -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--;
}
@ -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
@ -1029,6 +1072,7 @@ AP_GPS_UBLOX::_parse_gps(void)
_unconfigured_messages &= ~CONFIG_TP5;
break;
}
break;
case CLASS_MON:
switch(_buffer.ack.msgID) {
@ -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
@ -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;
}
}
}
@ -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",
@ -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);
@ -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
@ -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);
@ -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
@ -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];
@ -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
@ -2057,6 +2173,7 @@ static const char *reasons[] = {"navigation rate",
"Time mode settings",
"RTK MB",
"TIM TM2",
"F9",
"M10",
"L5 Enable Disable"};
@ -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

49
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -58,6 +58,7 @@
#define UBLOX_RXM_RAW_LOGGING 1
#define UBLOX_MAX_RXM_RAW_SATS 22
#define UBLOX_MAX_RXM_RAWX_SATS 32
#define UBLOX_MAX_EXTENSIONS 8
#define UBLOX_GNSS_SETTINGS 1
#ifndef UBLOX_TIM_TM2_LOGGING
#define UBLOX_TIM_TM2_LOGGING (BOARD_FLASH_SIZE>1024)
@ -68,6 +69,7 @@
#define UBX_TIMEGPS_VALID_WEEK_MASK 0x2
#define UBLOX_MAX_PORTS 6
#define UBLOX_MODULE_LEN 9
#define RATE_POSLLH 1
#define RATE_STATUS 1
@ -99,9 +101,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)
@ -518,7 +521,7 @@ private:
struct PACKED ubx_mon_ver {
char swVersion[30];
char hwVersion[10];
char extension[50]; // extensions are not enabled
char extension[30*UBLOX_MAX_EXTENSIONS]; // extensions are not enabled
};
struct PACKED ubx_nav_svinfo_header {
uint32_t itow;
@ -719,6 +722,12 @@ private:
// flagging state in the driver
};
enum ubx_hardware_variant {
UBLOX_F9_ZED, // comes from MON_VER extension strings
UBLOX_F9_NEO, // comes from MON_VER extension strings
UBLOX_UNKNOWN_HARDWARE_VARIANT = 0xff
};
enum config_step {
STEP_PVT = 0,
STEP_NAV_RATE, // poll NAV rate
@ -742,6 +751,8 @@ private:
STEP_VERSION,
STEP_RTK_MOVBASE, // setup moving baseline
STEP_TIM_TM2,
STEP_F9,
STEP_F9_VALIDATE,
STEP_M10,
STEP_L5,
STEP_LAST
@ -765,13 +776,16 @@ private:
uint32_t _last_cfg_sent_time;
uint8_t _num_cfg_save_tries;
uint32_t _last_config_time;
uint32_t _f9_config_time;
uint16_t _delay_time;
uint8_t _next_message;
uint8_t _ublox_port;
bool _have_version;
struct ubx_mon_ver _version;
char _module[UBLOX_MODULE_LEN];
uint32_t _unconfigured_messages;
uint8_t _hardware_generation;
uint8_t _hardware_variant;
uint32_t _last_pvt_itow;
uint32_t _last_relposned_itow;
uint32_t _last_relposned_ms;
@ -798,8 +812,17 @@ private:
bool havePvtMsg;
// structure for list of config key/value pairs for
// specific configurations
struct PACKED config_list {
ConfigKey key;
// support up to 4 byte values, assumes little-endian
uint32_t value;
};
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
bool _configure_valset(ConfigKey key, const void *value, uint8_t layers=UBX_VALSET_LAYER_ALL);
bool _configure_list_valset(const config_list *list, uint8_t count, uint8_t layers=UBX_VALSET_LAYER_ALL);
bool _configure_valget(ConfigKey key);
void _configure_rate(void);
void _configure_sbas(bool enable);
@ -828,14 +851,6 @@ private:
}
#endif
// structure for list of config key/value pairs for
// specific configurations
struct PACKED config_list {
ConfigKey key;
// support up to 4 byte values, assumes little-endian
uint32_t value;
};
// return size of a config key payload
uint8_t config_key_size(ConfigKey key) const;
@ -849,6 +864,13 @@ private:
// return true if GPS is capable of F9 config
bool supports_F9_config(void) const;
// is the config key a GNSS key
bool is_gnss_key(ConfigKey key) const;
// populate config_GNSS for F9P
uint8_t populate_F9_gnss(void);
uint8_t last_configured_gnss;
uint8_t _pps_freq = 1;
#ifdef HAL_GPIO_PPS
void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us);
@ -863,6 +885,7 @@ private:
uint32_t unconfig_bit;
uint8_t layers;
int8_t fetch_index;
int8_t set_index;
} active_config;
#if GPS_MOVING_BASELINE
@ -882,6 +905,8 @@ private:
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