Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2019-11-06 14:43:41 +11:00 committed by Randy Mackay
parent 26fddd443d
commit 7fd3193c09
2 changed files with 107 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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

26
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -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);