Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: fixed build after TMODE changes

Browse Source
This commit is contained in:
Andrew Tridgell 2019-12-16 18:20:57 +11:00
parent 69e8158a9d
commit 5d98edb689
2 changed files with 138 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -79,7 +79,7 @@ AP_GPS_UBLOX::_request_next_config(void)
}
// Ensure there is enough space for the largest possible outgoing message
if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
// not enough space - do it next time
return;
}
@ -210,8 +210,13 @@ AP_GPS_UBLOX::_request_next_config(void)
} else {
_unconfigured_messages &= ~CONFIG_VERSION;
}
// no need to send the initial rates, move to checking only
_next_message = STEP_PVT;
break;
case STEP_TMODE:
if (_hardware_generation >= UBLOX_F9) {
if (!_configure_valget(ConfigKey::TMODE_MODE)) {
_next_message--;
}
}
break;
default:
// this case should never be reached, do a full reset if it is hit
@ -349,7 +354,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
void
AP_GPS_UBLOX::_request_port(void)
{
if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+2)) {
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+2)) {
// not enough space - do it next time
return;
}
@ -870,8 +875,63 @@ AP_GPS_UBLOX::_parse_gps(void)
return false;
}
#endif // CONFIGURE_PPS_PIN
case MSG_CFG_VALGET: {
uint8_t cfg_len = _payload_length - sizeof(ubx_cfg_valget);
const uint8_t *cfg_data = (const uint8_t *)(&_buffer) + sizeof(ubx_cfg_valget);
while (cfg_len >= 5) {
ConfigKey id;
memcpy(&id, cfg_data, sizeof(uint32_t));
cfg_len -= 4;
cfg_data += 4;
switch (id) {
case ConfigKey::TMODE_MODE: {
uint8_t mode = cfg_data[0];
if (mode != 0) {
// ask for mode 0, to disable TIME mode
mode = 0;
_configure_valset(ConfigKey::TMODE_MODE, 1, &mode);
_unconfigured_messages |= CONFIG_TMODE_MODE;
} else {
_unconfigured_messages &= ~CONFIG_TMODE_MODE;
}
break;
}
default:
break;
}
// step over the value
const uint8_t key_size = ((uint32_t)id >> 28) & 0x07; // mask off the storage size
uint8_t step_size = 0;
switch (key_size) {
case 0x1: // bit
step_size = 1;
break;
case 0x2: // byte
step_size = 1;
break;
case 0x3: // 2 bytes
step_size = 2;
break;
case 0x4: // 4 bytes
step_size = 4;
break;
case 0x5: // 8 bytes
step_size = 8;
break;
default:
// unknown/bad key size
return false;
}
if (cfg_len <= step_size) {
cfg_len = 0;
} else {
cfg_len -= step_size;
cfg_data += step_size;
}
}
}
}
}
if (_class == CLASS_MON) {
@ -1318,7 +1378,7 @@ AP_GPS_UBLOX::_request_message_rate(uint8_t msg_class, uint8_t msg_id)
bool
AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
{
if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_msg_rate)+2)) {
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_msg_rate)+2)) {
return false;
}
@ -1329,6 +1389,52 @@ AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t
return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
}
/*
* configure F9 based key/value pair - VALSET
*/
bool
AP_GPS_UBLOX::_configure_valset(ConfigKey key, const uint8_t len, const uint8_t *value)
{
if (_hardware_generation < UBLOX_F9) {
return false;
}
struct ubx_cfg_valset msg {};
uint8_t buf[sizeof(msg)+len];
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
return false;
}
msg.version = 1;
msg.layers = 7; // all layers
msg.transaction = 0;
msg.key = uint32_t(key);
memcpy(buf, &msg, sizeof(msg));
memcpy(&buf[sizeof(msg)], value, len);
auto ret = _send_message(CLASS_CFG, MSG_CFG_VALSET, buf, sizeof(buf));
return ret;
}
/*
* configure F9 based key/value pair - VALGET
*/
bool
AP_GPS_UBLOX::_configure_valget(ConfigKey key)
{
if (_hardware_generation < UBLOX_F9) {
return false;
}
struct {
struct ubx_cfg_valget msg;
ConfigKey key;
} msg {};
if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(msg)+2)) {
return false;
}
msg.msg.version = 0;
msg.msg.layers = 0; // ram
msg.key = key;
return _send_message(CLASS_CFG, MSG_CFG_VALGET, &msg, sizeof(msg));
}
/*
* save gps configurations to non-volatile memory sent until the call of
* this message
@ -1442,7 +1548,8 @@ static const char *reasons[] = {"navigation rate",
"SBAS settings",
"PVT rate",
"time pulse settings",
"TIMEGPS rate"};
"TIMEGPS rate",
"Time mode settings"};
static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");

26
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -77,7 +77,8 @@
#define CONFIG_RATE_PVT (1<<13)
#define CONFIG_TP5 (1<<14)
#define CONFIG_RATE_TIMEGPS (1<<15)
#define CONFIG_LAST (1<<16) // this must always be the last bit
#define CONFIG_TMODE_MODE (1<<16)
#define CONFIG_LAST (1<<17) // this must always be the last bit
#define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
@ -211,6 +212,23 @@ private:
uint8_t scanmode2;
uint32_t scanmode1;
};
// F9 config keys
enum class ConfigKey : uint32_t {
TMODE_MODE = 0x20030001,
};
struct PACKED ubx_cfg_valset {
uint8_t version;
uint8_t layers;
uint8_t transaction;
uint8_t reserved[1];
uint32_t key;
};
struct PACKED ubx_cfg_valget {
uint8_t version;
uint8_t layers;
uint8_t reserved[2];
// variable length data, check buffer length
};
struct PACKED ubx_nav_posllh {
uint32_t itow; // GPS msToW
int32_t longitude;
@ -467,6 +485,7 @@ private:
ubx_cfg_gnss gnss;
#endif
ubx_cfg_sbas sbas;
ubx_cfg_valget valget;
ubx_nav_svinfo_header svinfo_header;
ubx_nav_relposned relposned;
#if UBLOX_RXM_RAW_LOGGING
@ -515,6 +534,8 @@ private:
MSG_CFG_SBAS = 0x16,
MSG_CFG_GNSS = 0x3E,
MSG_CFG_TP5 = 0x31,
MSG_CFG_VALSET = 0x8A,
MSG_CFG_VALGET = 0x8B,
MSG_MON_HW = 0x09,
MSG_MON_HW2 = 0x0B,
MSG_MON_VER = 0x04,
@ -568,6 +589,7 @@ private:
STEP_POLL_NAV, // poll NAV settings
STEP_POLL_GNSS, // poll GNSS
STEP_POLL_TP5, // poll TP5
STEP_TMODE, // set TMODE-MODE
STEP_DOP,
STEP_MON_HW,
STEP_MON_HW2,
@ -624,6 +646,8 @@ private:
bool havePvtMsg;
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
bool _configure_valset(ConfigKey key, const uint8_t len, const uint8_t *value);
bool _configure_valget(ConfigKey key);
void _configure_rate(void);
void _configure_sbas(bool enable);
void _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b);