diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 179c5d34d482e..7904e2ba03080 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -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--; } @@ -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 @@ -1029,6 +1071,7 @@ AP_GPS_UBLOX::_parse_gps(void) _unconfigured_messages &= ~CONFIG_TP5; break; } + break; case CLASS_MON: switch(_buffer.ack.msgID) { @@ -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 @@ -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; } } } @@ -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", @@ -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); @@ -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 @@ -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); @@ -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 @@ -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]; @@ -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; i1024) @@ -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 @@ class AP_GPS_UBLOX : public AP_GPS_Backend 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 @@ class AP_GPS_UBLOX : public AP_GPS_Backend // 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 @@ class AP_GPS_UBLOX : public AP_GPS_Backend 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 @@ class AP_GPS_UBLOX : public AP_GPS_Backend 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 @@ class AP_GPS_UBLOX : public AP_GPS_Backend 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 @@ class AP_GPS_UBLOX : public AP_GPS_Backend } #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 @@ class AP_GPS_UBLOX : public AP_GPS_Backend // 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 @@ class AP_GPS_UBLOX : public AP_GPS_Backend 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 @@ 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 + config_list* config_GNSS; }; #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat index bfc89616e307d..3a2959b13352c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat @@ -110,7 +110,6 @@ BARO MS56XX SPI:ms5611 PB8 TIM4_CH3 TIM4 PWM(1) PB9 TIM4_CH4 TIM4 PWM(2) - define GPS_MAX_RECEIVERS 1 define GPS_MAX_INSTANCES 1 define HAL_COMPASS_MAX_SENSORS 1 @@ -132,6 +131,7 @@ define HAL_PERIPH_GPS_PORT_DEFAULT 3 # for ProfiLed we need RC out and notify define HAL_PERIPH_ENABLE_RC_OUT define HAL_PERIPH_ENABLE_NOTIFY +define AP_SERIALLED_ENABLED 1 define HAL_SERIAL_ESC_COMM_ENABLED 1 define HAL_RCIN_THREAD_ENABLED 1