diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 245d5eb517..cf3cdd6d30 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -994,6 +994,40 @@ AP_GPS_UBLOX::_parse_gps(void) } } } + if(_msg_id == MSG_ACK_NACK) { + switch(_buffer.nack.clsID) { + case CLASS_CFG: + switch(_buffer.nack.msgID) { + case MSG_CFG_VALGET: + if (active_config.list != nullptr) { + /* + likely this device does not support fetching multiple keys at once, go one at a time + */ + if (active_config.fetch_index == -1) { + Debug("NACK starting %u", unsigned(active_config.count)); + active_config.fetch_index = 0; + } else { + // the device does not support the config key we asked for, + // consider the bit as done + active_config.done_mask |= (1U<= 0 && + active_config.fetch_index < active_config.count && + id == active_config.list[active_config.fetch_index].key) { + active_config.fetch_index++; + if (active_config.fetch_index < active_config.count) { + _configure_valget(active_config.list[active_config.fetch_index].key); + Debug("valget %d 0x%x", int(active_config.fetch_index), + unsigned(active_config.list[active_config.fetch_index].key)); + } + } } // step over the value @@ -1823,6 +1867,11 @@ AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint active_config.done_mask = 0; active_config.unconfig_bit = unconfig_bit; active_config.layers = layers; + // we start by fetching multiple values at once (with fetch_index + // -1) then if we get a NACK for VALGET we switch to fetching one + // value at a time. This copes with the M10S which can only fetch + // one value at a time + active_config.fetch_index = -1; uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)]; struct ubx_cfg_valget msg {}; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index e387ae4ed3..eea8967af9 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -565,6 +565,10 @@ private: uint8_t clsID; uint8_t msgID; }; + struct PACKED ubx_ack_nack { + uint8_t clsID; + uint8_t msgID; + }; struct PACKED ubx_cfg_cfg { @@ -618,6 +622,7 @@ private: ubx_rxm_rawx rxm_rawx; #endif ubx_ack_ack ack; + ubx_ack_nack nack; ubx_tim_tm2 tim_tm2; } _buffer; @@ -849,6 +854,7 @@ private: uint32_t done_mask; uint32_t unconfig_bit; uint8_t layers; + int8_t fetch_index; } active_config; #if GPS_MOVING_BASELINE