AP_GPS: fixed ublox M10S auto-config
this copes with the M10S rejecting multiple-key VALGET calls. We fall back to fetching one at a time
This commit is contained in:
parent
9eea693679
commit
0d4018107b
@ -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<<active_config.fetch_index);
|
||||||
|
Debug("NACK %d 0x%x done=0x%x",
|
||||||
|
int(active_config.fetch_index),
|
||||||
|
unsigned(active_config.list[active_config.fetch_index].key),
|
||||||
|
unsigned(active_config.done_mask));
|
||||||
|
if (active_config.done_mask == (1U<<active_config.count)-1) {
|
||||||
|
// all done!
|
||||||
|
_unconfigured_messages &= ~active_config.unconfig_bit;
|
||||||
|
}
|
||||||
|
active_config.fetch_index++;
|
||||||
|
}
|
||||||
|
if (active_config.fetch_index < active_config.count) {
|
||||||
|
_configure_valget(active_config.list[active_config.fetch_index].key);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1226,6 +1260,16 @@ AP_GPS_UBLOX::_parse_gps(void)
|
|||||||
_unconfigured_messages &= ~active_config.unconfig_bit;
|
_unconfigured_messages &= ~active_config.unconfig_bit;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
if (active_config.fetch_index >= 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
|
// 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.done_mask = 0;
|
||||||
active_config.unconfig_bit = unconfig_bit;
|
active_config.unconfig_bit = unconfig_bit;
|
||||||
active_config.layers = layers;
|
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)];
|
uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)];
|
||||||
struct ubx_cfg_valget msg {};
|
struct ubx_cfg_valget msg {};
|
||||||
|
@ -565,6 +565,10 @@ private:
|
|||||||
uint8_t clsID;
|
uint8_t clsID;
|
||||||
uint8_t msgID;
|
uint8_t msgID;
|
||||||
};
|
};
|
||||||
|
struct PACKED ubx_ack_nack {
|
||||||
|
uint8_t clsID;
|
||||||
|
uint8_t msgID;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
struct PACKED ubx_cfg_cfg {
|
struct PACKED ubx_cfg_cfg {
|
||||||
@ -618,6 +622,7 @@ private:
|
|||||||
ubx_rxm_rawx rxm_rawx;
|
ubx_rxm_rawx rxm_rawx;
|
||||||
#endif
|
#endif
|
||||||
ubx_ack_ack ack;
|
ubx_ack_ack ack;
|
||||||
|
ubx_ack_nack nack;
|
||||||
ubx_tim_tm2 tim_tm2;
|
ubx_tim_tm2 tim_tm2;
|
||||||
} _buffer;
|
} _buffer;
|
||||||
|
|
||||||
@ -849,6 +854,7 @@ private:
|
|||||||
uint32_t done_mask;
|
uint32_t done_mask;
|
||||||
uint32_t unconfig_bit;
|
uint32_t unconfig_bit;
|
||||||
uint8_t layers;
|
uint8_t layers;
|
||||||
|
int8_t fetch_index;
|
||||||
} active_config;
|
} active_config;
|
||||||
|
|
||||||
#if GPS_MOVING_BASELINE
|
#if GPS_MOVING_BASELINE
|
||||||
|
Loading…
Reference in New Issue
Block a user