From 6c31fb1bb6194bc1d54c0cc05c68ea12732f70e4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Nov 2019 14:43:41 +1100 Subject: [PATCH] AP_GPS: added automatic config of TMODE on F9 the Drotek F9 GPS ships with TMODE enabled, which means we don't get a 3D fix. This also adds the VALGET/VALSET msgs we will need for automatic moving baseline config --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 85 +++++++++++++++++++++++++++++-- libraries/AP_GPS/AP_GPS_UBLOX.h | 26 +++++++++- 2 files changed, 107 insertions(+), 4 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index ef49499deb..325adf0b09 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -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 @@ -870,8 +875,36 @@ 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]; + cfg_len -= 1; + cfg_data += 1; + 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: + // we don't know the length so we stop parsing + return false; + } + } + } } - } if (_class == CLASS_MON) { @@ -1325,6 +1358,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() < (int16_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() < (int16_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 diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 90b35785ed..292ee992df 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -77,13 +77,14 @@ #define CONFIG_RATE_PVT (1<<13) #define CONFIG_TP5 (1<<14) #define CONFIG_RATE_TIMEGPS (1<<15) +#define CONFIG_TMODE_MODE (1<<16) #define CONFIG_LAST (1<<16) // this must always be the last bit #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED) #define CONFIG_ALL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_SOL | CONFIG_RATE_VELNED \ | CONFIG_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \ - | CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS) + | CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS | CONFIG_TMODE_MODE) //Configuration Sub-Sections #define SAVE_CFG_IO (1<<0) @@ -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);