From f777421ecb559dd37d5a812213c59b18e61ae796 Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Wed, 10 Nov 2021 09:21:48 -0700 Subject: [PATCH] UAVCAN/UART1 Moving Baseline for U-Blox F9P --- src/ubx.cpp | 92 ++++++++++++++++++++++++++++++++++++++++++++++------- src/ubx.h | 53 ++++++++++++++++++++---------- 2 files changed, 117 insertions(+), 28 deletions(-) diff --git a/src/ubx.cpp b/src/ubx.cpp index d777190..ded7e9b 100644 --- a/src/ubx.cpp +++ b/src/ubx.cpp @@ -107,11 +107,15 @@ GPSDriverUBX::configure(unsigned &baudrate, const GPSConfig &config) if (_interface == Interface::UART) { /* try different baudrates */ - const unsigned baudrates[] = {38400, 57600, 9600, 115200, 230400}; + const unsigned baudrates[] = {38400, 57600, 9600, 115200, 230400, 460800, 921600}; unsigned baud_i; unsigned desired_baudrate = auto_baudrate ? UBX_BAUDRATE_M8_AND_NEWER : baudrate; + if ((_mode == UBXMode::RoverWithMovingBaseUART1) || (_mode == UBXMode::MovingBaseUART1)) { + desired_baudrate = UART1_BAUDRATE_HEADING; + } + for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) { unsigned test_baudrate = baudrates[baud_i]; @@ -529,7 +533,15 @@ int GPSDriverUBX::configureDevice(const GNSSSystemsMask &gnssSystems) // measurement rate // In case of F9P we use 10Hz, otherwise 8Hz (receivers such as M9N can go higher as well, but // the number of used satellites will be restricted to 16. Not mentioned in datasheet) - const int rate_meas = (_board == Board::u_blox9_F9P) ? 100 : 125; + int rate_meas; + + if ((_mode == UBXMode::MovingBaseUART1) || (_mode == UBXMode::RoverWithMovingBaseUART1)) { + rate_meas = 125; //8Hz for heading. At 8Hz, the rover will drop output to 4Hz. + + } else { + rate_meas = (_board == Board::u_blox9_F9P) ? 100 : 125; + } + cfgValset(UBX_CFG_KEY_RATE_MEAS, rate_meas, cfg_valset_msg_size); cfgValset(UBX_CFG_KEY_RATE_NAV, 1, cfg_valset_msg_size); cfgValset(UBX_CFG_KEY_RATE_TIMEREF, 0, cfg_valset_msg_size); @@ -658,6 +670,7 @@ int GPSDriverUBX::configureDevice(const GNSSSystemsMask &gnssSystems) } } + int uart1_baudrate = UART1_BAUDRATE_HEADING; int uart2_baudrate = 230400; if (_mode == UBXMode::RoverWithMovingBase) { @@ -713,6 +726,59 @@ int GPSDriverUBX::configureDevice(const GNSSSystemsMask &gnssSystems) if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) { return -1; } + + } else if (_mode == UBXMode::RoverWithMovingBaseUART1) { + UBX_DEBUG("Configuring UART1 for rover"); + // heading output @1Hz + set baudrate + cfg_valset_msg_size = initCfgValset(); + cfgValset(UBX_CFG_KEY_CFG_UART1INPROT_UBX, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 0, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_CFG_UART1_BAUDRATE, uart1_baudrate, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 1, cfg_valset_msg_size); + + if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) { + return -1; + } + + if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) { + return -1; + } + + setBaudrate(uart1_baudrate); + + } else if (_mode == UBXMode::MovingBaseUART1) { + UBX_DEBUG("Configuring UART1 for moving base"); + // enable RTCM output on uart1 + set baudrate + cfg_valset_msg_size = initCfgValset(); + cfgValset(UBX_CFG_KEY_CFG_UART1INPROT_UBX, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, 0, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_CFG_UART1_BAUDRATE, uart1_baudrate, cfg_valset_msg_size); + + cfgValset(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 0, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART2, 0, cfg_valset_msg_size); + + cfgValset(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART1, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART1, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART1, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1084_UART1, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1094_UART1, 1, cfg_valset_msg_size); + cfgValset(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1124_UART1, 1, cfg_valset_msg_size); + + if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) { + return -1; + } + + if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) { + return -1; + } + + setBaudrate(uart1_baudrate); } return 0; @@ -1139,7 +1205,7 @@ GPSDriverUBX::parseChar(const uint8_t b) case UBX_DECODE_RTCM3: if (_rtcm_parsing->addByte(b)) { - UBX_DEBUG("got RTCM message with length %i", static_cast(_rtcm_parsing->messageLength())); + //UBX_DEBUG("got RTCM message with length %i", static_cast(_rtcm_parsing->messageLength())); gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength()); decodeInit(); } @@ -1366,7 +1432,7 @@ GPSDriverUBX::payloadRxInit() if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL && _configured) { /* don't attempt for every message to disable, some might not be disabled */ _disable_cmd_last = t; - UBX_DEBUG("ubx disabling msg 0x%04x (0x%04x)", SWAP16((unsigned)_rx_msg), key_id); + UBX_DEBUG("ubx disabling msg 0x%04x (0x%04x)", SWAP16((unsigned)_rx_msg), (uint16_t)key_id); // this will overwrite _buf, which is fine, as we'll return -1 and abort further parsing int cfg_valset_msg_size = initCfgValset(); @@ -1617,7 +1683,7 @@ GPSDriverUBX::payloadRxAddMonVer(const uint8_t b) // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT); _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version); - UBX_DEBUG("VER hash 0x%08x", _ubx_version); + UBX_DEBUG("VER hash 0x%08x", (uint16_t)_ubx_version); UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); @@ -1904,7 +1970,7 @@ GPSDriverUBX::payloadRxDone() { ubx_payload_rx_nav_svin_t &svin = _buf.payload_rx_nav_svin; - UBX_DEBUG("Survey-in status: %is cur accuracy: %imm nr obs: %i valid: %i active: %i", + UBX_DEBUG("Survey-in status: %lus cur accuracy: %lumm nr obs: %lu valid: %i active: %i", svin.dur, svin.meanAcc / 10, svin.obs, static_cast(svin.valid), static_cast(svin.active)); SurveyInStatus status{}; @@ -1947,17 +2013,14 @@ GPSDriverUBX::payloadRxDone() case UBX_MSG_NAV_RELPOSNED: UBX_TRACE_RXMSG("Rx NAV-RELPOSNED"); - if (_mode == UBXMode::RoverWithMovingBase) { + if ((_mode == UBXMode::RoverWithMovingBase) || (_mode == UBXMode::RoverWithMovingBaseUART1)) { float heading = _buf.payload_rx_nav_relposned.relPosHeading * 1e-5f; float heading_acc = _buf.payload_rx_nav_relposned.accHeading * 1e-5f; float rel_length = _buf.payload_rx_nav_relposned.relPosLength + _buf.payload_rx_nav_relposned.relPosHPLength * 1e-2f; float rel_length_acc = _buf.payload_rx_nav_relposned.accLength * 1e-2f; bool heading_valid = _buf.payload_rx_nav_relposned.flags & (1 << 8); bool rel_pos_valid = _buf.payload_rx_nav_relposned.flags & (1 << 2); - (void)heading_acc; (void)rel_length_acc; - UBX_DEBUG("Heading: %.1f deg, acc: %.1f deg, relLen: %.1f cm, relAcc: %.1f cm, valid: %i %i", (double)heading, - (double)heading_acc, (double)rel_length, (double)rel_length_acc, heading_valid, rel_pos_valid); if (heading_valid && rel_pos_valid && rel_length < 1000.f) { // validity & sanity checks heading *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi] @@ -1968,6 +2031,13 @@ GPSDriverUBX::payloadRxDone() } _gps_position->heading = heading; + + heading_acc *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi] + + _gps_position->heading_accuracy = heading_acc; + + UBX_DEBUG("Heading: %.3f rad, acc: %.1f deg, relLen: %.1f cm, relAcc: %.1f cm, valid: %i %i", (double)heading, + (double)heading_acc, (double)rel_length, (double)rel_length_acc, heading_valid, rel_pos_valid); } ret = 1; @@ -2130,7 +2200,7 @@ GPSDriverUBX::decodeInit() _rx_payload_length = 0; _rx_payload_index = 0; - if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM) { + if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM || _mode == UBXMode::MovingBaseUART1) { if (!_rtcm_parsing) { _rtcm_parsing = new RTCMParsing(); } diff --git a/src/ubx.h b/src/ubx.h index 283ea6a..249b497 100644 --- a/src/ubx.h +++ b/src/ubx.h @@ -55,7 +55,7 @@ #define UBX_CONFIG_TIMEOUT 250 // ms, timeout for waiting ACK -#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_PACKET_TIMEOUT 8 // ms, if now data during this delay assume that full update received #define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval @@ -65,6 +65,8 @@ #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 +#define UART1_BAUDRATE_HEADING 921600 + /* Message Classes */ #define UBX_CLASS_NAV 0x01 #define UBX_CLASS_RXM 0x02 @@ -333,20 +335,35 @@ #define UBX_CFG_KEY_TMODE_SVIN_MIN_DUR 0x40030010 #define UBX_CFG_KEY_TMODE_SVIN_ACC_LIMIT 0x40030011 -#define UBX_CFG_KEY_MSGOUT_UBX_MON_RF_I2C 0x20910359 -#define UBX_CFG_KEY_MSGOUT_UBX_NAV_SVIN_I2C 0x20910088 -#define UBX_CFG_KEY_MSGOUT_UBX_NAV_SAT_I2C 0x20910015 -#define UBX_CFG_KEY_MSGOUT_UBX_NAV_DOP_I2C 0x20910038 -#define UBX_CFG_KEY_MSGOUT_UBX_NAV_PVT_I2C 0x20910006 +#define UBX_CFG_KEY_MSGOUT_UBX_MON_RF_I2C 0x20910359 +#define UBX_CFG_KEY_MSGOUT_UBX_NAV_SVIN_I2C 0x20910088 +#define UBX_CFG_KEY_MSGOUT_UBX_NAV_SAT_I2C 0x20910015 +#define UBX_CFG_KEY_MSGOUT_UBX_NAV_DOP_I2C 0x20910038 +#define UBX_CFG_KEY_MSGOUT_UBX_NAV_PVT_I2C 0x20910006 #define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_I2C 0x2091008d -#define UBX_CFG_KEY_MSGOUT_UBX_RXM_SFRBX_I2C 0x20910231 -#define UBX_CFG_KEY_MSGOUT_UBX_RXM_RAWX_I2C 0x209102a4 -#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1005_I2C 0x209102bd -#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_I2C 0x209102cc -#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_I2C 0x209102d1 -#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_I2C 0x20910318 -#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_I2C 0x209102d6 -#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C 0x20910303 +#define UBX_CFG_KEY_MSGOUT_UBX_RXM_SFRBX_I2C 0x20910231 +#define UBX_CFG_KEY_MSGOUT_UBX_RXM_RAWX_I2C 0x209102a4 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1005_I2C 0x209102bd +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_I2C 0x209102cc +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_I2C 0x209102d1 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_I2C 0x20910318 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_I2C 0x209102d6 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C 0x20910303 + +#define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1 0x2091008e +#define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART2 0x2091008f + +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART1 0x209102ff +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_1_UART1 0x20910382 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_UART1 0x209102cd +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_UART1 0x209102d2 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_UART1 0x20910319 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_UART1 0x209102d7 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART1 0x20910304 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART1 0x2091035f +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1084_UART1 0x20910364 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1094_UART1 0x20910369 +#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1124_UART1 0x2091036e #define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART2 0x20910300 #define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_1_UART2 0x20910383 @@ -888,9 +905,11 @@ class GPSDriverUBX : public GPSBaseStationSupport { public: enum class UBXMode : uint8_t { - Normal, ///< all non-heading configurations - RoverWithMovingBase, ///< expect RTCM input on UART2 from a moving base for heading output - MovingBase, ///< RTCM output on UART2 to a rover (GPS is installed on the vehicle) + Normal, ///< all non-heading configurations + RoverWithMovingBase, ///< expect RTCM input on UART2 from a moving base for heading output + MovingBase, ///< RTCM output on UART2 to a rover (GPS is installed on the vehicle) + RoverWithMovingBaseUART1, ///< expect RTCM input on UART1 from a moving base for heading output + MovingBaseUART1, ///< RTCM output on UART1 to a rover (GPS is installed on the vehicle) }; GPSDriverUBX(Interface gpsInterface, GPSCallbackPtr callback, void *callback_user,