From 4867400ac39ce2aede06d6275b42b2a1cb49e79d Mon Sep 17 00:00:00 2001 From: Thomas Frans Date: Thu, 30 May 2024 16:55:29 +0200 Subject: [PATCH] gnss(septentrio): rework driver Rewrite large parts from the original driver to make the code more understandable, maintainable, document more items, move related things into their own files and make the code more consistent. --- msg/GpsDump.msg | 12 +- msg/SensorGps.msg | 9 +- src/drivers/gnss/septentrio/CMakeLists.txt | 7 +- src/drivers/gnss/septentrio/README.md | 6 + src/drivers/gnss/septentrio/module.h | 80 + src/drivers/gnss/septentrio/module.yaml | 147 +- src/drivers/gnss/septentrio/rtcm.cpp | 137 +- src/drivers/gnss/septentrio/rtcm.h | 102 +- src/drivers/gnss/septentrio/sbf.h | 289 ---- src/drivers/gnss/septentrio/sbf/decoder.cpp | 252 +++ src/drivers/gnss/septentrio/sbf/decoder.h | 233 +++ src/drivers/gnss/septentrio/sbf/messages.h | 353 ++++ src/drivers/gnss/septentrio/septentrio.cpp | 1714 +++++++++++-------- src/drivers/gnss/septentrio/septentrio.h | 521 ++++-- src/drivers/gnss/septentrio/util.cpp | 13 +- src/drivers/gnss/septentrio/util.h | 13 +- 16 files changed, 2660 insertions(+), 1228 deletions(-) create mode 100644 src/drivers/gnss/septentrio/README.md create mode 100644 src/drivers/gnss/septentrio/module.h delete mode 100644 src/drivers/gnss/septentrio/sbf.h create mode 100644 src/drivers/gnss/septentrio/sbf/decoder.cpp create mode 100644 src/drivers/gnss/septentrio/sbf/decoder.h create mode 100644 src/drivers/gnss/septentrio/sbf/messages.h diff --git a/msg/GpsDump.msg b/msg/GpsDump.msg index 3aa1313aa680..2477bcfa3a1e 100644 --- a/msg/GpsDump.msg +++ b/msg/GpsDump.msg @@ -1,12 +1,10 @@ # This message is used to dump the raw gps communication to the log. -# Set the parameter GPS_DUMP_COMM to 1 to use this. -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -uint8 instance # Instance of GNSS receiver - -uint8 len # length of data, MSB bit set = message to the gps device, - # clear = message from the device -uint8[79] data # data to write to the log +uint8 instance # Instance of GNSS receiver +uint8 len # length of data, MSB bit set = message to the gps device, + # clear = message from the device +uint8[79] data # data to write to the log uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index db18899cb1c0..ce2bfad4fd8e 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -12,7 +12,14 @@ float64 altitude_ellipsoid_m # Altitude above Ellipsoid, meters float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) float32 c_variance_rad # GPS course accuracy estimate, (radians) -uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. +uint8 FIX_TYPE_NONE = 1 # Value 0 is also valid to represent no fix. +uint8 FIX_TYPE_2D = 2 +uint8 FIX_TYPE_3D = 3 +uint8 FIX_TYPE_RTCM_CODE_DIFFERENTIAL = 4 +uint8 FIX_TYPE_RTK_FLOAT = 5 +uint8 FIX_TYPE_RTK_FIXED = 6 +uint8 FIX_TYPE_EXTRAPOLATED = 8 +uint8 fix_type # Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. float32 eph # GPS horizontal position accuracy (metres) float32 epv # GPS vertical position accuracy (metres) diff --git a/src/drivers/gnss/septentrio/CMakeLists.txt b/src/drivers/gnss/septentrio/CMakeLists.txt index b4974135237f..62451fc9b1f3 100644 --- a/src/drivers/gnss/septentrio/CMakeLists.txt +++ b/src/drivers/gnss/septentrio/CMakeLists.txt @@ -35,11 +35,16 @@ px4_add_module( MODULE driver__septentrio MAIN septentrio COMPILE_FLAGS - -Wno-stringop-overflow # due to https://gcc.gnu.org/bugzilla/show_bug.cgi?id=91707 + # -DDEBUG_BUILD # Enable during development of the module + -DSEP_LOG_ERROR # Enable module-level error logs + # -DSEP_LOG_WARN # Enable module-level warning logs + # -DSEP_LOG_INFO # Enable module-level info logs + # -DSEP_LOG_TRACE_PARSING # Tracing of parsing steps SRCS septentrio.cpp util.cpp rtcm.cpp + sbf/decoder.cpp MODULE_CONFIG module.yaml ) diff --git a/src/drivers/gnss/septentrio/README.md b/src/drivers/gnss/septentrio/README.md new file mode 100644 index 000000000000..664df63a145d --- /dev/null +++ b/src/drivers/gnss/septentrio/README.md @@ -0,0 +1,6 @@ +# Septentrio GNSS Driver + +## SBF Library + +The `sbf/` directory contains all the logic required to work with SBF, including message +definitions, block IDs and a simple parser for the messages used by PX4. \ No newline at end of file diff --git a/src/drivers/gnss/septentrio/module.h b/src/drivers/gnss/septentrio/module.h new file mode 100644 index 000000000000..28b52240b643 --- /dev/null +++ b/src/drivers/gnss/septentrio/module.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file module.h + * + * Module functionality for the Septentrio GNSS driver. + * + * @author Thomas Frans +*/ + +#pragma once + +#include + +#ifdef DEBUG_BUILD +#ifndef SEP_LOG_ERROR +#define SEP_LOG_ERROR +#endif +#ifndef SEP_LOG_WARN +#define SEP_LOG_WARN +#endif +#ifndef SEP_LOG_INFO +#define SEP_LOG_INFO +#endif +#endif + +#ifdef SEP_LOG_ERROR +#define SEP_ERR(...) {PX4_WARN(__VA_ARGS__);} +#else +#define SEP_ERR(...) {} +#endif + +#ifdef SEP_LOG_WARN +#define SEP_WARN(...) {PX4_WARN(__VA_ARGS__);} +#else +#define SEP_WARN(...) {} +#endif + +#ifdef SEP_LOG_INFO +#define SEP_INFO(...) {PX4_INFO(__VA_ARGS__);} +#else +#define SEP_INFO(...) {} +#endif + +#ifdef SEP_LOG_TRACE_PARSING +#define SEP_TRACE_PARSING(...) {PX4_DEBUG(__VA_ARGS__);} +#else +#define SEP_TRACE_PARSING(...) {} +#endif diff --git a/src/drivers/gnss/septentrio/module.yaml b/src/drivers/gnss/septentrio/module.yaml index eedeb12c49aa..49788b7f9759 100644 --- a/src/drivers/gnss/septentrio/module.yaml +++ b/src/drivers/gnss/septentrio/module.yaml @@ -4,18 +4,57 @@ serial_config: - command: set DUAL_GPS_ARGS "-e ${SERIAL_DEV} -g p:${BAUD_PARAM}" port_config_param: name: SEP_PORT2_CFG - group: Septentrio GNSS Receiver + group: Septentrio label: Secondary GPS port - command: septentrio start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} ${DUAL_GPS_ARGS} port_config_param: name: SEP_PORT1_CFG - group: Septentrio GNSS Receiver + group: Septentrio default: GPS1 label: GPS Port parameters: - - group: Septentrio GNSS Receiver + - group: Septentrio definitions: + SEP_STREAM_MAIN: + description: + short: Main stream used during automatic configuration + long: | + The stream the autopilot sets up on the receiver to output the main data. + + Set this to another value if the default stream is already used for another purpose. + type: int32 + min: 1 + max: 10 + default: 1 + reboot_required: true + SEP_STREAM_LOG: + description: + short: Logging stream used during automatic configuration + long: | + The stream the autopilot sets up on the receiver to output the logging data. + + Set this to another value if the default stream is already used for another purpose. + type: int32 + min: 1 + max: 10 + default: 2 + reboot_required: true + SEP_OUTP_HZ: + description: + short: Output frequency of main SBF blocks + long: | + The output frequency of the main SBF blocks needed for PVT information. + type: enum + min: 0 + max: 3 + default: 1 + values: + 0: 5 Hz + 1: 10 Hz + 2: 20 Hz + 3: 25 Hz + reboot_required: true SEP_YAW_OFFS: description: short: Heading/Yaw offset for dual antenna GPS @@ -65,16 +104,102 @@ parameters: description: short: Log GPS communication data long: | - If this is set to 1, all GPS communication data will be published via uORB, - and written to the log file as gps_dump message. + Dump raw communication data from and to the connected receiver to the log file. + type: enum + default: 0 + min: 0 + max: 3 + values: + 0: Disabled + 1: From receiver + 2: To receiver + 3: Both + SEP_AUTO_CONFIG: + description: + short: Toggle automatic receiver configuration + long: | + By default, the receiver is automatically configured. Sometimes it may be used for multiple purposes. + If the offered parameters aren't sufficient, this parameter can be disabled to have full control of the receiver configuration. + A good way to use this is to enable automatic configuration, let the receiver be configured, and then disable it to make manual adjustments. + type: boolean + default: true + reboot_required: true + SEP_CONST_USAGE: + description: + short: Usage of different constellations + long: | + Choice of which constellations the receiver should use for PVT computation. - If this is set to 2, the main GPS is configured to output RTCM data, - which is then logged as gps_dump and can be used for PPK. + When this is 0, the constellation usage isn't changed. + type: bitmask + bit: + 0: GPS + 1: GLONASS + 2: Galileo + 3: SBAS + 4: BeiDou + default: 0 + min: 0 + max: 63 + reboot_required: true + SEP_LOG_HZ: + description: + short: Logging frequency for the receiver + long: | + Select the frequency at which the connected receiver should log data to its internal storage. + type: enum + default: 0 + min: 0 + max: 10 + values: + 0: Disabled + 1: 0.1 Hz + 2: 0.2 Hz + 3: 0.5 Hz + 4: 1 Hz + 5: 2 Hz + 6: 5 Hz + 7: 10 Hz + 8: 20 Hz + 9: 25 Hz + 10: 50 Hz + reboot_required: true + SEP_LOG_LEVEL: + description: + short: Logging level for the receiver + long: | + Select the level of detail that needs to be logged by the receiver. + type: enum + default: 2 + min: 0 + max: 3 + values: + 0: Lite + 1: Basic + 2: Default + 3: Full + reboot_required: true + SEP_LOG_FORCE: + description: + short: Whether to overwrite or add to existing logging + long: | + When the receiver is already set up to log data, this decides whether extra logged data should be added or overwrite existing data. + type: boolean + default: false + reboot_required: true + SEP_HARDW_SETUP: + description: + short: Setup and expected use of the hardware + long: | + Setup and expected use of the hardware. + + - Default: Use two receivers as completely separate instances. + - Moving base: Use two receivers in a rover & moving base setup for heading. type: enum default: 0 min: 0 - max: 2 + max: 1 values: - 0: Disable - 1: Full communication - 2: RTCM output (PPK) \ No newline at end of file + 0: Default + 1: Moving base + reboot_required: true diff --git a/src/drivers/gnss/septentrio/rtcm.cpp b/src/drivers/gnss/septentrio/rtcm.cpp index d98dc2932aa2..007b2a168e33 100644 --- a/src/drivers/gnss/septentrio/rtcm.cpp +++ b/src/drivers/gnss/septentrio/rtcm.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,57 +31,132 @@ * ****************************************************************************/ +/** + * @file rtcm.cpp + * + * @author Thomas Frans +*/ + #include "rtcm.h" + #include +#include + +#include "module.h" + +namespace septentrio +{ + +namespace rtcm +{ -RTCMParsing::RTCMParsing() +Decoder::Decoder() { reset(); } -RTCMParsing::~RTCMParsing() +Decoder::~Decoder() { - delete[] _buffer; + delete[] _message; } -void RTCMParsing::reset() +Decoder::State Decoder::add_byte(uint8_t byte) { - if (!_buffer) { - _buffer = new uint8_t[RTCM_INITIAL_BUFFER_LENGTH]; - _buffer_len = RTCM_INITIAL_BUFFER_LENGTH; + switch (_state) { + case State::SearchingPreamble: + if (byte == PREAMBLE) { + _message[_current_index] = byte; + _current_index++; + _state = State::GettingHeader; + } + + break; + + case State::GettingHeader: + _message[_current_index] = byte; + _current_index++; + + if (header_received()) { + _message_length = parse_message_length(); + + if (_message_length > MAX_BODY_SIZE) { + reset(); + return _state; + + } else if (_message_length + HEADER_SIZE + CRC_SIZE > INITIAL_BUFFER_LENGTH) { + uint16_t new_buffer_size = _message_length + HEADER_SIZE + CRC_SIZE; + uint8_t *new_buffer = new uint8_t[new_buffer_size]; + + if (!new_buffer) { + reset(); + return _state; + } + + memcpy(new_buffer, _message, HEADER_SIZE); + delete[](_message); + + _message = new_buffer; + } + + _state = State::Busy; + } + + break; + + case State::Busy: + _message[_current_index] = byte; + _current_index++; + + if (_message_length + HEADER_SIZE + CRC_SIZE == _current_index) { + _state = State::Done; + } + + break; + + case State::Done: + SEP_WARN("RTCM: Discarding excess byte"); + break; } - _pos = 0; - _message_length = _buffer_len; + return _state; } -bool RTCMParsing::addByte(uint8_t b) +void Decoder::reset() { - if (!_buffer) { - return false; + if (_message) { + delete[] _message; } - _buffer[_pos++] = b; + _message = new uint8_t[INITIAL_BUFFER_LENGTH]; + _current_index = 0; + _message_length = 0; + _state = State::SearchingPreamble; +} - if (_pos == 3) { - _message_length = (((uint16_t)_buffer[1] & 3) << 8) | (_buffer[2]); +uint16_t Decoder::parse_message_length() const +{ + if (!header_received()) { + return PX4_ERROR; + } - if (_message_length + 6 > _buffer_len) { - uint16_t new_buffer_len = _message_length + 6; - uint8_t *new_buffer = new uint8_t[new_buffer_len]; + return ((static_cast(_message[1]) & 3) << 8) | _message[2]; +} - if (!new_buffer) { - delete[](_buffer); - _buffer = nullptr; - return false; - } +bool Decoder::header_received() const +{ + return _current_index >= HEADER_SIZE; +} - memcpy(new_buffer, _buffer, 3); - delete[](_buffer); - _buffer = new_buffer; - _buffer_len = new_buffer_len; - } - } +uint16_t Decoder::received_bytes() const +{ + return _current_index; +} - return _message_length + 6 == _pos; +uint16_t Decoder::message_id() const +{ + return (_message[3] << 4) | (_message[4] >> 4); } + +} // namespace rtcm + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/rtcm.h b/src/drivers/gnss/septentrio/rtcm.h index eb1c5eb9cf16..672ac08da9e1 100644 --- a/src/drivers/gnss/septentrio/rtcm.h +++ b/src/drivers/gnss/septentrio/rtcm.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,40 +31,102 @@ * ****************************************************************************/ +/** + * @file rtcm.h + * + * @author Thomas Frans + */ + #pragma once #include +#include + +namespace septentrio +{ -/* RTCM3 */ -#define RTCM3_PREAMBLE 0xD3 -#define RTCM_INITIAL_BUFFER_LENGTH 300 /**< initial maximum message length of an RTCM message */ +namespace rtcm +{ +constexpr uint8_t PREAMBLE = 0xD3; +constexpr uint8_t HEADER_SIZE = 3; ///< Total number of bytes in a message header. +constexpr uint8_t CRC_SIZE = 3; ///< Total number of bytes in the CRC. +constexpr uint8_t LENGTH_FIELD_BITS = 10; ///< Total number of bits used for the length. +constexpr uint16_t MAX_BODY_SIZE = 1 << LENGTH_FIELD_BITS; ///< Maximum allowed size of the message body. -class RTCMParsing +class Decoder { public: - RTCMParsing(); - ~RTCMParsing(); + enum class State { + /// Searching for the first byte of an RTCM message. + SearchingPreamble, + + /// Getting the complete header of an RTCM message. + GettingHeader, + + /// Getting a complete RTCM message. + Busy, + + /// Complete RTCM message is available. + Done, + }; + + Decoder(); + ~Decoder(); /** - * reset the parsing state + * Add a byte to the current message. + * + * @param byte The new byte. + * + * @return true if message complete (use @message to get it) */ - void reset(); + State add_byte(uint8_t b); /** - * add a byte to the message - * @param b - * @return true if message complete (use @message to get it) + * @brief Reset the parser to a clean state. */ - bool addByte(uint8_t b); + void reset(); + + uint8_t *message() const { return _message; } + + /** + * @brief Number of received bytes of the current message. + */ + uint16_t received_bytes() const; - uint8_t *message() const { return _buffer; } - uint16_t messageLength() const { return _pos; } - uint16_t messageId() const { return (_buffer[3] << 4) | (_buffer[4] >> 4); } + /** + * @brief The id of the current message. + * + * This should only be called if the message has been received completely. + * + * @return The id of the current complete message. + */ + uint16_t message_id() const; private: - uint8_t *_buffer{nullptr}; - uint16_t _buffer_len{}; - uint16_t _pos{}; ///< next position in buffer - uint16_t _message_length{}; ///< message length without header & CRC (both 3 bytes) + static constexpr uint16_t INITIAL_BUFFER_LENGTH = 300; + + /** + * @brief Parse the message lentgh of the current message. + * + * @return The expected length of the current message without header and CRC. + */ + uint16_t parse_message_length() const; + + /** + * @brief Check whether the full header has been received. + * + * @return `true` if the full header is available, `false` otherwise. + */ + bool header_received() const; + + uint8_t *_message{nullptr}; + uint16_t _message_length; ///< The total length of the message excluding header and CRC (3 bytes each). + uint16_t _current_index; ///< The current index of the byte we expect to read into the buffer. + State _state{State::SearchingPreamble}; ///< Current state of the parser. }; + +} // namespace rtcm + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/sbf.h b/src/drivers/gnss/septentrio/sbf.h deleted file mode 100644 index 58be4fb3e49a..000000000000 --- a/src/drivers/gnss/septentrio/sbf.h +++ /dev/null @@ -1,289 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -/** - * @file sbf.h - * @brief Septentrio binary format (SBF) protocol definitions. - */ - -#define SBF_SYNC1 0x24 ///< Leading '$' of SBF blocks -#define SBF_SYNC2 0x40 ///< Leading '@' of SBF blocks -#define SBF_PVTGEODETIC_DNU 100000.0 ///< Do-Not-Use value for PVTGeodetic - -// Block IDs -#define SBF_ID_DOP 4001 -#define SBF_ID_PVTGeodetic 4007 -#define SBF_ID_ChannelStatus 4013 -#define SBF_ID_VelCovGeodetic 5908 -#define SBF_ID_AttEuler 5938 -#define SBF_ID_AttCovEuler 5939 - -#pragma pack(push, 1) // SBF protocol binary message and payload definitions - -typedef struct { - uint8_t mode_type: 4; /**< Bit field indicating the PVT mode type, as follows: - 0: No PVT available (the Error field indicates the cause of the absence of the PVT solution) - 1: Stand-Alone PVT - 2: Differential PVT - 3: Fixed location - 4: RTK with fixed ambiguities - 5: RTK with float ambiguities - 6: SBAS aided PVT - 7: moving-base RTK with fixed ambiguities - 8: moving-base RTK with float ambiguities - 10:Precise Point Positioning (PPP) */ - uint8_t mode_reserved: 2; /**< Reserved */ - uint8_t mode_base_fixed: 1; /**< Set if the user has entered the command setPVTMode,base,auto and the receiver - is still in the process of determining its fixed position. */ - uint8_t mode_2d: 1; /**< 2D/3D flag: set in 2D mode(height assumed constant and not computed). */ - uint8_t error; /**< PVT error code. The following values are defined: - 0: No Error - 1: Not enough measurements - 2: Not enough ephemerides available - 3: DOP too large (larger than 15) - 4: Sum of squared residuals too large - 5: No convergence - 6: Not enough measurements after outlier rejection - 7: Position output prohibited due to export laws - 8: Not enough differential corrections available - 9: Base station coordinates unavailable - 10:Ambiguities not fixed and user requested to only output RTK-fixed positions - Note: if this field has a non-zero value, all following fields are set to their Do-Not-Use value. */ - double latitude; /**< Marker latitude, from -PI/2 to +PI/2, positive North of Equator */ - double longitude; /**< Marker longitude, from -PI to +PI, positive East of Greenwich */ - double height; /**< Marker ellipsoidal height (with respect to the ellipsoid specified by Datum) */ - float undulation; /**< Geoid undulation computed from the global geoid model defined in - the document 'Technical Characteristics of the NAVSTAR GPS, NATO, June 1991' */ - float vn; /**< Velocity in the North direction */ - float ve; /**< Velocity in the East direction */ - float vu; /**< Velocity in the Up direction */ - float cog; /**< Course over ground: this is defined as the angle of the vehicle with respect - to the local level North, ranging from 0 to 360, and increasing towards east. - Set to the do-not-use value when the speed is lower than 0.1m/s. */ - double rx_clk_bias; /**< Receiver clock bias relative to system time reported in the Time System field. - To transfer the receiver time to the system time, use: tGPS/GST=trx-RxClkBias */ - float RxClkDrift; /**< Receiver clock drift relative to system time (relative frequency error) */ - uint8_t time_system; /**< Time system of which the offset is provided in this sub-block: - 0:GPStime - 1:Galileotime - 3:GLONASStime */ - uint8_t datum; /**< This field defines in which datum the coordinates are expressed: - 0: WGS84/ITRS - 19: Datum equal to that used by the DGNSS/RTK basestation - 30: ETRS89(ETRF2000 realization) - 31: NAD83(2011), North American Datum(2011) - 32: NAD83(PA11), North American Datum, Pacificplate (2011) - 33: NAD83(MA11), North American Datum, Marianas plate(2011) - 34: GDA94(2010), Geocentric Datum of Australia (2010) - 250:First user-defined datum - 251:Second user-defined datum */ - uint8_t nr_sv; /**< Total number of satellites used in the PVT computation. */ - uint8_t wa_corr_info; /**< Bit field providing information about which wide area corrections have been applied: - Bit 0: set if orbit and satellite clock correction information is used - Bit 1: set if range correction information is used - Bit 2: set if ionospheric information is used - Bit 3: set if orbit accuracy information is used(UERE/SISA) - Bit 4: set if DO229 Precision Approach mode is active - Bits 5-7: Reserved */ - uint16_t reference_id; /**< In case of DGPS or RTK operation, this field is to be interpreted as the base station identifier. - In SBAS operation, this field is to be interpreted as the PRN of the geostationary satellite - used (from 120 to 158). If multiple base stations or multiple geostationary satellites are used - the value is set to 65534.*/ - uint16_t mean_corr_age; /**< In case of DGPS or RTK, this field is the mean age of the differential corrections. - In case of SBAS operation, this field is the mean age of the 'fast corrections' - provided by the SBAS satellites */ - uint32_t signal_info; /**< Bit field indicating the type of GNSS signals having been used in the PVT computations. - If a bit i is set, the signal type having index i has been used. */ - uint8_t alert_flag; /**< Bit field indicating integrity related information */ - - // Revision 1 - uint8_t nr_bases; - uint16_t ppp_info; - // Revision 2 - uint16_t latency; - uint16_t h_accuracy; - uint16_t v_accuracy; -} sbf_payload_pvt_geodetic_t; - -typedef struct { - uint8_t mode_type: 4; /**< Bit field indicating the PVT mode type, as follows: - 0: No PVT available (the Error field indicates the cause of the absence of the PVT solution) - 1: Stand-Alone PVT - 2: Differential PVT - 3: Fixed location - 4: RTK with fixed ambiguities - 5: RTK with float ambiguities - 6: SBAS aided PVT - 7: moving-base RTK with fixed ambiguities - 8: moving-base RTK with float ambiguities - 10:Precise Point Positioning (PPP) */ - uint8_t mode_reserved: 2; /**< Reserved */ - uint8_t mode_base_fixed: 1;/**< Set if the user has entered the command setPVTMode,base,auto and the receiver - is still in the process of determining its fixed position. */ - uint8_t mode_2d: 1; /**< 2D/3D flag: set in 2D mode(height assumed constant and not computed). */ - uint8_t error; /**< PVT error code. The following values are defined: - 0: No Error - 1: Not enough measurements - 2: Not enough ephemerides available - 3: DOP too large (larger than 15) - 4: Sum of squared residuals too large - 5: No convergence - 6: Not enough measurements after outlier rejection - 7: Position output prohibited due to export laws - 8: Not enough differential corrections available - 9: Base station coordinates unavailable - 10:Ambiguities not fixed and user requested to only output RTK-fixed positions - Note: if this field has a non-zero value, all following fields are set to their Do-Not-Use value. */ - float cov_vn_vn; /**< Variance of the north-velocity estimate */ - float cov_ve_ve; /**< Variance of the east-velocity estimate */ - float cov_vu_vu; /**< Variance of the up - velocity estimate */ - float cov_dt_dt; /**< Variance of the clock drift estimate */ - float cov_vn_ve; /**< Covariance between the north - and east - velocity estimates */ - float cov_vn_vu; /**< Covariance between the north - and up - velocity estimates */ - float cov_vn_dt; /**< Covariance between the north - velocity and clock drift estimates */ - float cov_ve_vu; /**< Covariance between the east - and up - velocity estimates */ - float cov_ve_dt; /**< Covariance between the east - velocity and clock drift estimates */ - float cov_vu_dt; /**< Covariance between the up - velocity and clock drift estimates */ -} sbf_payload_vel_cov_geodetic_t; - -typedef struct { - uint8_t nr_sv; /**< Total number of satellites used in the PVT computation. */ - uint8_t reserved; - uint16_t pDOP; - uint16_t tDOP; - uint16_t hDOP; - uint16_t vDOP; - float hpl; /**< Horizontal Protection Level (see the DO229 standard). */ - float vpl; /**< Vertical Protection Level (see the DO229 standard). */ -} sbf_payload_dop_t; - -typedef struct { - uint8_t antenna; - uint8_t reserved; - uint16_t tracking_status; - uint16_t pvt_status; - uint16_t pvt_info; -} sbf_payload_channel_state_info_t; - -typedef struct { - uint8_t nr_sv; /**< The average over all antennas of the number of satellites currently included in the attitude calculations. */ - uint8_t error_aux1: 2; /**< Bits 0-1: Error code for Main-Aux1 baseline: - 0: No error - 1: Not enough measurements - 2: Reserved - 3: Reserved */ - uint8_t error_aux2: 2; /**< Bits 2-3: Error code for Main-Aux2 baseline, same definition as bit 0-1. */ - uint8_t error_reserved: 3; /**< Bits 4-6: Reserved */ -uint8_t error_not_requested: - 1; /**< Bit 7: Set when GNSS-based attitude not requested by user. In that case, the other bits are all zero. */ - - uint16_t mode; /**< Attitude mode code: - 0: No attitude - 1: Heading, pitch (roll = 0), aux antenna positions obtained with float - ambiguities - 2: Heading, pitch (roll = 0), aux antenna positions obtained with fixed - ambiguities - 3: Heading, pitch, roll, aux antenna positions obtained with float ambiguities - 4: Heading, pitch, roll, aux antenna positions obtained with fixed ambiguities */ - uint16_t reserved; /**< Reserved for future use, to be ignored by decoding software */ - - float heading; /**< Heading */ - float pitch; /**< Pitch */ - float roll; /**< Roll */ - float pitch_dot; /**< Rate of change of the pitch angle */ - float roll_dot; /**< Rate of change of the roll angle */ - float heading_dot; /**< Rate of change of the heading angle */ -} sbf_payload_att_euler; - -typedef struct { - uint8_t reserved; /**< Reserved for future use, to be ignored by decoding software */ - - uint8_t error_aux1: 2; /**< Bits 0-1: Error code for Main-Aux1 baseline: - 0: No error - 1: Not enough measurements - 2: Reserved - 3: Reserved */ - uint8_t error_aux2: 2; /**< Bits 2-3: Error code for Main-Aux2 baseline, same definition as bit 0-1. */ - uint8_t error_reserved: 3; /**< Bits 4-6: Reserved */ -uint8_t error_not_requested: - 1; /**< Bit 7: Set when GNSS-based attitude not requested by user. In that case, the other bits are all zero. */ - - float cov_headhead; /**< Variance of the heading estimate */ - float cov_pitchpitch; /**< Variance of the pitch estimate */ - float cov_rollroll; /**< Variance of the roll estimate */ - float cov_headpitch; /**< Covariance between Euler angle estimates. - Future functionality. The values are currently set to their Do-Not-Use values. */ - float cov_headroll; /**< Covariance between Euler angle estimates. - Future functionality. The values are currently set to their Do-Not-Use values. */ - float cov_pitchroll; /**< Covariance between Euler angle estimates. - Future functionality. The values are currently set to their Do-Not-Use values. */ -} sbf_payload_att_cov_euler; - -// General message and payload buffer union - -typedef struct { - uint16_t sync; /** The Sync field is a 2-byte array always set to 0x24, 0x40. The first byte of every SBF block has - hexadecimal value 24 (decimal 36, ASCII '$'). The second byte of every SBF block has hexadecimal - value 40 (decimal 64, ASCII '@'). */ - uint16_t crc16; /** The CRC field is the 16-bit CRC of all the bytes in an SBF block from and including the ID field - to the last byte of the block. The generator polynomial for this CRC is the so-called CRC-CCITT - polynomial: x 16 + x 12 + x 5 + x 0 . The CRC is computed in the forward direction using a seed of 0, no - reverse and no final XOR. */ -uint16_t msg_id: - 13; /** The ID field is a 2-byte block ID, which uniquely identifies the block type and its contents */ -uint8_t msg_revision: - 3; /** block revision number, starting from 0 at the initial block definition, and incrementing - each time backwards - compatible changes are performed to the block */ - uint16_t length; /** The Length field is a 2-byte unsigned integer containing the size of the SBF block. - It is the total number of bytes in the SBF block including the header. - It is always a multiple of 4. */ - uint32_t TOW; /** Time-Of-Week: Time-tag, expressed in whole milliseconds from - the beginning of the current Galileo/GPSweek. */ - uint16_t WNc; /** The GPS week number associated with the TOW. WNc is a continuous - weekcount (hence the "c"). It is not affected by GPS week roll overs, - which occur every 1024 weeks. By definition of the Galileo system time, - WNc is also the Galileo week number + 1024. */ - union { - sbf_payload_pvt_geodetic_t payload_pvt_geodetic; - sbf_payload_vel_cov_geodetic_t payload_vel_col_geodetic; - sbf_payload_dop_t payload_dop; - sbf_payload_att_euler payload_att_euler; - sbf_payload_att_cov_euler payload_att_cov_euler; - }; - - uint8_t padding[16]; -} sbf_buf_t; - -#pragma pack(pop) // End of SBF protocol binary message and payload definitions diff --git a/src/drivers/gnss/septentrio/sbf/decoder.cpp b/src/drivers/gnss/septentrio/sbf/decoder.cpp new file mode 100644 index 000000000000..318a5c4db968 --- /dev/null +++ b/src/drivers/gnss/septentrio/sbf/decoder.cpp @@ -0,0 +1,252 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file decoder.cpp + * + * Decoding logic for the Septentrio Binary Format (SBF). + * + * @author Thomas Frans + */ + +#include "decoder.h" + +#include +#include +#include +#include + +#include "../module.h" +#include "../util.h" +#include "drivers/gnss/septentrio/sbf/messages.h" + +namespace septentrio +{ + +namespace sbf +{ + +Decoder::State Decoder::add_byte(uint8_t byte) +{ + uint8_t *message = reinterpret_cast(&_message); + + switch (_state) { + case State::SearchingSync1: + if (byte == (uint8_t)k_sync1) { + // Sync is always same, so we don't store it. + _current_index++; + _state = State::SearchingSync2; + } + + break; + + case State::SearchingSync2: + if (byte == (uint8_t)k_sync2) { + // Sync is always same, so we don't store it. + _current_index++; + _state = State::Busy; + + } else { + reset(); + } + + break; + + case State::Busy: + message[_current_index] = byte; + _current_index++; + + if (done()) { + _state = State::Done; + } + + break; + + case State::Done: + SEP_WARN("SBF: Discarding excess byte"); + break; + } + + return _state; +} + +BlockID Decoder::id() const +{ + return _state == State::Done ? _message.header.id_number : BlockID::Invalid; +} + +int Decoder::parse(Header *header) const +{ + if (can_parse()) { + memcpy(header, &_message.header, sizeof(Header)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(DOP *message) const +{ + if (can_parse() && id() == BlockID::DOP) { + memcpy(message, _message.payload, sizeof(DOP)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(PVTGeodetic *message) const +{ + if (can_parse() && id() == BlockID::PVTGeodetic) { + memcpy(message, _message.payload, sizeof(PVTGeodetic)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(ReceiverStatus *message) const +{ + if (can_parse() && id() == BlockID::ReceiverStatus) { + memcpy(message, _message.payload, sizeof(ReceiverStatus)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(QualityInd *message) const +{ + if (can_parse() && id() == BlockID::QualityInd) { + // Safe to copy entire size of the message as it is smaller than the maximum expected SBF message size. + // It's up to the user of the parsed message to ignore the invalid fields. + memcpy(message, _message.payload, sizeof(QualityInd)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(RFStatus *message) const +{ + if (can_parse() && id() == BlockID::PVTGeodetic) { + memcpy(message, _message.payload, sizeof(RFStatus) - sizeof(RFStatus::rf_band)); + + for (uint8_t i = 0; i < math::min(message->n, k_max_rfband_blocks); i++) { + memcpy(&message->rf_band[i], &_message.payload[sizeof(RFStatus) - sizeof(RFStatus::rf_band) + i * + message->sb_length], sizeof(RFBand)); + } + + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(GALAuthStatus *message) const +{ + if (can_parse() && id() == BlockID::GALAuthStatus) { + memcpy(message, _message.payload, sizeof(GALAuthStatus)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(VelCovGeodetic *message) const +{ + if (can_parse() && id() == BlockID::VelCovGeodetic) { + memcpy(message, _message.payload, sizeof(VelCovGeodetic)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(GEOIonoDelay *message) const +{ + if (can_parse() && id() == BlockID::GEOIonoDelay) { + memcpy(message, _message.payload, sizeof(GEOIonoDelay) - sizeof(GEOIonoDelay::idc)); + + for (size_t i = 0; i < math::min(message->n, (uint8_t)4); i++) { + memcpy(&message->idc[i], &_message.payload[sizeof(GEOIonoDelay) - sizeof(GEOIonoDelay::idc) + i * + message->sb_length], sizeof(IDC)); + } + + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(AttEuler *message) const +{ + if (can_parse() && id() == BlockID::AttEuler) { + memcpy(message, _message.payload, sizeof(AttEuler)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(AttCovEuler *message) const +{ + if (can_parse() && id() == BlockID::AttCovEuler) { + memcpy(message, _message.payload, sizeof(AttCovEuler)); + return PX4_OK; + } + + return PX4_ERROR; +} + +void Decoder::reset() +{ + _current_index = 0; + _state = State::SearchingSync1; + memset(&_message, 0, sizeof(_message)); +} + +bool Decoder::done() const +{ + return (_current_index >= 14 && _current_index >= _message.header.length) || _current_index >= sizeof(_message); +} + +bool Decoder::can_parse() const +{ + return done() + && _message.header.crc == buffer_crc16(reinterpret_cast(&_message) + 4, _message.header.length - 4); +} + +} // namespace sbf + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/sbf/decoder.h b/src/drivers/gnss/septentrio/sbf/decoder.h new file mode 100644 index 000000000000..78a62ebc0f42 --- /dev/null +++ b/src/drivers/gnss/septentrio/sbf/decoder.h @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file decoder.h + * + * Decoding logic for the Septentrio Binary Format (SBF). + * + * @author Thomas Frans +*/ + +#pragma once + +#include + +#include "messages.h" + +namespace septentrio +{ + +namespace sbf +{ + +#pragma pack(push, 1) + +/// A complete SBF message with parsed header and unparsed body. +typedef struct { + Header header; + uint8_t payload[k_max_message_size]; +} message_t; + +#pragma pack(pop) + +/** + * @brief A decoder and parser for Septentrio Binary Format (SBF) data. + */ +class Decoder +{ +public: + /** + * @brief The current decoding state of the decoder. + */ + enum class State { + /// Searching for the first sync byte of an SBF message. + SearchingSync1, + + /// Searching for the second sync byte of an SBF message. + SearchingSync2, + + /// In the process of receiving an SBF message. + Busy, + + /// Done receiving an SBF message and ready to parse. + Done, + }; + + /** + * @brief Add one byte to the decoder. + * + * @param byte The byte to add. + * + * @return The decoding state after adding the byte. + */ + State add_byte(uint8_t byte); + + /** + * @brief Get the id of the current message. + * + * @return The SBF id of the current message. + */ + BlockID id() const; + + /** + * @brief Try to parse the current header. + * + * @param header The header data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(Header *header) const; + + /** + * @brief Parse a DOP SBF message. + * + * @param message The DOP data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(DOP *message) const; + + /** + * @brief Parse a PVTGeodetic SBF message. + * + * @param message The PVTGeodetic data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(PVTGeodetic *message) const; + + /** + * @brief Parse a ReceiverStatus SBF message. + * + * @param message The ReceiverStatus data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(ReceiverStatus *message) const; + + /** + * @brief Parse a QualityInd SBF message. + * + * @param message The QualityInd data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(QualityInd *message) const; + + /** + * @brief Parse an RFSTatus SBF message. + * + * @param message The RFStatus data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(RFStatus *message) const; + + /** + * @brief Parse a GALAuthStatus SBF message. + * + * @param message The GALAuthStatus data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(GALAuthStatus *message) const; + + /** + * @brief Parse a VelCovGeodetic SBF message. + * + * @param message The VelCovGeodetic data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(VelCovGeodetic *message) const; + + /** + * @brief Parse a GEOIonoDelay SBF message. + * + * @param message The GEOIonoDelay data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(GEOIonoDelay *message) const; // NOTE: This serves as an example of how to parse sub-blocks. + + /** + * @brief Parse an AttEuler SBF message. + * + * @param message The AttEuler data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(AttEuler *message) const; + + /** + * @brief Parse an AttCovEuler SBF message. + * + * @param message The AttCovEuler data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(AttCovEuler *message) const; + + /** + * @brief Reset the decoder to a clean state. + * + * If the decoder is in the process of decoding a message or contains a complete message, it will discard it and + * become ready for the next message. + */ + void reset(); +private: + /** + * @brief Check whether a full message has been received. + * + * Does not guarantee validity of the message, only that a complete message should be available. + * + * @return `true` if a message is ready, `false` if still decoding. + */ + bool done() const; + + /** + * @brief Check whether a full message has been received and is valid. + * + * @return `true` if the data can be parsed, `false` if the message is incomplete or not valid. + */ + bool can_parse() const; + + State _state{State::SearchingSync1}; + uint16_t _current_index; + message_t _message; +}; + +} // namespace sbf + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/sbf/messages.h b/src/drivers/gnss/septentrio/sbf/messages.h new file mode 100644 index 000000000000..e51a6f469b31 --- /dev/null +++ b/src/drivers/gnss/septentrio/sbf/messages.h @@ -0,0 +1,353 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file messages.h + * + * @brief Septentrio binary format (SBF) protocol message definitions. + * + * @author Thomas Frans + */ + +#pragma once + +#include +#include + +namespace septentrio +{ + +namespace sbf +{ + +constexpr char k_sync1 {'$'}; +constexpr char k_sync2 {'@'}; + +// Do-Not-Use values for fields in SBF blocks. The receiver can set certain fields to these values to signal that they are invalid. +// Most fields of a certain type will use these values (u2 representing a value often has DNU value of 65535). +// For the ones that do not use these, custom ones can be specified together with the blocks. +constexpr uint32_t k_dnu_u4_value {4294967295}; +constexpr uint32_t k_dnu_u4_bitfield {0}; +constexpr uint16_t k_dnu_u2_value {65535}; +constexpr uint16_t k_dnu_u2_bitfield {0}; +constexpr float k_dnu_f4_value {-2 * 10000000000}; +constexpr double k_dnu_f8_value {-2 * 10000000000}; + +/// Maximum size of all expected messages. +/// This needs to be bigger than the maximum size of all declared SBF blocks so that `memcpy()` can safely copy from the decoding buffer using this value into messages. +constexpr size_t k_max_message_size {300}; + +/// Maximum expected number of sub-blocks. +constexpr uint8_t k_max_quality_indicators {9}; +constexpr uint8_t k_max_rfband_blocks {4}; + +/** + * IDs of all the available SBF messages that we care about. +*/ +enum class BlockID : uint16_t { + Invalid = 0, + DOP = 4001, + PVTGeodetic = 4007, + ReceiverStatus = 4014, + QualityInd = 4082, + RFStatus = 4092, + GALAuthStatus = 4245, + VelCovGeodetic = 5908, + EndOfPVT = 5921, + GEOIonoDelay = 5933, + AttEuler = 5938, + AttCovEuler = 5939, +}; + +#pragma pack(push, 1) + +/** + * Common SBF message header. + */ +struct Header { + uint8_t sync1; + uint8_t sync2; + uint16_t crc; + BlockID id_number: 13; + uint16_t id_revision: 3; + uint16_t length; + uint32_t tow; + uint16_t wnc; +}; + +struct DOP { + uint8_t nr_sv; + uint8_t reserved; + uint16_t p_dop; + uint16_t t_dop; + uint16_t h_dop; + uint16_t v_dop; + float hpl; + float vpl; +}; + +struct PVTGeodetic { + static constexpr uint8_t k_dnu_nr_sv {255}; + enum class ModeType : uint8_t { + NoPVT = 0, + StandAlonePVT = 1, + DifferentialPVT = 2, + FixedLocation = 3, + RTKFixed = 4, + RTKFloat = 5, + PVTWithSBAS = 6, + MovingBaseRTKFixed = 7, + MovingBaseRTKFloat = 8, + PrecisePointPositioning = 10, + }; + enum class Error : uint8_t { + None = 0, + InsufficientMeasurements = 1, + InsufficientEphemerides = 2, + DOPTooLarge = 3, + SquaredResidualSumTooLarge = 4, + NoConvergence = 5, + InsufficientMeasurementsAfterOutlierRejection = 6, + ExportLawsForbidPositionOutput = 7, + InsufficientDifferentialCorrections = 8, + MissingBaseStationCoordinates = 9, + MissingRequiredRTKFixedPosition = 10, + }; + ModeType mode_type: 4; + uint8_t mode_reserved: 2; + uint8_t mode_base_fixed: 1; + uint8_t mode_2d: 1; + Error error; + double latitude; + double longitude; + double height; + float undulation; + float vn; + float ve; + float vu; + float cog; + double rx_clk_bias; + float rx_clk_drift; + uint8_t time_system; + uint8_t datum; + uint8_t nr_sv; + uint8_t wa_corr_info; + uint16_t reference_id; + uint16_t mean_corr_age; + uint32_t signal_info; + uint8_t alert_flag; + uint8_t nr_bases; + uint16_t ppp_info; + uint16_t latency; + uint16_t h_accuracy; + uint16_t v_accuracy; +}; + +struct ReceiverStatus { + uint8_t cpu_load; + uint8_t ext_error_siserror: 1; + uint8_t ext_error_diff_corr_error: 1; + uint8_t ext_error_ext_sensor_error: 1; + uint8_t ext_error_setup_error: 1; + uint8_t ext_error_reserved: 4; + uint32_t uptime; + uint32_t rx_state_reserved1: 1; + uint32_t rx_state_active_antenna: 1; + uint32_t rx_state_ext_freq: 1; + uint32_t rx_state_ext_time: 1; + uint32_t rx_state_wn_set: 1; + uint32_t rx_state_tow_set: 1; + uint32_t rx_state_fine_time: 1; + uint32_t rx_state_internal_disk_activity: 1; + uint32_t rx_state_internal_disk_full: 1; + uint32_t rx_state_internal_disk_mounted: 1; + uint32_t rx_state_int_ant: 1; + uint32_t rx_state_refout_locked: 1; + uint32_t rx_state_reserved2: 1; + uint32_t rx_state_external_disk_activity: 1; + uint32_t rx_state_external_disk_full: 1; + uint32_t rx_state_external_disk_mounted: 1; + uint32_t rx_state_pps_in_cal: 1; + uint32_t rx_state_diff_corr_in: 1; + uint32_t rx_state_internet: 1; + uint32_t rx_state_reserved3: 13; + uint32_t rx_error_reserved1: 3; + uint32_t rx_error_software: 1; + uint32_t rx_error_watchdog: 1; + uint32_t rx_error_antenna: 1; + uint32_t rx_error_congestion: 1; + uint32_t rx_error_reserved2: 1; + uint32_t rx_error_missed_event: 1; + uint32_t rx_error_cpu_overload: 1; + uint32_t rx_error_invalid_config: 1; + uint32_t rx_error_out_of_geofence: 1; + uint32_t rx_error_reserved3: 22; + uint8_t n; + uint8_t sb_length; + uint8_t cmd_count; + uint8_t temperature; +}; + +struct QualityIndicator { + enum class Type : uint8_t { + Overall = 0, + GNSSSignalsMainAntenna = 1, + GNSSSignalsAuxAntenna = 2, + RFPowerMainAntenna = 11, + RFPowerAuxAntenna = 12, + CPUHeadroom = 21, + OCXOStability = 25, + BaseStationMeasurements = 30, + RTKPostProcessing = 31, + }; + Type type: 8; + uint16_t value: 4; + uint16_t reserved: 4; +}; + +struct QualityInd { + uint8_t n; + uint8_t reserved1; + // Only support the currently available indicators for now so we don't have to allocate. + QualityIndicator indicators[k_max_quality_indicators]; +}; + +struct RFBand { + uint32_t frequency; + uint16_t bandwidth; + uint8_t info_mode: 4; + uint8_t info_reserved: 2; + uint8_t info_antenna_id: 2; +}; + +struct RFStatus { + uint8_t n; + uint8_t sb_length; + uint8_t flags_inauthentic_gnss_signals: 1; + uint8_t flags_inauthentic_navigation_message: 1; + uint8_t flags_reserved: 6; + uint8_t reserved[3]; + RFBand rf_band[k_max_rfband_blocks]; +}; + +struct GALAuthStatus { + uint16_t osnma_status_status: 3; + uint16_t osnma_status_initialization_progress: 8; + uint16_t osnma_status_trusted_time_source: 3; + uint16_t osnma_status_merkle_tree_busy: 1; + uint16_t osnma_status_reserved: 1; + float trusted_time_delta; + uint64_t gal_active_mask; + uint64_t gal_authentic_mask; + uint64_t gps_active_mask; + uint64_t gps_authentic_mask; +}; + +struct VelCovGeodetic { + uint8_t mode_type: 4; + uint8_t mode_reserved: 2; + uint8_t mode_base_fixed: 1; + uint8_t mode_2d: 1; + uint8_t error; + float cov_vn_vn; + float cov_ve_ve; + float cov_vu_vu; + float cov_dt_dt; + float cov_vn_ve; + float cov_vn_vu; + float cov_vn_dt; + float cov_ve_vu; + float cov_ve_dt; + float cov_vu_dt; +}; + +struct IDC { + uint8_t igp_mask_no; + uint8_t givei; + uint8_t reserved[2]; + float vertical_delay; +}; + +struct GEOIonoDelay { + uint8_t prn; + uint8_t bandnbr; + uint8_t iodi; + uint8_t n; + uint8_t sb_length; + uint8_t reserved; + IDC idc[4]; +}; + +struct AttEuler { + enum class Error : uint8_t { + None = 0, + InsufficientMeasurements = 1, + }; + uint8_t nr_sv; + Error error_aux1: 2; + Error error_aux2: 2; + uint8_t error_reserved: 3; + uint8_t error_not_requested: 1; + uint16_t mode; + uint16_t reserved; + float heading; + float pitch; + float roll; + float pitch_dot; + float roll_dot; + float heading_dot; +}; + +struct AttCovEuler { + enum class Error : uint8_t { + None = 0, + InsufficientMeasurements = 1, + }; + uint8_t reserved; + Error error_aux1: 2; + Error error_aux2: 2; + uint8_t error_reserved: 3; + uint8_t error_not_requested: 1; + float cov_headhead; + float cov_pitchpitch; + float cov_rollroll; + float cov_headpitch; + float cov_headroll; + float cov_pitchroll; +}; + +#pragma pack(pop) + +} // namespace sbf + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index f8794e17e35b..ad99ed6342c8 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -44,74 +44,98 @@ #include "septentrio.h" #include +#include #include #include #include -#include #include #include +#include #include #include #include #include #include +#include #include "util.h" +#include "sbf/messages.h" -using namespace time_literals; - -#ifndef GPS_READ_BUFFER_SIZE -#define GPS_READ_BUFFER_SIZE 150 ///< Buffer size for `read()` call -#endif - -// TODO: This functionality is not available on the Snapdragon yet. -#ifdef __PX4_QURT -#define NO_MKTIME -#endif - -#define SBF_CONFIG_TIMEOUT 1000 ///< Timeout for waiting on ACK in ms -#define SBF_PACKET_TIMEOUT 2 ///< If no data during this delay (in ms) assume that full update received -#define MSG_SIZE 100 ///< Size of the message to be sent to the receiver -#define TIMEOUT_5HZ 500 ///< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error -#define RATE_MEASUREMENT_PERIOD 5_s ///< Rate at which bandwith measurements are taken -// TODO: This number seems wrong. It's also not clear why an ULL is created and casted to UL (time_t). -#define GPS_EPOCH_SECS ((time_t)1234567890ULL) - -// Trace macros (disable for production builds) -#define SBF_TRACE_PARSER(...) {/*PX4_INFO(__VA_ARGS__);*/} ///< decoding progress in parse_char() -#define SBF_TRACE_RXMSG(...) {/*PX4_INFO(__VA_ARGS__);*/} ///< Rx msgs in payload_rx_done() - -// Warning macros (disable to save memory) -#define SBF_WARN(...) {PX4_WARN(__VA_ARGS__);} ///< Module debug warnings -#define SBF_INFO(...) {PX4_INFO(__VA_ARGS__);} ///< Module debug info -#define SBF_DEBUG(...) {/*PX4_INFO(__VA_ARGS__);*/} ///< Module debug tracing - -// Commands -#define SBF_FORCE_INPUT "SSSSSSSSSS\n" /**< Force input on the connected port */ - -#define SBF_CONFIG_RESET_HOT "ExeResetReceiver, soft, none\n" - -#define SBF_CONFIG_RESET_WARM "ExeResetReceiver, soft, PVTData\n" - -#define SBF_CONFIG_RESET_COLD "ExeResetReceiver, hard, SatData\n" - -#define SBF_CONFIG "setSBFOutput, Stream1, %s, PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler, msec100\n" /**< Configure the correct blocks for GPS positioning and heading */ +using namespace device; -#define SBF_CONFIG_BAUDRATE "setCOMSettings, %s, baud%lu\n" - -#define SBF_CONFIG_RESET "setSBFOutput, Stream1, %s, none, off\n" - -#define SBF_CONFIG_RECEIVER_DYNAMICS "setReceiverDynamics, %s, UAV\n" - -#define SBF_CONFIG_ATTITUDE_OFFSET "setAttitudeOffset, %.3f, %.3f\n" +namespace septentrio +{ -#define SBF_DATA_IO "setDataInOut, %s, Auto, SBF\n" +/** + * RTC drift time when time synchronization is needed (in seconds). +*/ +constexpr int k_max_allowed_clock_drift = 5; -static constexpr int SEP_SET_CLOCK_DRIFT_TIME_S{5}; ///< RTC drift time when time synchronization is needed (in seconds) +/** + * If silence from the receiver for this time (ms), assume full data received. +*/ +constexpr int k_receiver_read_timeout = 2; -px4::atomic SeptentrioGPS::_secondary_instance{nullptr}; +/** + * The maximum allowed time for reading from the receiver. + */ +constexpr int k_max_receiver_read_timeout = 50; -SeptentrioGPS::SeptentrioGPS(const char *device_path, SeptentrioInstance instance, uint32_t baud_rate) : +/** + * Minimum amount of bytes we try to read at one time from the receiver. +*/ +constexpr size_t k_min_receiver_read_bytes = 32; + +constexpr uint8_t k_max_command_size = 120; +constexpr uint16_t k_timeout_5hz = 500; +constexpr uint32_t k_read_buffer_size = 150; +constexpr time_t k_gps_epoch_secs = 1234567890ULL; // TODO: This seems wrong + +// Septentrio receiver commands +// - erst: exeResetReceiver +// - sso: setSBFOutput +// - ssu: setSatelliteUsage +// - scs: setCOMSettings +// - srd: setReceiverDynamics +// - sto: setAttitudeOffset +// - sdio: setDataInOut +// - gecm: getEchoMessage +// - sga: setGNSSAttitude +constexpr const char *k_command_force_input = "SSSSSSSSSS\n"; +constexpr const char *k_command_reset_hot = "erst,soft,none\n"; +constexpr const char *k_command_reset_warm = "erst,soft,PVTData\n"; +constexpr const char *k_command_reset_cold = "erst,hard,SatData\n"; +constexpr const char *k_command_sbf_output_pvt = + "sso,Stream%lu,%s,PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler+EndOfPVT+ReceiverStatus,%s\n"; +constexpr const char *k_command_set_sbf_output = + "sso,Stream%lu,%s,%s%s,%s\n"; +constexpr const char *k_command_clear_sbf = "sso,Stream%lu,%s,none,off\n"; +constexpr const char *k_command_set_baud_rate = + "scs,%s,baud%lu\n"; // The receiver sends the reply at the new baud rate! +constexpr const char *k_command_set_dynamics = "srd,%s,UAV\n"; +constexpr const char *k_command_set_attitude_offset = "sto,%.3f,%.3f\n"; +constexpr const char *k_command_set_data_io = "sdio,%s,Auto,%s\n"; +constexpr const char *k_command_set_satellite_usage = "ssu,%s\n"; +constexpr const char *k_command_ping = "gecm\n"; +constexpr const char *k_command_set_gnss_attitude = "sga,%s\n"; + +constexpr const char *k_gnss_attitude_source_moving_base = "MovingBase"; +constexpr const char *k_gnss_attitude_source_multi_antenna = "MultiAntenna"; + +constexpr const char *k_frequency_0_1hz = "sec10"; +constexpr const char *k_frequency_0_2hz = "sec5"; +constexpr const char *k_frequency_0_5hz = "sec2"; +constexpr const char *k_frequency_1_0hz = "sec1"; +constexpr const char *k_frequency_2_0hz = "msec500"; +constexpr const char *k_frequency_5_0hz = "msec200"; +constexpr const char *k_frequency_10_0hz = "msec100"; +constexpr const char *k_frequency_20_0hz = "msec50"; +constexpr const char *k_frequency_25_0hz = "msec40"; +constexpr const char *k_frequency_50_0hz = "msec20"; + +px4::atomic SeptentrioDriver::_secondary_instance {nullptr}; + +SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate) : Device(MODULE_NAME), _instance(instance), _baud_rate(baud_rate) @@ -120,198 +144,202 @@ SeptentrioGPS::SeptentrioGPS(const char *device_path, SeptentrioInstance instanc // Enforce null termination. _port[sizeof(_port) - 1] = '\0'; - _report_gps_pos.heading = NAN; - _report_gps_pos.heading_offset = NAN; + reset_gps_state_message(); - int32_t enable_sat_info = 0; - param_get(param_find("SEP_SAT_INFO"), &enable_sat_info); + int32_t enable_sat_info {0}; + get_parameter("SEP_SAT_INFO", &enable_sat_info); - // Create satellite info data object if requested if (enable_sat_info) { - _sat_info = new GPSSatelliteInfo(); - _p_report_sat_info = &_sat_info->_data; - memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); + _message_satellite_info = new satellite_info_s(); + } + + get_parameter("SEP_YAW_OFFS", &_heading_offset); + get_parameter("SEP_PITCH_OFFS", &_pitch_offset); + + int32_t dump_mode {0}; + get_parameter("SEP_DUMP_COMM", &dump_mode); + DumpMode mode = static_cast(dump_mode); + + if (mode != DumpMode::Disabled) { + initialize_communication_dump(mode); + } + + int32_t receiver_stream_main {k_default_main_stream}; + get_parameter("SEP_STREAM_MAIN", &receiver_stream_main); + _receiver_stream_main = receiver_stream_main; + int32_t receiver_stream_log {k_default_log_stream}; + get_parameter("SEP_STREAM_LOG", &receiver_stream_log); + _receiver_stream_log = receiver_stream_log; + + int32_t automatic_configuration {true}; + get_parameter("SEP_AUTO_CONFIG", &automatic_configuration); + _automatic_configuration = static_cast(automatic_configuration); + + get_parameter("SEP_CONST_USAGE", &_receiver_constellation_usage); + + int32_t logging_frequency {static_cast(ReceiverLogFrequency::Hz1_0)}; + get_parameter("SEP_LOG_HZ", &logging_frequency); + _receiver_logging_frequency = static_cast(logging_frequency); + int32_t logging_level {static_cast(ReceiverLogLevel::Default)}; + get_parameter("SEP_LOG_LEVEL", &logging_level); + _receiver_logging_level = static_cast(logging_level); + int32_t logging_overwrite {false}; + get_parameter("SEP_LOG_FORCE", &logging_overwrite); + _receiver_logging_overwrite = logging_overwrite; + int32_t receiver_setup {static_cast(ReceiverSetup::Default)}; + get_parameter("SEP_HARDW_SETUP", &receiver_setup); + _receiver_setup = static_cast(receiver_setup); + int32_t sbf_output_frequency {static_cast(SBFOutputFrequency::Hz5_0)}; + get_parameter("SEP_OUTP_HZ", &sbf_output_frequency); + _sbf_output_frequency = static_cast(sbf_output_frequency); + + if (_instance == Instance::Secondary && _receiver_setup == ReceiverSetup::MovingBase) { + _rtcm_decoder = new rtcm::Decoder(); } + + set_device_type(DRV_GPS_DEVTYPE_SBF); } -SeptentrioGPS::~SeptentrioGPS() +SeptentrioDriver::~SeptentrioDriver() { - SeptentrioGPS *secondary_instance = _secondary_instance.load(); + if (_instance == Instance::Main) { + if (await_second_instance_shutdown() == PX4_ERROR) { + SEP_ERR("Secondary instance shutdown timed out"); + } + } - if (_instance == SeptentrioInstance::Main && secondary_instance) { - secondary_instance->request_stop(); + if (_message_data_from_receiver) { + delete _message_data_from_receiver; + } - // Wait for exit - uint32_t i = 0; + if (_message_data_to_receiver) { + delete _message_data_to_receiver; + } - do { - px4_usleep(20000); - ++i; - } while (_secondary_instance.load() && i < 100); + if (_message_satellite_info) { + delete _message_satellite_info; } - delete _dump_from_device; - delete _dump_to_device; - delete _rtcm_parsing; - delete _sat_info; + if (_rtcm_decoder) { + delete _rtcm_decoder; + } } -int SeptentrioGPS::print_status() +int SeptentrioDriver::print_status() { - SeptentrioGPS *secondary_instance = _secondary_instance.load(); + SeptentrioDriver *secondary_instance = _secondary_instance.load(); switch (_instance) { - case SeptentrioInstance::Main: + case Instance::Main: PX4_INFO("Main GPS"); break; - case SeptentrioInstance::Secondary: + case Instance::Secondary: PX4_INFO(""); PX4_INFO("Secondary GPS"); - - default: break; } - PX4_INFO("status: %s, port: %s, baud rate: %lu", _healthy ? "OK" : "NOT OK", _port, _baud_rate); - PX4_INFO("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); - PX4_INFO("rate reading: \t\t%6i B/s", _rate_reading); + PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _baud_rate); + PX4_INFO("controller -> receiver data rate: %lu B/s", output_data_rate()); + PX4_INFO("receiver -> controller data rate: %lu B/s", input_data_rate()); + PX4_INFO("sat info: %s", (_message_satellite_info != nullptr) ? "enabled" : "disabled"); - if (_report_gps_pos.timestamp != 0) { - PX4_INFO("rate position: \t\t%6.2f Hz", (double)get_position_update_rate()); - PX4_INFO("rate velocity: \t\t%6.2f Hz", (double)get_velocity_update_rate()); + if (_message_gps_state.timestamp != 0) { - PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate); - PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection); + PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast(rtcm_injection_frequency())); - print_message(ORB_ID(sensor_gps), _report_gps_pos); + print_message(ORB_ID(sensor_gps), _message_gps_state); } - if (_instance == SeptentrioInstance::Main && secondary_instance) { + if (_instance == Instance::Main && secondary_instance) { secondary_instance->print_status(); } return 0; } -void SeptentrioGPS::run() +void SeptentrioDriver::run() { - uint64_t last_rate_measurement = hrt_absolute_time(); - unsigned last_rate_count = 0; - param_t handle = param_find("SEP_YAW_OFFS"); - float heading_offset = 0.f; - - if (handle != PARAM_INVALID) { - param_get(handle, &heading_offset); - heading_offset = matrix::wrap_pi(math::radians(heading_offset)); - } - - if (initialize_communication_dump() == PX4_ERROR) { - SBF_WARN("GPS communication logging could not be initialized"); - } - - // Set up the communication, configure the receiver and start processing data until we have to exit. while (!should_exit()) { - _uart.setPort(_port); + switch (_state) { + case State::OpeningSerialPort: { + _uart.setPort(_port); - if (!_uart.open()) { - PX4_ERR("Error opening serial device %s", _port); - continue; - } + if (_uart.open()) { + _state = State::ConfiguringDevice; - decode_init(); - - // TODO: Not sure whether this is still correct when drivers are separate modules. - set_device_type(DRV_GPS_DEVTYPE_SBF); - - if (_dump_communication_mode == SeptentrioDumpCommMode::RTCM) { - _output_mode = SeptentrioGPSOutputMode::GPSAndRTCM; - - } else { - _output_mode = SeptentrioGPSOutputMode::GPS; - } + } else { + // Failed to open port, so wait a bit before trying again. + px4_usleep(200000); + } - // If configuration is successful, start processing messages. - if (configure(heading_offset) == 0) { + break; + } - PX4_INFO("Automatic configuration finished"); + case State::ConfiguringDevice: { + if (configure() == PX4_OK) { + SEP_INFO("Automatic configuration successful"); + _state = State::ReceivingData; - // Reset report - memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - _report_gps_pos.heading = NAN; - _report_gps_pos.heading_offset = heading_offset; + } else { + // Failed to configure device, so wait a bit before trying again. + px4_usleep(200000); + } - // Read data from the receiver and publish it until an error occurs. - int helper_ret; - unsigned receive_timeout = TIMEOUT_5HZ; + break; + } - while ((helper_ret = receive(receive_timeout)) > 0 && !should_exit()) { + case State::ReceivingData: { + int receive_result {0}; - if (helper_ret & 1) { - publish(); + receive_result = receive(k_timeout_5hz); - last_rate_count++; + if (receive_result == -1) { + _state = State::ConfiguringDevice; } - if (_p_report_sat_info && (helper_ret & 2)) { + if (_message_satellite_info && (receive_result & 2)) { publish_satellite_info(); } - reset_if_scheduled(); - - // Measure update rate every 5 seconds - if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { - float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; - _rate = last_rate_count / dt; - _rate_rtcm_injection = _last_rate_rtcm_injection_count / dt; - _rate_reading = _num_bytes_read / dt; - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - _last_rate_rtcm_injection_count = 0; - _num_bytes_read = 0; - store_update_rates(); - reset_update_rates(); - } - - if (!_healthy) { - _healthy = true; - } + break; } - if (_healthy) { - _healthy = false; - _rate = 0.0f; - _rate_rtcm_injection = 0.0f; - } } - _uart.close(); + reset_if_scheduled(); + + handle_inject_data_topic(); + + if (update_monitoring_interval_ended()) { + start_update_monitoring_interval(); + } } - PX4_INFO("exiting"); } -int SeptentrioGPS::task_spawn(int argc, char *argv[]) +int SeptentrioDriver::task_spawn(int argc, char *argv[]) { - return task_spawn(argc, argv, SeptentrioInstance::Main); + return task_spawn(argc, argv, Instance::Main); } -int SeptentrioGPS::task_spawn(int argc, char *argv[], SeptentrioInstance instance) +int SeptentrioDriver::task_spawn(int argc, char *argv[], Instance instance) { px4_main_t entry_point; - static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(2040); + static constexpr int k_task_stack_size = PX4_STACK_ADJUSTED(2048); - if (instance == SeptentrioInstance::Main) { - entry_point = (px4_main_t)&run_trampoline; + if (instance == Instance::Main) { + entry_point = &run_trampoline; } else { - entry_point = (px4_main_t)&run_trampoline_secondary; + entry_point = &run_trampoline_secondary; } px4_task_t task_id = px4_task_spawn_cmd("septentrio", SCHED_DEFAULT, SCHED_PRIORITY_SLOW_DRIVER, - TASK_STACK_SIZE, + k_task_stack_size, entry_point, (char *const *)argv); @@ -322,114 +350,70 @@ int SeptentrioGPS::task_spawn(int argc, char *argv[], SeptentrioInstance instanc return -errno; } - if (instance == SeptentrioInstance::Main) { + if (instance == Instance::Main) { _task_id = task_id; } return 0; } -int SeptentrioGPS::run_trampoline_secondary(int argc, char *argv[]) +int SeptentrioDriver::run_trampoline_secondary(int argc, char *argv[]) { // Get rid of the task name (first argument) argc -= 1; argv += 1; - SeptentrioGPS *gps = instantiate(argc, argv, SeptentrioInstance::Secondary); + SeptentrioDriver *gps = instantiate(argc, argv, Instance::Secondary); if (gps) { _secondary_instance.store(gps); gps->run(); _secondary_instance.store(nullptr); delete gps; + + } else { + return -1; } return 0; } -SeptentrioGPS *SeptentrioGPS::instantiate(int argc, char *argv[]) +SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[]) { - return instantiate(argc, argv, SeptentrioInstance::Main); + return instantiate(argc, argv, Instance::Main); } -SeptentrioGPS *SeptentrioGPS::instantiate(int argc, char *argv[], SeptentrioInstance instance) +SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance instance) { - const char *device_path = nullptr; - int baud_rate = 0; - const char *device_path_secondary = nullptr; - int baud_rate_secondary = 0; - SeptentrioGPS *gps = nullptr; - bool error_flag = false; - int ch; - int myoptind = 1; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "d:e:b:g:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'b': - if (px4_get_parameter_value(myoptarg, baud_rate) != 0) { - PX4_ERR("baudrate parsing failed"); - error_flag = true; - } - break; - case 'g': - if (px4_get_parameter_value(myoptarg, baud_rate_secondary) != 0) { - PX4_ERR("baudrate parsing failed"); - error_flag = true; - } - break; - case 'd': - device_path = myoptarg; - break; + ModuleArguments arguments{}; + SeptentrioDriver *gps{nullptr}; - case 'e': - device_path_secondary = myoptarg; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { + if (parse_cli_arguments(argc, argv, arguments) == PX4_ERROR) { return nullptr; } - if (instance == SeptentrioInstance::Main) { - if (device::Serial::validatePort(device_path)) { - gps = new SeptentrioGPS(device_path, instance, baud_rate); + if (instance == Instance::Main) { + if (Serial::validatePort(arguments.device_path_main)) { + gps = new SeptentrioDriver(arguments.device_path_main, instance, arguments.baud_rate_main); } else { - PX4_ERR("invalid device (-d) %s", device_path ? device_path : ""); + PX4_ERR("invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : ""); } - if (gps && device_path_secondary) { - task_spawn(argc, argv, SeptentrioInstance::Secondary); - - // Wait until the secondary instance is running - uint32_t i = 0; - - do { - px4_usleep(2500); - } while (!_secondary_instance.load() && ++i < 400); + if (gps && arguments.device_path_secondary) { + task_spawn(argc, argv, Instance::Secondary); - if (i == 400) { - PX4_ERR("Timed out while waiting for second instance to start"); + if (await_second_instance_startup() == PX4_ERROR) { + return nullptr; } } } else { - if (device::Serial::validatePort(device_path_secondary)) { - gps = new SeptentrioGPS(device_path_secondary, instance, baud_rate_secondary); + if (Serial::validatePort(arguments.device_path_secondary)) { + gps = new SeptentrioDriver(arguments.device_path_secondary, instance, arguments.baud_rate_secondary); } else { - PX4_ERR("Invalid secondary device (-e) %s", device_path_secondary ? device_path_secondary : ""); + PX4_ERR("Invalid secondary device (-e) %s", arguments.device_path_secondary ? arguments.device_path_secondary : ""); } } @@ -438,10 +422,10 @@ SeptentrioGPS *SeptentrioGPS::instantiate(int argc, char *argv[], SeptentrioInst // Called from outside driver thread. // Return 0 on success, -1 otherwise. -int SeptentrioGPS::custom_command(int argc, char *argv[]) +int SeptentrioDriver::custom_command(int argc, char *argv[]) { bool handled = false; - SeptentrioGPS *driver_instance; + SeptentrioDriver *driver_instance; if (!is_running()) { PX4_INFO("not running"); @@ -450,31 +434,37 @@ int SeptentrioGPS::custom_command(int argc, char *argv[]) driver_instance = get_instance(); - if (argc == 2 && !strcmp(argv[0], "reset")) { + if (argc >= 1 && strcmp(argv[0], "reset") == 0) { + if (argc == 2) { + ReceiverResetType type{ReceiverResetType::None}; - if (!strcmp(argv[1], "hot")) { - handled = true; - driver_instance->schedule_reset(SeptentrioGPSResetType::Hot); + if (strcmp(argv[1], "hot") == 0) { + type = ReceiverResetType::Hot; - } else if (!strcmp(argv[1], "cold")) { - handled = true; - driver_instance->schedule_reset(SeptentrioGPSResetType::Cold); + } else if (strcmp(argv[1], "warm") == 0) { + type = ReceiverResetType::Warm; - } else if (!strcmp(argv[1], "warm")) { - handled = true; - driver_instance->schedule_reset(SeptentrioGPSResetType::Warm); - } - } + } else if (strcmp(argv[1], "cold") == 0) { + type = ReceiverResetType::Cold; + + } else { + print_usage("incorrect reset type"); + } - if (handled) { - PX4_INFO("Resetting GPS - %s", argv[1]); - return 0; + if (type != ReceiverResetType::None) { + driver_instance->schedule_reset(type); + handled = true; + } + + } else { + print_usage("incorrect usage of reset command"); + } } return (handled) ? 0 : print_usage("unknown command"); } -int SeptentrioGPS::print_usage(const char *reason) +int SeptentrioDriver::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); @@ -498,8 +488,10 @@ Initiate warm restart of GPS device )DESCR_STR"); PRINT_MODULE_USAGE_NAME("septentrio", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "Primary Septentrio receiver", true); - PRINT_MODULE_USAGE_PARAM_STRING('e', "/dev/ttyS4", "", "Secondary Septentrio receiver", true); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "Primary Septentrio receiver", false); + PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary baud rate", true); + PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "", "Secondary Septentrio receiver", true); + PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary baud rate", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver"); @@ -508,26 +500,23 @@ Initiate warm restart of GPS device return 0; } -int SeptentrioGPS::reset(SeptentrioGPSResetType type) +int SeptentrioDriver::reset(ReceiverResetType type) { bool res = false; - send_message(SBF_FORCE_INPUT); - - // The receiver can't receive commands right after forcing input. - px4_usleep(500000); + force_input(); switch (type) { - case SeptentrioGPSResetType::Hot: - res = send_message_and_wait_for_ack(SBF_CONFIG_RESET_HOT, SBF_CONFIG_TIMEOUT); + case ReceiverResetType::Hot: + res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout); break; - case SeptentrioGPSResetType::Warm: - res = send_message_and_wait_for_ack(SBF_CONFIG_RESET_WARM, SBF_CONFIG_TIMEOUT); + case ReceiverResetType::Warm: + res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout); break; - case SeptentrioGPSResetType::Cold: - res = send_message_and_wait_for_ack(SBF_CONFIG_RESET_COLD, SBF_CONFIG_TIMEOUT); + case ReceiverResetType::Cold: + res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout); break; default: @@ -541,75 +530,162 @@ int SeptentrioGPS::reset(SeptentrioGPSResetType type) } } -float SeptentrioGPS::get_position_update_rate() +int SeptentrioDriver::force_input() { - return _rate_lat_lon; + ssize_t written = write(reinterpret_cast(k_command_force_input), strlen(k_command_force_input)); + + if (written < 0) { + return PX4_ERROR; + } else { + // The receiver can't receive input right after forcing input. From testing, the duration seems to be 1 ms, so wait 10 ms to be sure. + px4_usleep(10000); + return PX4_OK; + } +} + +int SeptentrioDriver::parse_cli_arguments(int argc, char *argv[], ModuleArguments& arguments) +{ + int ch{'\0'}; + int myoptind{1}; + const char *myoptarg{nullptr}; + + while ((ch = px4_getopt(argc, argv, "d:e:b:g:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'b': + if (px4_get_parameter_value(myoptarg, arguments.baud_rate_main) != 0) { + PX4_ERR("baud rate parsing failed"); + return PX4_ERROR; + } + break; + case 'g': + if (px4_get_parameter_value(myoptarg, arguments.baud_rate_secondary) != 0) { + PX4_ERR("baud rate parsing failed"); + return PX4_ERROR; + } + break; + case 'd': + arguments.device_path_main = myoptarg; + break; + + case 'e': + arguments.device_path_secondary = myoptarg; + break; + + case '?': + return PX4_ERROR; + + default: + PX4_WARN("unrecognized flag"); + return PX4_ERROR; + break; + } + } + + return PX4_OK; } -float SeptentrioGPS::get_velocity_update_rate() +int SeptentrioDriver::await_second_instance_startup() { - return _rate_vel; + uint32_t i = 0; + + do { + px4_usleep(2500); + } while (!_secondary_instance.load() && ++i < 400); + + if (!_secondary_instance.load()) { + SEP_ERR("Timed out while waiting for second instance to start"); + return PX4_ERROR; + } + + return PX4_OK; } -void SeptentrioGPS::schedule_reset(SeptentrioGPSResetType reset_type) +int SeptentrioDriver::await_second_instance_shutdown() { - SeptentrioGPS *secondary_instance = _secondary_instance.load(); + if (_instance == Instance::Secondary) { + return PX4_OK; + } + + SeptentrioDriver* secondary_instance = _secondary_instance.load(); + + if (secondary_instance) { + secondary_instance->request_stop(); + + uint32_t i {0}; + + // Give the secondary instance 2 seconds at most to properly shut down. + while (_secondary_instance.load() && i < 100) { + px4_usleep(20000); + + ++i; + } + + return _secondary_instance.load() ? PX4_ERROR : PX4_OK; + } else { + return PX4_OK; + } +} + +void SeptentrioDriver::schedule_reset(ReceiverResetType reset_type) +{ + SeptentrioDriver *secondary_instance = _secondary_instance.load(); _scheduled_reset.store((int)reset_type); - if (_instance == SeptentrioInstance::Main && secondary_instance) { + if (_instance == Instance::Main && secondary_instance) { secondary_instance->schedule_reset(reset_type); } } -uint32_t SeptentrioGPS::detect_receiver_baud_rate(bool force_input) { - // Baud rates we expect the receiver to be running at. - const uint32_t expected_baud_rates[] = {115200, 230400, 57600, 460800, 500000, 576000, 1200, 2400, 4800, 9600, 19200, 38400, 921600, 1000000, 1500000}; +uint32_t SeptentrioDriver::detect_receiver_baud_rate(bool forced_input) { // So we can restore the port to its original state. const uint32_t original_baud_rate = _uart.getBaudrate(); + // Baud rates we expect the receiver to be running at. + uint32_t expected_baud_rates[] = {115200, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000}; + if (_baud_rate != 0) { + expected_baud_rates[0] = _baud_rate; + } + for (uint i = 0; i < sizeof(expected_baud_rates)/sizeof(expected_baud_rates[0]); i++) { - if (set_baudrate(expected_baud_rates[i]) != PX4_OK) + if (set_baudrate(expected_baud_rates[i]) != PX4_OK) { + set_baudrate(original_baud_rate); return 0; + } - SBF_INFO("`_uart` now running at %lu", _uart.getBaudrate()); - - px4_usleep(20000); - - if (force_input) { - send_message(SBF_FORCE_INPUT); - px4_usleep(20000); + if (forced_input) { + force_input(); } - if (send_message_and_wait_for_ack("getEchoMessage\n", SBF_CONFIG_TIMEOUT)) { - _uart.setBaudrate(original_baud_rate); - PX4_INFO("Detected baud rate: %lu", expected_baud_rates[i]); + if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) { + SEP_INFO("Detected baud rate: %lu", expected_baud_rates[i]); + set_baudrate(original_baud_rate); return expected_baud_rates[i]; } - - px4_usleep(20000); } - _uart.setBaudrate(original_baud_rate); + set_baudrate(original_baud_rate); return 0; } -int SeptentrioGPS::detect_serial_port(char* const port_name) { +int SeptentrioDriver::detect_serial_port(char* const port_name) { // Read buffer to get the COM port - char buf[GPS_READ_BUFFER_SIZE]; + char buf[k_read_buffer_size]; size_t buffer_offset = 0; // The offset into the string where the next data should be read to. - hrt_abstime time_started = hrt_absolute_time(); + hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout; bool response_detected = false; - // Receiver prints prompt after forcing input. - send_message(SBF_FORCE_INPUT); + // Receiver prints prompt after a message. + if (!send_message(k_command_ping)) { + return PX4_ERROR; + } do { // Read at most the amount of available bytes in the buffer after the current offset, -1 because we need '\0' at the end for a valid string. - int read_result = read(reinterpret_cast(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, SBF_CONFIG_TIMEOUT); + int read_result = read(reinterpret_cast(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout); if (read_result < 0) { - SBF_WARN("SBF read error"); + SEP_WARN("SBF read error"); return PX4_ERROR; } @@ -647,457 +723,542 @@ int SeptentrioGPS::detect_serial_port(char* const port_name) { } buffer_offset = 3; } - } while (time_started + 5 * 1000 * SBF_CONFIG_TIMEOUT > hrt_absolute_time()); + } while (timeout_time > hrt_absolute_time()); if (!response_detected) { - SBF_WARN("No valid serial port detected"); + SEP_WARN("No valid serial port detected"); return PX4_ERROR; } else { - PX4_INFO("Serial port found: %s", port_name); + SEP_INFO("Serial port found: %s", port_name); return PX4_OK; } } -int SeptentrioGPS::configure(float heading_offset) +int SeptentrioDriver::configure() { - uint32_t detected_receiver_baud_rate = 0; - char com_port[5] {}; - float pitch_offset = 0.f; - char msg[MSG_SIZE]; - - _configured = false; + char msg[k_max_command_size] {}; // Passively detect receiver baud rate. - detected_receiver_baud_rate = detect_receiver_baud_rate(true); + uint32_t detected_receiver_baud_rate = detect_receiver_baud_rate(true); if (detected_receiver_baud_rate == 0) { - SBF_INFO("CONFIG: failed baud detection"); + SEP_INFO("CONFIG: failed baud detection"); return PX4_ERROR; } // Set same baud rate on our end. if (set_baudrate(detected_receiver_baud_rate) != PX4_OK) { - SBF_INFO("CONFIG: failed local baud rate setting"); + SEP_INFO("CONFIG: failed local baud rate setting"); return PX4_ERROR; } + char com_port[5] {}; + // Passively detect receiver port. if (detect_serial_port(com_port) != PX4_OK) { - SBF_INFO("CONFIG: failed port detection"); + SEP_INFO("CONFIG: failed port detection"); return PX4_ERROR; } + // We should definitely match baud rates and detect used port, but don't do other configuration if not requested. + // This will force input on the receiver. That shouldn't be a problem as it's on our own connection. + if (!_automatic_configuration) { + return PX4_OK; + } + // If user requested specific baud rate, set it now. Otherwise keep detected baud rate. if (strstr(com_port, "COM") != nullptr && _baud_rate != 0) { - snprintf(msg, sizeof(msg), SBF_CONFIG_BAUDRATE, com_port, _baud_rate); + snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _baud_rate); if (!send_message(msg)) { - SBF_INFO("CONFIG: failed baud rate command write"); + SEP_INFO("CONFIG: baud rate command write error"); return PX4_ERROR; } - px4_usleep(20000); - - if (!send_message(msg)) { - SBF_INFO("CONFIG: failed baud rate command write"); - return PX4_ERROR; - } - - px4_usleep(20000); + // When sending a command and setting the baud rate right after, the controller could send the command at the new baud rate. + // From testing this could take some time. + px4_usleep(1000000); if (set_baudrate(_baud_rate) != PX4_OK) { - SBF_INFO("CONFIG: failed local baud rate setting"); + SEP_INFO("CONFIG: failed local baud rate setting"); return PX4_ERROR; } - if (!send_message_and_wait_for_ack("getEchoMessage\n", SBF_CONFIG_TIMEOUT)) { - SBF_INFO("CONFIG: failed baud rate setting"); + if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) { + SEP_INFO("CONFIG: failed remote baud rate setting"); return PX4_ERROR; } + } else { + _baud_rate = detected_receiver_baud_rate; } // Delete all sbf outputs on current COM port to remove clutter data - snprintf(msg, sizeof(msg), SBF_CONFIG_RESET, com_port); + snprintf(msg, sizeof(msg), k_command_clear_sbf, _receiver_stream_main, com_port); - if (!send_message_and_wait_for_ack(msg, SBF_CONFIG_TIMEOUT)) { - SBF_INFO("CONFIG: failed delete output"); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { + SEP_INFO("CONFIG: failed delete output"); return PX4_ERROR; // connection and/or baudrate detection failed } // Define/inquire the type of data that the receiver should accept/send on a given connection descriptor - snprintf(msg, sizeof(msg), SBF_DATA_IO, com_port); + snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF"); - if (!send_message_and_wait_for_ack(msg, SBF_CONFIG_TIMEOUT)) { + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { return PX4_ERROR; } - param_t handle = param_find("SEP_PITCH_OFFS"); - - if (handle != PARAM_INVALID) { - param_get(handle, &pitch_offset); - } - // Specify the offsets that the receiver applies to the computed attitude angles. - snprintf(msg, sizeof(msg), SBF_CONFIG_ATTITUDE_OFFSET, (double)(heading_offset * 180 / M_PI_F), (double)pitch_offset); + snprintf(msg, sizeof(msg), k_command_set_attitude_offset, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset); - if (!send_message_and_wait_for_ack(msg, SBF_CONFIG_TIMEOUT)) { + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { return PX4_ERROR; } - snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "high"); - if (!send_message_and_wait_for_ack(msg, SBF_CONFIG_TIMEOUT)) { + snprintf(msg, sizeof(msg), k_command_set_dynamics, "high"); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { return PX4_ERROR; } + const char *sbf_frequency {k_frequency_10_0hz}; + switch (_sbf_output_frequency) { + case SBFOutputFrequency::Hz5_0: + sbf_frequency = k_frequency_5_0hz; + break; + case SBFOutputFrequency::Hz10_0: + sbf_frequency = k_frequency_10_0hz; + break; + case SBFOutputFrequency::Hz20_0: + sbf_frequency = k_frequency_20_0hz; + break; + case SBFOutputFrequency::Hz25_0: + sbf_frequency = k_frequency_25_0hz; + break; + } + // Output a set of SBF blocks on a given connection at a regular interval. - snprintf(msg, sizeof(msg), SBF_CONFIG, com_port); - if (!send_message_and_wait_for_ack(msg, SBF_CONFIG_TIMEOUT)) { - PX4_ERR("Failed to configure SBF"); + snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { + SEP_INFO("Failed to configure SBF"); return PX4_ERROR; } - _configured = true; - - return PX4_OK; -} - -int SeptentrioGPS::parse_char(const uint8_t byte) -{ - int ret = 0; - - switch (_decode_state) { - - // Expecting Sync1 - case SBF_DECODE_SYNC1: - if (byte == SBF_SYNC1) { // Sync1 found --> expecting Sync2 - SBF_TRACE_PARSER("A"); - payload_rx_add(byte); // add a payload byte - _decode_state = SBF_DECODE_SYNC2; - - } else if (byte == RTCM3_PREAMBLE && _rtcm_parsing) { - SBF_TRACE_PARSER("RTCM"); - _decode_state = SBF_DECODE_RTCM3; - _rtcm_parsing->addByte(byte); + if (_receiver_setup == ReceiverSetup::MovingBase) { + if (_instance == Instance::Secondary) { + snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3"); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { + SEP_INFO("Failed to configure RTCM output"); + } + } else { + snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { + SEP_INFO("Failed to configure attitude source"); + } } - - break; - - // Expecting Sync2 - case SBF_DECODE_SYNC2: - if (byte == SBF_SYNC2) { // Sync2 found --> expecting CRC - SBF_TRACE_PARSER("B"); - payload_rx_add(byte); // add a payload byte - _decode_state = SBF_DECODE_PAYLOAD; - - } else { // Sync1 not followed by Sync2: reset parser - decode_init(); + } else { + snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna); + // This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement. + if (!send_message(msg)) { + SEP_INFO("Failed to configure attitude source"); + return PX4_ERROR; } + } - break; + // Internal logging on the receiver. + if (_receiver_logging_frequency != ReceiverLogFrequency::Disabled && _receiver_stream_log != _receiver_stream_main) { + const char *frequency {nullptr}; + const char *level {nullptr}; - // Expecting payload - case SBF_DECODE_PAYLOAD: SBF_TRACE_PARSER("."); + switch (_receiver_logging_frequency) { + case ReceiverLogFrequency::Hz0_1: + frequency = k_frequency_0_1hz; + break; + case ReceiverLogFrequency::Hz0_2: + frequency = k_frequency_0_2hz; + break; + case ReceiverLogFrequency::Hz0_5: + frequency = k_frequency_0_5hz; + break; + case ReceiverLogFrequency::Hz1_0: + default: + frequency = k_frequency_1_0hz; + break; + case ReceiverLogFrequency::Hz2_0: + frequency = k_frequency_2_0hz; + break; + case ReceiverLogFrequency::Hz5_0: + frequency = k_frequency_5_0hz; + break; + case ReceiverLogFrequency::Hz10_0: + frequency = k_frequency_10_0hz; + break; + case ReceiverLogFrequency::Hz20_0: + frequency = k_frequency_20_0hz; + break; + case ReceiverLogFrequency::Hz25_0: + frequency = k_frequency_25_0hz; + break; + case ReceiverLogFrequency::Hz50_0: + frequency = k_frequency_50_0hz; + break; + } - ret = payload_rx_add(byte); // add a payload byte + switch (_receiver_logging_level) { + case ReceiverLogLevel::Lite: + level = "Comment+ReceiverStatus"; + break; + case ReceiverLogLevel::Basic: + level = "Comment+ReceiverStatus+PostProcess+Event"; + break; + case ReceiverLogLevel::Default: + default: + level = "Comment+ReceiverStatus+PostProcess+Event+Support"; + break; + case ReceiverLogLevel::Full: + level = "Comment+ReceiverStatus+PostProcess+Event+Support+BBSamples"; + break; + } - if (ret < 0) { - // payload not handled, discard message - ret = 0; - decode_init(); + snprintf(msg, sizeof(msg), k_command_set_sbf_output, _receiver_stream_log, "DSK1", _receiver_logging_overwrite ? "" : "+", level, frequency); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { + SEP_ERR("Failed to configure logging"); + return PX4_ERROR; + } + } else if (_receiver_stream_log == _receiver_stream_main) { + SEP_WARN("Skipping logging setup: logging stream can't equal main stream"); + } - } else if (ret > 0) { - ret = payload_rx_done(); // finish payload processing - decode_init(); + // Set up the satellites used in PVT computation. + if (_receiver_constellation_usage != static_cast(SatelliteUsage::Default)) { + char requested_satellites[40] {}; + if (_receiver_constellation_usage & static_cast(SatelliteUsage::GPS)) { + strcat(requested_satellites, "GPS+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::GLONASS)) { + strcat(requested_satellites, "GLONASS+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::Galileo)) { + strcat(requested_satellites, "GALILEO+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::SBAS)) { + strcat(requested_satellites, "SBAS+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::BeiDou)) { + strcat(requested_satellites, "BEIDOU+"); + } + // Make sure to remove the trailing '+' if any. + requested_satellites[math::max(static_cast(strlen(requested_satellites)) - 1, 0)] = '\0'; + snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { + SEP_ERR("Failed to configure constellation usage"); + return PX4_ERROR; + } + } - } else { - // expecting more payload, stay in state SBF_DECODE_PAYLOAD - ret = 0; + return PX4_OK; +} +int SeptentrioDriver::parse_char(const uint8_t byte) +{ + int result = 0; + + switch (_active_decoder) { + case DecodingStatus::Searching: + if (_sbf_decoder.add_byte(byte) != sbf::Decoder::State::SearchingSync1) { + _active_decoder = DecodingStatus::SBF; + } else if (_rtcm_decoder && _rtcm_decoder->add_byte(byte) != rtcm::Decoder::State::SearchingPreamble) { + _active_decoder = DecodingStatus::RTCMv3; } - break; - - case SBF_DECODE_RTCM3: - if (_rtcm_parsing->addByte(byte)) { - SBF_DEBUG("got RTCM message with length %i", (int) _rtcm_parsing->messageLength()); - got_rtcm_message(_rtcm_parsing->message(), _rtcm_parsing->messageLength()); - decode_init(); + case DecodingStatus::SBF: + if (_sbf_decoder.add_byte(byte) == sbf::Decoder::State::Done) { + if (process_message() == PX4_OK) { + result = 1; + } + _sbf_decoder.reset(); + _active_decoder = DecodingStatus::Searching; } - break; - - default: + case DecodingStatus::RTCMv3: + if (_rtcm_decoder->add_byte(byte) == rtcm::Decoder::State::Done) { + if (process_message() == PX4_OK) { + result = 1; + } + _rtcm_decoder->reset(); + _active_decoder = DecodingStatus::Searching; + } break; } - return ret; + return result; } -int SeptentrioGPS::payload_rx_add(const uint8_t byte) +int SeptentrioDriver::process_message() { - int ret = 0; - uint8_t *p_buf = reinterpret_cast(&_buf); - - p_buf[_rx_payload_index++] = byte; + int result = PX4_ERROR; - if ((_rx_payload_index > 7 && _rx_payload_index >= _buf.length) || _rx_payload_index >= sizeof(_buf)) { - ret = 1; // payload received completely + switch (_active_decoder) { + case DecodingStatus::Searching: { + SEP_ERR("Can't process incomplete message!"); + result = PX4_ERROR; + break; } + case DecodingStatus::SBF: { + using namespace sbf; - return ret; -} + switch (_sbf_decoder.id()) { + case BlockID::Invalid: { + SEP_TRACE_PARSING("Tried to process invalid SBF message"); + break; + } + case BlockID::DOP: { + SEP_TRACE_PARSING("Processing DOP SBF message"); + _current_interval_messages.dop = true; -int SeptentrioGPS::payload_rx_done() -{ - int ret = 0; + DOP dop; - if (_buf.length <= 4 || - _buf.length > _rx_payload_index || - _buf.crc16 != sep_crc16(reinterpret_cast(&_buf) + 4, _buf.length - 4)) { - return 0; - } + if (_sbf_decoder.parse(&dop) == PX4_OK) { + _message_gps_state.hdop = dop.h_dop * 0.01f; + _message_gps_state.vdop = dop.v_dop * 0.01f; + result = PX4_OK; + } - // Handle message - switch (_buf.msg_id) { - case SBF_ID_PVTGeodetic: SBF_TRACE_RXMSG("Rx PVTGeodetic"); - _msg_status |= 1; + break; + } + case BlockID::PVTGeodetic: { + using ModeType = PVTGeodetic::ModeType; + using Error = PVTGeodetic::Error; - if (_buf.payload_pvt_geodetic.mode_type < 1) { - _report_gps_pos.fix_type = 1; + SEP_TRACE_PARSING("Processing PVTGeodetic SBF message"); + _current_interval_messages.pvt_geodetic = true; - } else if (_buf.payload_pvt_geodetic.mode_type == 6) { - _report_gps_pos.fix_type = 4; + Header header; + PVTGeodetic pvt_geodetic; - } else if (_buf.payload_pvt_geodetic.mode_type == 5 || _buf.payload_pvt_geodetic.mode_type == 8) { - _report_gps_pos.fix_type = 5; + if (_sbf_decoder.parse(&header) == PX4_OK && _sbf_decoder.parse(&pvt_geodetic) == PX4_OK) { + if (pvt_geodetic.mode_type < ModeType::StandAlonePVT) { + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE; - } else if (_buf.payload_pvt_geodetic.mode_type == 4 || _buf.payload_pvt_geodetic.mode_type == 7) { - _report_gps_pos.fix_type = 6; + } else if (pvt_geodetic.mode_type == ModeType::PVTWithSBAS) { + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL; - } else { - _report_gps_pos.fix_type = 3; - } + } else if (pvt_geodetic.mode_type == ModeType::RTKFloat || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFloat) { + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT; - // Check fix and error code - _report_gps_pos.vel_ned_valid = _report_gps_pos.fix_type > 1 && _buf.payload_pvt_geodetic.error == 0; + } else if (pvt_geodetic.mode_type == ModeType::RTKFixed || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFixed) { + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED; - // Check boundaries and invalidate GPS velocities - // We're not just checking for the do-not-use value (-2*10^10) but for any value beyond the specified max values - if (fabsf(_buf.payload_pvt_geodetic.vn) > 600.0f || fabsf(_buf.payload_pvt_geodetic.ve) > 600.0f || - fabsf(_buf.payload_pvt_geodetic.vu) > 600.0f) { - _report_gps_pos.vel_ned_valid = false; - } + } else { + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_3D; + } - // Check boundaries and invalidate position - // We're not just checking for the do-not-use value (-2*10^10) but for any value beyond the specified max values - if (fabs(_buf.payload_pvt_geodetic.latitude) > (double)(M_PI_F / 2.0f) || - fabs(_buf.payload_pvt_geodetic.longitude) > (double) M_PI_F || - fabs(_buf.payload_pvt_geodetic.height) > SBF_PVTGEODETIC_DNU || - fabsf(_buf.payload_pvt_geodetic.undulation) > (float) SBF_PVTGEODETIC_DNU) { - _report_gps_pos.fix_type = 0; - } + // Check fix and error code + _message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None; - if (_buf.payload_pvt_geodetic.nr_sv < 255) { // 255 = do not use value - _report_gps_pos.satellites_used = _buf.payload_pvt_geodetic.nr_sv; + // Check boundaries and invalidate GPS velocities + if (pvt_geodetic.vn <= k_dnu_f4_value || pvt_geodetic.ve <= k_dnu_f4_value || pvt_geodetic.vu <= k_dnu_f4_value) { + _message_gps_state.vel_ned_valid = false; + } - if (_p_report_sat_info) { - // Only fill in the satellite count for now (we could use the ChannelStatus message for the - // other data, but it's really large: >800B) - _p_report_sat_info->timestamp = hrt_absolute_time(); - _p_report_sat_info->count = _report_gps_pos.satellites_used; - ret = 2; - } + // Check boundaries and invalidate position + // We're not just checking for the do-not-use value but for any value beyond the specified max values + if (pvt_geodetic.latitude <= k_dnu_f8_value || + pvt_geodetic.longitude <= k_dnu_f8_value || + pvt_geodetic.height <= k_dnu_f8_value || + pvt_geodetic.undulation <= k_dnu_f4_value) { + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE; + } - } else { - _report_gps_pos.satellites_used = 0; - } + if (pvt_geodetic.nr_sv != PVTGeodetic::k_dnu_nr_sv) { + _message_gps_state.satellites_used = pvt_geodetic.nr_sv; - _report_gps_pos.latitude_deg = _buf.payload_pvt_geodetic.latitude * M_RAD_TO_DEG; - _report_gps_pos.longitude_deg = _buf.payload_pvt_geodetic.longitude * M_RAD_TO_DEG; - _report_gps_pos.altitude_ellipsoid_m = _buf.payload_pvt_geodetic.height; - _report_gps_pos.altitude_msl_m = _buf.payload_pvt_geodetic.height - static_cast - (_buf.payload_pvt_geodetic.undulation); + if (_message_satellite_info) { + // Only fill in the satellite count for now (we could use the ChannelStatus message for the + // other data, but it's really large: >800B) + _message_satellite_info->timestamp = hrt_absolute_time(); + _message_satellite_info->count = _message_gps_state.satellites_used; + } - /* H and V accuracy are reported in 2DRMS, but based off the uBlox reporting we expect RMS. - * Devide by 100 from cm to m and in addition divide by 2 to get RMS. */ - _report_gps_pos.eph = static_cast(_buf.payload_pvt_geodetic.h_accuracy) / 200.0f; - _report_gps_pos.epv = static_cast(_buf.payload_pvt_geodetic.v_accuracy) / 200.0f; + } else { + _message_gps_state.satellites_used = 0; + } - _report_gps_pos.vel_n_m_s = static_cast(_buf.payload_pvt_geodetic.vn); - _report_gps_pos.vel_e_m_s = static_cast(_buf.payload_pvt_geodetic.ve); - _report_gps_pos.vel_d_m_s = -1.0f * static_cast(_buf.payload_pvt_geodetic.vu); - _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + - _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s); + _message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG; + _message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG; + _message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast(pvt_geodetic.undulation); + _message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height; + + /* H and V accuracy are reported in 2DRMS, but based off the u-blox reporting we expect RMS. + * Divide by 100 from cm to m and in addition divide by 2 to get RMS. */ + _message_gps_state.eph = static_cast(pvt_geodetic.h_accuracy) / 200.0f; + _message_gps_state.epv = static_cast(pvt_geodetic.v_accuracy) / 200.0f; + + _message_gps_state.vel_n_m_s = pvt_geodetic.vn; + _message_gps_state.vel_e_m_s = pvt_geodetic.ve; + _message_gps_state.vel_d_m_s = -1.0f * pvt_geodetic.vu; + _message_gps_state.vel_m_s = sqrtf(_message_gps_state.vel_n_m_s * _message_gps_state.vel_n_m_s + + _message_gps_state.vel_e_m_s * _message_gps_state.vel_e_m_s); + + _message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F; + _message_gps_state.c_variance_rad = M_DEG_TO_RAD_F; + + _message_gps_state.time_utc_usec = 0; +#ifndef __PX4_QURT // NOTE: Functionality isn't available on Snapdragon yet. + struct tm timeinfo; + time_t epoch; + + // Convert to unix timestamp + memset(&timeinfo, 0, sizeof(timeinfo)); + + timeinfo.tm_year = 1980 - 1900; + timeinfo.tm_mon = 0; + timeinfo.tm_mday = 6 + header.wnc * 7; + timeinfo.tm_hour = 0; + timeinfo.tm_min = 0; + timeinfo.tm_sec = header.tow / 1000; + + epoch = mktime(&timeinfo); + + if (epoch > k_gps_epoch_secs) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + memset(&ts, 0, sizeof(ts)); + ts.tv_sec = epoch; + ts.tv_nsec = (header.tow % 1000) * 1000 * 1000; + set_clock(ts); + + _message_gps_state.time_utc_usec = static_cast(epoch) * 1000000ULL; + _message_gps_state.time_utc_usec += (header.tow % 1000) * 1000; + } - _report_gps_pos.cog_rad = static_cast(_buf.payload_pvt_geodetic.cog) * M_DEG_TO_RAD_F; - _report_gps_pos.c_variance_rad = 1.0f * M_DEG_TO_RAD_F; +#endif + _message_gps_state.timestamp = hrt_absolute_time(); + result = PX4_OK; + } - // _buf.payload_pvt_geodetic.cog is set to -2*10^10 for velocities below 0.1m/s - if (_buf.payload_pvt_geodetic.cog > 360.0f) { - _buf.payload_pvt_geodetic.cog = NAN; + break; } - _report_gps_pos.time_utc_usec = 0; -#ifndef NO_MKTIME - struct tm timeinfo; - time_t epoch; - - // Convert to unix timestamp - memset(&timeinfo, 0, sizeof(timeinfo)); - - timeinfo.tm_year = 1980 - 1900; - timeinfo.tm_mon = 0; - timeinfo.tm_mday = 6 + _buf.WNc * 7; - timeinfo.tm_hour = 0; - timeinfo.tm_min = 0; - timeinfo.tm_sec = _buf.TOW / 1000; + case BlockID::ReceiverStatus: { + SEP_TRACE_PARSING("Processing ReceiverStatus SBF message"); - epoch = mktime(&timeinfo); + ReceiverStatus receiver_status; - if (epoch > GPS_EPOCH_SECS) { - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts; - memset(&ts, 0, sizeof(ts)); - ts.tv_sec = epoch; - ts.tv_nsec = (_buf.TOW % 1000) * 1000 * 1000; - set_clock(ts); + if (_sbf_decoder.parse(&receiver_status) == PX4_OK) { + _message_gps_state.rtcm_msg_used = receiver_status.rx_state_diff_corr_in ? sensor_gps_s::RTCM_MSG_USED_USED : sensor_gps_s::RTCM_MSG_USED_NOT_USED; + } - _report_gps_pos.time_utc_usec = static_cast(epoch) * 1000000ULL; - _report_gps_pos.time_utc_usec += (_buf.TOW % 1000) * 1000; + break; } - -#endif - _report_gps_pos.timestamp = hrt_absolute_time(); - _last_timestamp_time = _report_gps_pos.timestamp; - _rate_count_vel++; - _rate_count_lat_lon++; - // NOTE: Isn't this just `ret |= (_msg_status == 7)`? - ret |= (_msg_status == 7) ? 1 : 0; - break; - - case SBF_ID_VelCovGeodetic: SBF_TRACE_RXMSG("Rx VelCovGeodetic"); - _msg_status |= 2; - _report_gps_pos.s_variance_m_s = _buf.payload_vel_col_geodetic.cov_ve_ve; - - if (_report_gps_pos.s_variance_m_s < _buf.payload_vel_col_geodetic.cov_vn_vn) { - _report_gps_pos.s_variance_m_s = _buf.payload_vel_col_geodetic.cov_vn_vn; + case BlockID::QualityInd: { + SEP_TRACE_PARSING("Processing QualityInd SBF message"); + break; + } + case BlockID::RFStatus: { + SEP_TRACE_PARSING("Processing RFStatus SBF message"); + break; + } + case BlockID::GALAuthStatus: { + SEP_TRACE_PARSING("Processing GALAuthStatus SBF message"); + break; } + case BlockID::EndOfPVT: { + SEP_TRACE_PARSING("Processing EndOfPVT SBF message"); - if (_report_gps_pos.s_variance_m_s < _buf.payload_vel_col_geodetic.cov_vu_vu) { - _report_gps_pos.s_variance_m_s = _buf.payload_vel_col_geodetic.cov_vu_vu; + // EndOfPVT guarantees that all PVT blocks for this epoch have been sent, so it's safe to assume the uORB message contains all required data. + publish(); + break; } + case BlockID::VelCovGeodetic: { + SEP_TRACE_PARSING("Processing VelCovGeodetic SBF message"); + _current_interval_messages.vel_cov_geodetic = true; - //SBF_DEBUG("VelCovGeodetic handled"); - break; + VelCovGeodetic vel_cov_geodetic; - case SBF_ID_DOP: SBF_TRACE_RXMSG("Rx DOP"); - _msg_status |= 4; - _report_gps_pos.hdop = _buf.payload_dop.hDOP * 0.01f; - _report_gps_pos.vdop = _buf.payload_dop.vDOP * 0.01f; - //SBF_DEBUG("DOP handled"); - break; + if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) { + _message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_ve_ve; - case SBF_ID_AttEuler: SBF_TRACE_RXMSG("Rx AttEuler"); + if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vn_vn) { + _message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vn_vn; + } + + if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vu_vu) { + _message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vu_vu; + } + } - if (!_buf.payload_att_euler.error_not_requested) { + break; + } + case BlockID::GEOIonoDelay: { + SEP_TRACE_PARSING("Processing GEOIonoDelay SBF message"); + break; + } + case BlockID::AttEuler: { + using Error = AttEuler::Error; - int error_aux1 = _buf.payload_att_euler.error_aux1; - int error_aux2 = _buf.payload_att_euler.error_aux2; + SEP_TRACE_PARSING("Processing AttEuler SBF message"); + _current_interval_messages.att_euler = true; - // SBF_DEBUG("Mode: %u", _buf.payload_att_euler.mode) - if (error_aux1 == 0 && error_aux2 == 0) { - float heading = _buf.payload_att_euler.heading; - heading *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi] + AttEuler att_euler; + if (_sbf_decoder.parse(&att_euler) && + !att_euler.error_not_requested && + att_euler.error_aux1 == Error::None && + att_euler.error_aux2 == Error::None) { + float heading = att_euler.heading * M_PI_F / 180.0f; // Range of degrees to range of radians in [0, 2PI]. + // Ensure range is in [-PI, PI]. if (heading > M_PI_F) { - heading -= 2.f * M_PI_F; // final range is [-pi, pi] + heading -= 2.f * M_PI_F; } - _report_gps_pos.heading = heading; - // SBF_DEBUG("Heading: %.3f rad", (double) _report_gps_pos.heading) - //SBF_DEBUG("AttEuler handled"); - - } else if (error_aux1 != 0) { - //SBF_DEBUG("Error code for Main-Aux1 baseline: Not enough measurements"); - } else if (error_aux2 != 0) { - //SBF_DEBUG("Error code for Main-Aux2 baseline: Not enough measurements"); + _message_gps_state.heading = heading; } - } else { - //SBF_DEBUG("AttEuler: attitude not requested by user"); - } - - - break; - case SBF_ID_AttCovEuler: SBF_TRACE_RXMSG("Rx AttCovEuler"); + break; + } + case BlockID::AttCovEuler: { + using Error = AttCovEuler::Error; - if (!_buf.payload_att_cov_euler.error_not_requested) { - int error_aux1 = _buf.payload_att_cov_euler.error_aux1; - int error_aux2 = _buf.payload_att_cov_euler.error_aux2; + SEP_TRACE_PARSING("Processing AttCovEuler SBF message"); + _current_interval_messages.att_cov_euler = true; - if (error_aux1 == 0 && error_aux2 == 0) { - float heading_acc = _buf.payload_att_cov_euler.cov_headhead; - heading_acc *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi] - _report_gps_pos.heading_accuracy = heading_acc; - // SBF_DEBUG("Heading-Accuracy: %.3f rad", (double) _report_gps_pos.heading_accuracy) - //SBF_DEBUG("AttCovEuler handled"); + AttCovEuler att_cov_euler; - } else if (error_aux1 != 0) { - //SBF_DEBUG("Error code for Main-Aux1 baseline: %u: Not enough measurements", error_aux1); - } else if (error_aux2 != 0) { - //SBF_DEBUG("Error code for Main-Aux2 baseline: %u: Not enough measurements", error_aux2); + if (_sbf_decoder.parse(&att_cov_euler) == PX4_OK && + !att_cov_euler.error_not_requested && + att_cov_euler.error_aux1 == Error::None && + att_cov_euler.error_aux2 == Error::None) { + _message_gps_state.heading_accuracy = att_cov_euler.cov_headhead * M_PI_F / 180.0f; // Convert range of degrees to range of radians in [0, 2PI] } - } else { - //SBF_DEBUG("AttCovEuler: attitude not requested by user"); - } - break; + break; + } + } - default: break; } - - if (ret > 0) { - // NOTE: Isn't this always 0? - _report_gps_pos.timestamp_time_relative = static_cast(_last_timestamp_time - _report_gps_pos.timestamp); + case DecodingStatus::RTCMv3: { + SEP_TRACE_PARSING("Processing RTCMv3 message"); + publish_rtcm_corrections(_rtcm_decoder->message(), _rtcm_decoder->received_bytes()); + break; } - - if (ret == 1) { - _msg_status &= ~1; } - return ret; + return result; } -void SeptentrioGPS::decode_init() +bool SeptentrioDriver::send_message(const char *msg) { - _decode_state = SBF_DECODE_SYNC1; - _rx_payload_index = 0; - - if (_output_mode == SeptentrioGPSOutputMode::GPSAndRTCM) { - if (!_rtcm_parsing) { - _rtcm_parsing = new RTCMParsing(); - } - - if (_rtcm_parsing) { - _rtcm_parsing->reset(); - } - } -} - -bool SeptentrioGPS::send_message(const char *msg) -{ - SBF_DEBUG("Send MSG: %s", msg); + PX4_DEBUG("Send MSG: %s", msg); int length = strlen(msg); return (write(reinterpret_cast(msg), length) == length); } -bool SeptentrioGPS::send_message_and_wait_for_ack(const char *msg, const int timeout) +bool SeptentrioDriver::send_message_and_wait_for_ack(const char *msg, const int timeout) { if (!send_message(msg)) { return false; @@ -1105,80 +1266,61 @@ bool SeptentrioGPS::send_message_and_wait_for_ack(const char *msg, const int tim // Wait for acknowledge // For all valid set -, get - and exe -commands, the first line of the reply is an exact copy - // of the command as entered by the user, preceded with "$R:" - char buf[GPS_READ_BUFFER_SIZE]; - size_t buffer_offset = 0; // The offset into the string where the next data should be read to. - hrt_abstime time_started = hrt_absolute_time(); + // of the command as entered by the user, preceded with "$R: " + char buf[k_read_buffer_size]; + char expected_response[k_max_command_size+4]; + snprintf(expected_response, sizeof(expected_response), "$R: %s", msg); + uint16_t response_check_character = 0; + // Length of the message without the newline but including the preceding response part "$R: " + size_t response_len = strlen(msg) + 3; + hrt_abstime timeout_time = hrt_absolute_time() + 1000 * timeout; do { - // Read at most the amount of available bytes in the buffer after the current offset, -1 because we need '\0' at the end for a valid string. - int read_result = read(reinterpret_cast(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, 50); + int read_result = read(reinterpret_cast(buf), sizeof(buf), 50); if (read_result < 0) { - SBF_WARN("SBF read error"); + SEP_WARN("SBF read error"); return false; } - // Sanitize the data so it doesn't contain any `0` values. - for (size_t i = buffer_offset; i < buffer_offset + read_result; i++) { - if (buf[i] == 0) { - buf[i] = 1; - } - } - - buffer_offset += read_result; - - // Make sure the current buffer is a valid string. - buf[buffer_offset] = '\0'; - - if (strstr(buf, "$R: ") != nullptr) { - SBF_DEBUG("Response: received"); - return true; - } - - if (buffer_offset + 1 >= sizeof(buf)) { - // Copy the last 3 bytes such that a half response isn't lost. - for (int i = 0; i < 4; i++) { - buf[i] = buf[sizeof(buf) - 4 + i]; + for (int i = 0; i < read_result; i++) { + if (response_check_character == response_len) { + // We encountered the complete response + return true; + } else if (expected_response[response_check_character] == buf[i]) { + ++response_check_character; + } else { + response_check_character = 0; } - buffer_offset = 3; } - } while (time_started + 1000 * timeout > hrt_absolute_time()); + } while (timeout_time > hrt_absolute_time()); - SBF_DEBUG("Response: timeout"); + PX4_DEBUG("Response: timeout"); return false; } -int SeptentrioGPS::receive(unsigned timeout) +int SeptentrioDriver::receive(unsigned timeout) { int ret = 0; int handled = 0; - uint8_t buf[GPS_READ_BUFFER_SIZE]; - - if (!_configured) { - px4_usleep(timeout * 1000); - return 0; - } + uint8_t buf[k_read_buffer_size]; // timeout additional to poll hrt_abstime time_started = hrt_absolute_time(); while (true) { - // Wait for only SBF_PACKET_TIMEOUT if something already received. - ret = read(buf, sizeof(buf), handled ? SBF_PACKET_TIMEOUT : timeout); + // Wait for only `k_receiver_read_timeout` if something already received. + ret = read(buf, sizeof(buf), handled ? k_receiver_read_timeout : timeout); if (ret < 0) { // Something went wrong when polling or reading. - SBF_WARN("ubx poll_or_read err"); + SEP_WARN("poll_or_read err"); return -1; } else { - SBF_DEBUG("Read %d bytes (receive)", ret); - // Pass received bytes to the packet decoder. for (int i = 0; i < ret; i++) { handled |= parse_char(buf[i]); - SBF_DEBUG("parsed %d: 0x%x", i, buf[i]); } } @@ -1188,101 +1330,104 @@ int SeptentrioGPS::receive(unsigned timeout) // abort after timeout if no useful packets received if (time_started + timeout * 1000 < hrt_absolute_time()) { - SBF_DEBUG("timed out, returning"); + PX4_DEBUG("timed out, returning"); return -1; } } } -int SeptentrioGPS::read(uint8_t *buf, size_t buf_length, int timeout) +int SeptentrioDriver::read(uint8_t *buf, size_t buf_length, int timeout) { int num_read = poll_or_read(buf, buf_length, timeout); if (num_read > 0) { - dump_gps_data(buf, (size_t)num_read, SeptentrioDumpCommMode::Full, false); + _current_interval_bytes_read += num_read; + if (should_dump_incoming()) { + dump_gps_data(buf, num_read, DataDirection::FromReceiver); + } } return num_read; } -int SeptentrioGPS::poll_or_read(uint8_t *buf, size_t buf_length, int timeout) +int SeptentrioDriver::poll_or_read(uint8_t *buf, size_t buf_length, int timeout) { - const size_t character_count = 32; // minimum bytes that we want to read - const int max_timeout = 50; - int timeout_adjusted = math::min(max_timeout, timeout); + int read_timeout = math::min(k_max_receiver_read_timeout, timeout); - handle_inject_data_topic(); - - return _uart.readAtLeast(buf, buf_length, math::min(character_count, buf_length), timeout_adjusted); + return _uart.readAtLeast(buf, buf_length, math::min(k_min_receiver_read_bytes, buf_length), read_timeout); } -int SeptentrioGPS::write(const uint8_t* buf, size_t buf_length) +int SeptentrioDriver::write(const uint8_t* buf, size_t buf_length) { - dump_gps_data(buf, buf_length, SeptentrioDumpCommMode::Full, true); + ssize_t write_result = _uart.write(buf, buf_length); - return _uart.write(buf, buf_length); + if (write_result >= 0) { + _current_interval_bytes_written += write_result; + if (should_dump_outgoing()) { + dump_gps_data(buf, write_result, DataDirection::ToReceiver); + } + } + + return write_result; } -int SeptentrioGPS::initialize_communication_dump() +int SeptentrioDriver::initialize_communication_dump(DumpMode mode) { - param_t handle = param_find("SEP_DUMP_COMM"); - int32_t param_dump_comm; + if (mode == DumpMode::FromReceiver || mode == DumpMode::Both) { + _message_data_from_receiver = new gps_dump_s(); - if (handle == PARAM_INVALID || param_get(handle, ¶m_dump_comm) != 0) { - return PX4_ERROR; - } + if (!_message_data_from_receiver) { + SEP_ERR("Failed to allocate incoming dump buffer"); + return PX4_ERROR; + } - // Check whether dumping is disabled. - if (param_dump_comm < 1 || param_dump_comm > 2) { - return PX4_ERROR; + memset(_message_data_from_receiver, 0, sizeof(*_message_data_from_receiver)); } + if (mode == DumpMode::ToReceiver || mode == DumpMode::Both) { + _message_data_to_receiver = new gps_dump_s(); - _dump_from_device = new gps_dump_s(); - _dump_to_device = new gps_dump_s(); + if (!_message_data_to_receiver) { + SEP_ERR("failed to allocated dump data"); + return PX4_ERROR; + } - if (!_dump_from_device || !_dump_to_device) { - PX4_ERR("failed to allocated dump data"); - return PX4_ERROR; + memset(_message_data_to_receiver, 0, sizeof(*_message_data_to_receiver)); } - memset(_dump_to_device, 0, sizeof(gps_dump_s)); - memset(_dump_from_device, 0, sizeof(gps_dump_s)); - - // Make sure to use a large enough queue size, so that we don't lose - // messages. You may also want to increase the logger rate for that. - _dump_communication_pub.advertise(); - - _dump_communication_mode = (SeptentrioDumpCommMode)param_dump_comm; + if (mode != DumpMode::Disabled) { + _gps_dump_pub.advertise(); + } return PX4_OK; } -void SeptentrioGPS::reset_if_scheduled() +void SeptentrioDriver::reset_if_scheduled() { - SeptentrioGPSResetType reset_type = (SeptentrioGPSResetType)_scheduled_reset.load(); + ReceiverResetType reset_type = (ReceiverResetType)_scheduled_reset.load(); - if (reset_type != SeptentrioGPSResetType::None) { - _scheduled_reset.store((int)SeptentrioGPSResetType::None); + if (reset_type != ReceiverResetType::None) { + _scheduled_reset.store((int)ReceiverResetType::None); int res = reset(reset_type); if (res == PX4_OK) { - PX4_INFO("Reset succeeded."); + SEP_INFO("Reset succeeded."); } else { - PX4_INFO("Reset failed."); + SEP_INFO("Reset failed."); } } } -int SeptentrioGPS::set_baudrate(uint32_t baud) +int SeptentrioDriver::set_baudrate(uint32_t baud) { if (_uart.setBaudrate(baud)) { + SEP_INFO("baud controller: %lu", baud); return PX4_OK; } else { return PX4_ERROR; } } -void SeptentrioGPS::handle_inject_data_topic() +void SeptentrioDriver::handle_inject_data_topic() { // We don't want to call copy again further down if we have already done a copy in the selection process. bool already_copied = false; @@ -1291,11 +1436,11 @@ void SeptentrioGPS::handle_inject_data_topic() // If there has not been a valid RTCM message for a while, try to switch to a different RTCM link if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) { - for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { - const bool exists = _orb_inject_data_sub[instance].advertised(); + for (int instance = 0; instance < _gps_inject_data_sub.size(); instance++) { + const bool exists = _gps_inject_data_sub[instance].advertised(); if (exists) { - if (_orb_inject_data_sub[instance].copy(&msg)) { + if (_gps_inject_data_sub[instance].copy(&msg)) { if ((hrt_absolute_time() - msg.timestamp) < 5_s) { // Remember that we already did a copy on this instance. already_copied = true; @@ -1310,7 +1455,7 @@ void SeptentrioGPS::handle_inject_data_topic() bool updated = already_copied; // Limit maximum number of GPS injections to 8 since usually - // GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo). + // GPS injections should consist of 1-4 packets (GPS, GLONASS, BeiDou, Galileo). // Looking at 8 packets thus guarantees, that at least a full injection // data set is evaluated. // Moving Base requires a higher rate, so we allow up to 8 packets. @@ -1321,72 +1466,64 @@ void SeptentrioGPS::handle_inject_data_topic() if (updated) { num_injections++; - // Prevent injection of data from self - if (msg.device_id != get_device_id()) { + // Prevent injection of data from self or from ground if moving base and this is rover. + if ((_instance == Instance::Secondary && msg.device_id != get_device_id()) || (_instance == Instance::Main && msg.device_id == get_device_id()) || _receiver_setup != ReceiverSetup::MovingBase) { /* Write the message to the gps device. Note that the message could be fragmented. * But as we don't write anywhere else to the device during operation, we don't * need to assemble the message first. */ - inject_data(msg.data, msg.len); + write(msg.data, msg.len); - ++_last_rate_rtcm_injection_count; + ++_current_interval_rtcm_injections; _last_rtcm_injection_time = hrt_absolute_time(); } } - updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg); + updated = _gps_inject_data_sub[_selected_rtcm_instance].update(&msg); } while (updated && num_injections < max_num_injections); } -bool SeptentrioGPS::inject_data(uint8_t *data, size_t len) -{ - dump_gps_data(data, len, SeptentrioDumpCommMode::Full, true); - - size_t written = _uart.write(data, len); - return written == len; -} - -void SeptentrioGPS::publish() +void SeptentrioDriver::publish() { - _report_gps_pos.device_id = get_device_id(); - _report_gps_pos.selected_rtcm_instance = _selected_rtcm_instance; - _report_gps_pos.rtcm_injection_rate = _rate_rtcm_injection; + _message_gps_state.device_id = get_device_id(); + _message_gps_state.selected_rtcm_instance = _selected_rtcm_instance; + _message_gps_state.rtcm_injection_rate = rtcm_injection_frequency(); - _report_gps_pos_pub.publish(_report_gps_pos); + _sensor_gps_pub.publish(_message_gps_state); // Heading/yaw data can be updated at a lower rate than the other navigation data. // The uORB message definition requires this data to be set to a NAN if no new valid data is available. - _report_gps_pos.heading = NAN; + _message_gps_state.heading = NAN; - if (_report_gps_pos.spoofing_state != _spoofing_state) { + if (_message_gps_state.spoofing_state != _spoofing_state) { - if (_report_gps_pos.spoofing_state > sensor_gps_s::SPOOFING_STATE_NONE) { - PX4_WARN("GPS spoofing detected! (state: %d)", _report_gps_pos.spoofing_state); + if (_message_gps_state.spoofing_state > sensor_gps_s::SPOOFING_STATE_NONE) { + SEP_WARN("GPS spoofing detected! (state: %d)", _message_gps_state.spoofing_state); } - _spoofing_state = _report_gps_pos.spoofing_state; + _spoofing_state = _message_gps_state.spoofing_state; } - if (_report_gps_pos.jamming_state != _jamming_state) { + if (_message_gps_state.jamming_state != _jamming_state) { - if (_report_gps_pos.jamming_state > sensor_gps_s::JAMMING_STATE_WARNING) { - PX4_WARN("GPS jamming detected! (state: %d) (indicator: %d)", _report_gps_pos.jamming_state, - (uint8_t)_report_gps_pos.jamming_indicator); + if (_message_gps_state.jamming_state > sensor_gps_s::JAMMING_STATE_WARNING) { + SEP_WARN("GPS jamming detected! (state: %d) (indicator: %d)", _message_gps_state.jamming_state, + (uint8_t)_message_gps_state.jamming_indicator); } - _jamming_state = _report_gps_pos.jamming_state; + _jamming_state = _message_gps_state.jamming_state; } } -void SeptentrioGPS::publish_satellite_info() +void SeptentrioDriver::publish_satellite_info() { - if (_p_report_sat_info != nullptr) { - _report_sat_info_pub.publish(*_p_report_sat_info); + if (_message_satellite_info) { + _satellite_info_pub.publish(*_message_satellite_info); } } -void SeptentrioGPS::publish_rtcm_corrections(uint8_t *data, size_t len) +void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len) { gps_inject_data_s gps_inject_data{}; @@ -1420,15 +1557,10 @@ void SeptentrioGPS::publish_rtcm_corrections(uint8_t *data, size_t len) } } -void SeptentrioGPS::dump_gps_data(const uint8_t *data, size_t len, SeptentrioDumpCommMode mode, bool msg_to_gps_device) +void SeptentrioDriver::dump_gps_data(const uint8_t *data, size_t len, DataDirection data_direction) { - gps_dump_s *dump_data = msg_to_gps_device ? _dump_to_device : _dump_from_device; - - if (_dump_communication_mode != mode || !dump_data) { - return; - } - - dump_data->instance = 0; + gps_dump_s *dump_data = data_direction == DataDirection::FromReceiver ? _message_data_from_receiver : _message_data_to_receiver; + dump_data->instance = _instance == Instance::Main ? 0 : 1; while (len > 0) { size_t write_len = len; @@ -1443,50 +1575,128 @@ void SeptentrioGPS::dump_gps_data(const uint8_t *data, size_t len, SeptentrioDum len -= write_len; if (dump_data->len >= sizeof(dump_data->data)) { - if (msg_to_gps_device) { + if (data_direction == DataDirection::ToReceiver) { dump_data->len |= 1 << 7; } dump_data->timestamp = hrt_absolute_time(); - _dump_communication_pub.publish(*dump_data); + _gps_dump_pub.publish(*dump_data); dump_data->len = 0; } } } -void SeptentrioGPS::got_rtcm_message(uint8_t *data, size_t len) +bool SeptentrioDriver::should_dump_incoming() const +{ + return _message_data_from_receiver != 0; +} + +bool SeptentrioDriver::should_dump_outgoing() const { - publish_rtcm_corrections(data, len); - dump_gps_data(data, len, SeptentrioDumpCommMode::RTCM, false); + return _message_data_to_receiver != 0; } -void SeptentrioGPS::store_update_rates() +void SeptentrioDriver::start_update_monitoring_interval() { - _rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); - _rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); + PX4_DEBUG("Update monitoring interval started"); + _last_interval_rtcm_injections = _current_interval_rtcm_injections; + _last_interval_bytes_written = _current_interval_bytes_written; + _last_interval_bytes_read = _current_interval_bytes_read; + _last_interval_messages = _current_interval_messages; + _current_interval_rtcm_injections = 0; + _current_interval_bytes_written = 0; + _current_interval_bytes_read = 0; + _current_interval_messages = MessageTracker {}; + _current_interval_start_time = hrt_absolute_time(); } -void SeptentrioGPS::reset_update_rates() +bool SeptentrioDriver::update_monitoring_interval_ended() const { - _rate_count_vel = 0; - _rate_count_lat_lon = 0; - _interval_rate_start = hrt_absolute_time(); + return current_monitoring_interval_duration() > k_update_monitoring_interval_duration; } -void SeptentrioGPS::set_clock(timespec rtc_gps_time) +hrt_abstime SeptentrioDriver::current_monitoring_interval_duration() const +{ + return hrt_absolute_time() - _current_interval_start_time; +} + +float SeptentrioDriver::rtcm_injection_frequency() const +{ + return _last_interval_rtcm_injections / us_to_s(static_cast(k_update_monitoring_interval_duration)); +} + +uint32_t SeptentrioDriver::output_data_rate() const +{ + return static_cast(_last_interval_bytes_written / us_to_s(static_cast(k_update_monitoring_interval_duration))); +} + +uint32_t SeptentrioDriver::input_data_rate() const +{ + return static_cast(_last_interval_bytes_read / us_to_s(static_cast(k_update_monitoring_interval_duration))); +} + +bool SeptentrioDriver::receiver_configuration_healthy() const +{ + return _last_interval_messages.dop && + _last_interval_messages.pvt_geodetic && + _last_interval_messages.vel_cov_geodetic && + _last_interval_messages.att_euler && + _last_interval_messages.att_cov_euler; +} + +float SeptentrioDriver::us_to_s(uint64_t us) +{ + return static_cast(us) / 1000000.0f; +} + +bool SeptentrioDriver::clock_needs_update(timespec real_time) { timespec rtc_system_time; + px4_clock_gettime(CLOCK_REALTIME, &rtc_system_time); - int drift_time = abs(rtc_system_time.tv_sec - rtc_gps_time.tv_sec); + int drift_time = abs(rtc_system_time.tv_sec - real_time.tv_sec); - // As of 2021 setting the time on Nuttx temporarily pauses interrupts so only set the time if it is very wrong. - if (drift_time >= SEP_SET_CLOCK_DRIFT_TIME_S) { - // TODO: clock slewing of the RTC for small time differences + return drift_time >= k_max_allowed_clock_drift; +} + +void SeptentrioDriver::set_clock(timespec rtc_gps_time) +{ + if (clock_needs_update(rtc_gps_time)) { px4_clock_settime(CLOCK_REALTIME, &rtc_gps_time); } } +bool SeptentrioDriver::is_healthy() const +{ + if (_state == State::ReceivingData) { + if (!receiver_configuration_healthy()) { + return false; + } + } + + return true; +} + +void SeptentrioDriver::reset_gps_state_message() +{ + memset(&_message_gps_state, 0, sizeof(_message_gps_state)); + _message_gps_state.heading = NAN; + _message_gps_state.heading_offset = matrix::wrap_pi(math::radians(_heading_offset)); +} + +uint32_t SeptentrioDriver::get_parameter(const char *name, int32_t *value) +{ + return _get_parameter(name, value); +} + +uint32_t SeptentrioDriver::get_parameter(const char *name, float *value) +{ + return _get_parameter(name, value); +} + +} // namespace septentrio + extern "C" __EXPORT int septentrio_main(int argc, char *argv[]) { - return SeptentrioGPS::main(argc, argv); + return septentrio::SeptentrioDriver::main(argc, argv); } diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h index c950757c24f4..371c0f2d98cc 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -54,39 +54,62 @@ #include #include #include +#include #include #include -#include "sbf.h" +#include "module.h" +#include "sbf/decoder.h" #include "rtcm.h" -/* Message decoder state */ -typedef enum { - SBF_DECODE_SYNC1 = 0, - SBF_DECODE_SYNC2, - SBF_DECODE_PAYLOAD, - SBF_DECODE_RTCM3, -} sbf_decode_state_t; +using namespace time_literals; + +namespace septentrio +{ /** - * Struct for dynamic allocation of satellite info data. + * The parsed command line arguments for this module. */ -struct GPSSatelliteInfo { - satellite_info_s _data; +struct ModuleArguments { + int baud_rate_main {0}; + int baud_rate_secondary {0}; + const char *device_path_main {nullptr}; + const char *device_path_secondary {nullptr}; }; -enum class SeptentrioDumpCommMode : int32_t { +/** + * Which raw communication data to dump to the log file. +*/ +enum class DumpMode : int32_t { Disabled = 0, - Full, ///< dump full RX and TX data for all devices - RTCM, ///< dump received RTCM from Main GPS + FromReceiver = 1, + ToReceiver = 2, + Both = 3, }; -enum class SeptentrioInstance : uint8_t { +/** + * Instance of the driver. +*/ +enum class Instance : uint8_t { Main = 0, Secondary, }; -enum class SeptentrioGPSResetType { +/** + * Hardware setup and use of the connected receivers. +*/ +enum class ReceiverSetup { + /// Two receivers with the same purpose. + Default = 0, + + /// One rover and one moving base receiver, used for heading with two single-antenna receivers. + MovingBase = 1, +}; + +/** + * Type of reset to perform on the receiver. +*/ +enum class ReceiverResetType { /** * There is no pending GPS reset. */ @@ -117,27 +140,107 @@ enum class SeptentrioGPSResetType { }; /** - * The type of data the receiver should output. - */ -enum class SeptentrioGPSOutputMode { - GPS, ///< Only GPS output - GPSAndRTCM, ///< GPS and RTCM output + * Direction of raw data. +*/ +enum class DataDirection { + /// Data sent by the flight controller to the receiver. + ToReceiver, + + /// Data sent by the receiver to the flight controller. + FromReceiver, +}; + +/** + * Which satellites the receiver should use for PVT computation. +*/ +enum class SatelliteUsage : int32_t { + Default = 0, + GPS = 1 << 0, + GLONASS = 1 << 1, + Galileo = 1 << 2, + SBAS = 1 << 3, + BeiDou = 1 << 4, +}; + +/** + * General logging level of the receiver that determines the blocks to log. +*/ +enum class ReceiverLogLevel : int32_t { + Lite = 0, + Basic = 1, + Default = 2, + Full = 3, +}; + +/** + * Logging frequency of the receiver that determines SBF output frequency. +*/ +enum class ReceiverLogFrequency : int32_t { + Disabled = 0, + Hz0_1 = 1, + Hz0_2 = 2, + Hz0_5 = 3, + Hz1_0 = 4, + Hz2_0 = 5, + Hz5_0 = 6, + Hz10_0 = 7, + Hz20_0 = 8, + Hz25_0 = 9, + Hz50_0 = 10, +}; + +/** + * Output frequency for the main SBF blocks required for PVT computation. +*/ +enum class SBFOutputFrequency : int32_t { + Hz5_0 = 0, + Hz10_0 = 1, + Hz20_0 = 2, + Hz25_0 = 3, +}; + +/** + * Tracker for messages received by the driver. +*/ +struct MessageTracker { + bool dop {false}; + bool pvt_geodetic {false}; + bool vel_cov_geodetic {false}; + bool att_euler {false}; + bool att_cov_euler {false}; }; -class SeptentrioGPS : public ModuleBase, public device::Device +/** + * Used for a bitmask to keep track of received messages to know when we need to broadcast them and to monitor receiver health. +*/ +enum class ReceiverOutputTracker { + None = 0, + DOP = 1 << 0, + PVTGeodetic = 1 << 1, + VelCovGeodetic = 1 << 2, + AttEuler = 1 << 3, + AttCovEuler = 1 << 4, + HeadingMessages = AttEuler + AttCovEuler, + RequiredPositionMessages = DOP + PVTGeodetic + VelCovGeodetic + AttCovEuler, + PositioningMessages = DOP + PVTGeodetic + VelCovGeodetic + AttEuler + AttCovEuler, +}; + +class SeptentrioDriver : public ModuleBase, public device::Device { public: - SeptentrioGPS(const char *device_path, SeptentrioInstance instance, uint32_t baud_rate); - ~SeptentrioGPS() override; + SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate); + ~SeptentrioDriver() override; + /** @see ModuleBase */ int print_status() override; + /** @see ModuleBase */ void run() override; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); - static int task_spawn(int argc, char *argv[], SeptentrioInstance instance); + static int task_spawn(int argc, char *argv[], Instance instance); /** * @brief Secondary run trampoline to support two driver instances. @@ -145,9 +248,9 @@ class SeptentrioGPS : public ModuleBase, public device::Device static int run_trampoline_secondary(int argc, char *argv[]); /** @see ModuleBase */ - static SeptentrioGPS *instantiate(int argc, char *argv[]); + static SeptentrioDriver *instantiate(int argc, char *argv[]); - static SeptentrioGPS *instantiate(int argc, char *argv[], SeptentrioInstance instance); + static SeptentrioDriver *instantiate(int argc, char *argv[], Instance instance); /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); @@ -160,26 +263,88 @@ class SeptentrioGPS : public ModuleBase, public device::Device * * @return `PX4_OK` on success, `PX4_ERROR` on otherwise */ - int reset(SeptentrioGPSResetType type); + int reset(ReceiverResetType type); /** - * @brief Get the update rate for position information from the receiver. + * @brief Force command input on the currently used port on the receiver. * - * @return the position update rate of the receiver + * @return `PX4_OK` on success, `PX4_ERROR` otherwise */ - float get_position_update_rate(); + int force_input(); +private: + enum class State { + OpeningSerialPort, + ConfiguringDevice, + ReceivingData, + }; + + /** + * The current decoder that data has to be fed to. + */ + enum class DecodingStatus { + Searching, + SBF, + RTCMv3, + }; + + enum class ReceiveResult { + ReadError, + Timeout, + Receiving, + MessageAvailable, + }; + + /** + * Maximum timeout to wait for command acknowledgement by the receiver. + */ + static constexpr uint16_t k_receiver_ack_timeout = 200; + + /** + * Duration of one update monitoring interval in us. + * This should be longer than the time it takes for all *positioning* SBF messages to be sent once by the receiver! + * Otherwise the driver will assume the receiver configuration isn't healthy because it doesn't see all blocks in time. + */ + static constexpr hrt_abstime k_update_monitoring_interval_duration = 5_s; /** - * @brief Get the update rate for velocity information from the receiver. + * The default stream for output of PVT data. + */ + static constexpr uint8_t k_default_main_stream = 1; + + /** + * The default stream for output of logging data. + */ + static constexpr uint8_t k_default_log_stream = 2; + + /** + * @brief Parse command line arguments for this module. * - * @return the velocity update rate of the receiver - */ - float get_velocity_update_rate(); -private: + * @param argc Number of arguments. + * @param argv The arguments. + * @param arguments The parsed arguments. + * + * @return `PX4_OK` on success, `PX4_ERROR` on a parsing error. + */ + static int parse_cli_arguments(int argc, char *argv[], ModuleArguments &arguments); + + /** + * @brief Wait for the second instance to properly start up. + * + * @return `PX4_OK` once the second instance has started, or `PX4_ERROR` if timed out waiting. + */ + static int await_second_instance_startup(); + + /** + * @brief Wait for the second instance to properly shut down. + * + * @return `PX4_OK` once the second instance has shut down, or `PX4_ERROR` if timed out waiting. + */ + int await_second_instance_shutdown(); + /** * @brief Schedule a reset of the connected receiver. */ - void schedule_reset(SeptentrioGPSResetType type); + void schedule_reset(ReceiverResetType type); /** * @brief Detect the current baud rate used by the receiver on the connected port. @@ -188,7 +353,7 @@ class SeptentrioGPS : public ModuleBase, public device::Device * * @return The detected baud rate on success, or `0` on error */ - uint32_t detect_receiver_baud_rate(bool force_input); + uint32_t detect_receiver_baud_rate(bool forced_input); /** * @brief Try to detect the serial port used on the receiver side. @@ -200,11 +365,13 @@ class SeptentrioGPS : public ModuleBase, public device::Device int detect_serial_port(char *const port_name); /** - * Configure the receiver. + * @brief Configure the attached receiver based on the user's parameters. * - * @return `PX4_OK` on success, `PX4_ERROR` otherwise + * If the user has disabled automatic configuration, only execute the steps that do not touch the receiver (e.g., baud rate detection, port detection...). + * + * @return `PX4_OK` on success, `PX4_ERROR` otherwise. */ - int configure(float heading_offset); + int configure(); /** * @brief Parse the next byte of a received message from the receiver. @@ -213,6 +380,13 @@ class SeptentrioGPS : public ModuleBase, public device::Device */ int parse_char(const uint8_t byte); + /** + * @brief Process a fully received message from the receiver. + * + * @return `PX4_OK` on message handled, `PX4_ERROR` on error. + */ + int process_message(); + /** * @brief Add payload rx byte. * @@ -227,17 +401,12 @@ class SeptentrioGPS : public ModuleBase, public device::Device */ int payload_rx_done(); - /** - * @brief Reset the parse state machine for a fresh start. - */ - void decode_init(); - /** * @brief Send a message. * * @return true on success, false on write error (errno set) */ - bool send_message(const char *msg); + [[nodiscard]] bool send_message(const char *msg); /** * @brief Send a message and waits for acknowledge. @@ -247,11 +416,13 @@ class SeptentrioGPS : public ModuleBase, public device::Device * * @return true on success, false on write error (errno set) or ack wait timeout */ - bool send_message_and_wait_for_ack(const char *msg, const int timeout); + [[nodiscard]] bool send_message_and_wait_for_ack(const char *msg, const int timeout); /** * @brief Receive incoming messages. * + * @param timeout Maximum time to wait for new data in ms, after which we return. + * * @return -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled */ int receive(unsigned timeout); @@ -268,7 +439,7 @@ class SeptentrioGPS : public ModuleBase, public device::Device int read(uint8_t *buf, size_t buf_length, int timeout); /** - * This is an abstraction for the poll on serial used. + * @brief This is an abstraction for the poll on serial used. * * @param buf The read buffer * @param buf_length Size of the read buffer @@ -293,7 +464,7 @@ class SeptentrioGPS : public ModuleBase, public device::Device * * @return `PX4_OK` on success, `PX4_ERROR` otherwise */ - int initialize_communication_dump(); + int initialize_communication_dump(DumpMode mode); /** * @brief Reset the receiver if it was requested by the user. @@ -325,17 +496,17 @@ class SeptentrioGPS : public ModuleBase, public device::Device inline bool inject_data(uint8_t *data, size_t len); /** - * Publish the gps struct. + * @brief Publish new GPS data with uORB. */ void publish(); /** - * Publish the satellite info. + * @brief Publish new GPS satellite data with uORB. */ void publish_satellite_info(); /** - * Publish RTCM corrections. + * @brief Publish RTCM corrections. * * @param data: The raw data to publish * @param len: The size of `data` @@ -343,80 +514,202 @@ class SeptentrioGPS : public ModuleBase, public device::Device void publish_rtcm_corrections(uint8_t *data, size_t len); /** - * Dump gps communication. + * @brief Dump gps communication. * - * @param data The raw data of the message - * @param len The size of `data` - * @param mode The calling source - * @param msg_to_gps_device `true` if the message should be sent to the device, `false` otherwise + * @param data The raw data of the message. + * @param len The size of `data`. + * @param data_direction The type of data, either incoming or outgoing. */ - void dump_gps_data(const uint8_t *data, size_t len, SeptentrioDumpCommMode mode, bool msg_to_gps_device); + void dump_gps_data(const uint8_t *data, size_t len, DataDirection data_direction); /** - * Handle an RTCM message. - */ - void got_rtcm_message(uint8_t *data, size_t len); + * @brief Check whether we should dump incoming data. + * + * @return `true` when we should dump incoming data, `false` otherwise. + */ + bool should_dump_incoming() const; /** - * @brief Store the currently recorded update rates. - */ - void store_update_rates(); + * @brief Check whether we should dump outgoing data. + * + * @return `true` when we should dump outgoing data, `false` otherwise. + */ + bool should_dump_outgoing() const; /** - * @brief Reset the update rate measurement state. - */ - void reset_update_rates(); + * @brief Start a new update frequency monitoring interval. + */ + void start_update_monitoring_interval(); + + /** + * @brief Check whether the current update monitoring interval should end. + * + * @return `true` if a new interval should be started, or `false` if the current interval is still valid. + */ + bool update_monitoring_interval_ended() const; + + /** + * @brief Get the duration of the current update frequency monitoring interval. + * + * @return The duration of the current interval in us. + */ + hrt_abstime current_monitoring_interval_duration() const; + + /** + * @brief Calculate RTCM message injection frequency for the current measurement interval. + * + * @return The RTCM message injection frequency for the current measurement interval in Hz. + */ + float rtcm_injection_frequency() const; + + /** + * @brief Calculate output data rate to the receiver for the current measurement interval. + * + * @return The output data rate for the current measurement interval in B/s. + */ + uint32_t output_data_rate() const; /** - * Used to set the system clock accurately. + * @brief Calculate input data rate from the receiver for the current measurement interval. + * + * @return The input data rate for the current measurement interval in B/s. + */ + uint32_t input_data_rate() const; + + /** + * @brief Check whether the current receiver configuration is likely healthy. + * + * @return `true` if the receiver is operating correctly, `false` if it needs to be reconfigured. + */ + bool receiver_configuration_healthy() const; + + /** + * @brief Convert from microseconds to seconds. + * + * @return `us` converted into seconds. + */ + static float us_to_s(uint64_t us); + + /** + * @brief Check if the system clock needs to be updated with new time obtained from the receiver. + * + * Setting the clock on Nuttx temporarily pauses interrupts. Therefore it should only be set if it is absolutely necessary. + * + * @return `true` if the clock should be update, `false` if the clock is still accurate enough. + */ + static bool clock_needs_update(timespec real_time); + + /** + * @brief Used to set the system clock accurately. + * + * This does not guarantee that the clock will be set. * * @param time The current time. */ - void set_clock(timespec rtc_gps_time); - - px4::atomic _scheduled_reset{(int)SeptentrioGPSResetType::None}; ///< The type of receiver reset that is scheduled - SeptentrioGPSOutputMode _output_mode{SeptentrioGPSOutputMode::GPS}; ///< The type of data the receiver should output - SeptentrioDumpCommMode _dump_communication_mode{SeptentrioDumpCommMode::Disabled}; ///< GPS communication dump mode - sbf_decode_state_t _decode_state{SBF_DECODE_SYNC1}; ///< State of the SBF parser - device::Serial _uart {}; ///< Serial UART port for communication with the receiver - char _port[20] {}; ///< The path of the used serial device - bool _configured{false}; ///< Configuration status of the connected receiver - uint64_t _last_timestamp_time{0}; ///< The last time a timestamp was added to a position uORB message (us) - hrt_abstime _last_rtcm_injection_time{0}; ///< Time of last RTCM injection - uint8_t _msg_status{0}; - uint16_t _rx_payload_index{0}; ///< State for the message parser - sbf_buf_t _buf; ///< The complete received message - RTCMParsing *_rtcm_parsing{nullptr}; ///< RTCM message parser - uint8_t _selected_rtcm_instance{0}; ///< uORB instance that is being used for RTCM corrections - bool _healthy{false}; ///< Flag to signal if the GPS is OK - uint8_t _spoofing_state{0}; ///< Receiver spoofing state - uint8_t _jamming_state{0}; ///< Receiver jamming state - const SeptentrioInstance _instance; ///< The receiver that this instance of the driver controls - uint32_t _baud_rate; ///< Baud rate the driver uses with the receiver (0 means automatically detect) - static px4::atomic _secondary_instance; ///< Optional secondary instance of the driver + static void set_clock(timespec rtc_gps_time); + + /** + * @brief Check whether the driver is operating correctly. + * + * @return `true` if the driver is working as expected, `false` otherwise. + */ + bool is_healthy() const; + + /** + * @brief Reset the GPS state uORB message for reuse. + */ + void reset_gps_state_message(); + + /** + * @brief Get the parameter with the given name into `value`. + * + * @param name The name of the parameter. + * @param value The value in which to store the parameter. + * + * @return `PX4_OK` on success, or `PX4_ERROR` if the parameter wasn't found. + */ + static uint32_t get_parameter(const char *name, int32_t *value); + + /** + * @brief Get the parameter with the given name into `value`. + * + * @param name The name of the parameter. + * @param value The value in which to store the parameter. + * + * @return `PX4_OK` on success, or `PX4_ERROR` if the parameter wasn't found. + */ + static uint32_t get_parameter(const char *name, float *value); + + /** + * @brief Don't use this, use the other parameter functions instead! + */ + template + static uint32_t _get_parameter(const char *name, T *value) + { + param_t handle = param_find(name); + + if (handle == PARAM_INVALID || param_get(handle, value) == PX4_ERROR) { + SEP_ERR("Failed to get parameter %s", name); + return PX4_ERROR; + } + + return PX4_OK; + } + + State _state {State::OpeningSerialPort}; ///< Driver state which allows for single run loop + px4::atomic _scheduled_reset {static_cast(ReceiverResetType::None)}; ///< The type of receiver reset that is scheduled + DumpMode _dump_communication_mode {DumpMode::Disabled}; ///< GPS communication dump mode + device::Serial _uart {}; ///< Serial UART port for communication with the receiver + char _port[20] {}; ///< The path of the used serial device + hrt_abstime _last_rtcm_injection_time {0}; ///< Time of last RTCM injection + uint8_t _selected_rtcm_instance {0}; ///< uORB instance that is being used for RTCM corrections + uint8_t _spoofing_state {0}; ///< Receiver spoofing state + uint8_t _jamming_state {0}; ///< Receiver jamming state + const Instance _instance {Instance::Main}; ///< The receiver that this instance of the driver controls + uint32_t _baud_rate {0}; + static px4::atomic _secondary_instance; + hrt_abstime _sleep_end {0}; ///< End time for sleeping + State _resume_state {State::OpeningSerialPort}; ///< Resume state after sleep + + // Module configuration + float _heading_offset {0.0f}; ///< The heading offset given by the `SEP_YAW_OFFS` parameter + float _pitch_offset {0.0f}; ///< The pitch offset given by the `SEP_PITCH_OFFS` parameter + uint32_t _receiver_stream_main {k_default_main_stream}; ///< The main output stream for the receiver given by the `SEP_STREAM_MAIN` parameter + uint32_t _receiver_stream_log {k_default_log_stream}; ///< The log output stream for the receiver given by the `SEP_STREAM_LOG` parameter + SBFOutputFrequency _sbf_output_frequency {SBFOutputFrequency::Hz5_0}; ///< Output frequency of the main SBF blocks given by the `SEP_OUTP_HZ` parameter + ReceiverLogFrequency _receiver_logging_frequency {ReceiverLogFrequency::Hz1_0}; ///< Logging frequency of the receiver given by the `SEP_LOG_HZ` parameter + ReceiverLogLevel _receiver_logging_level {ReceiverLogLevel::Default}; ///< Logging level of the receiver given by the `SEP_LOG_LEVEL` parameter + bool _receiver_logging_overwrite {0}; ///< Logging overwrite behavior, given by the `SEP_LOG_FORCE` parameter + bool _automatic_configuration {true}; ///< Automatic configuration of the receiver given by the `SEP_AUTO_CONFIG` parameter + ReceiverSetup _receiver_setup {ReceiverSetup::Default}; ///< Purpose of the receivers, given by the `SEP_HARDW_SETUP` parameter + int32_t _receiver_constellation_usage {0}; ///< Constellation usage in PVT computation given by the `SEP_CONST_USAGE` parameter + + // Decoding and parsing + DecodingStatus _active_decoder {DecodingStatus::Searching}; ///< Currently active decoder that new data has to be fed into + sbf::Decoder _sbf_decoder {}; ///< SBF message decoder + rtcm::Decoder *_rtcm_decoder {nullptr}; ///< RTCM message decoder // uORB topics and subscriptions - gps_dump_s *_dump_to_device{nullptr}; ///< uORB GPS dump data (to the receiver) - gps_dump_s *_dump_from_device{nullptr}; ///< uORB GPS dump data (from the receiver) - sensor_gps_s _report_gps_pos{}; ///< uORB topic for gps position - satellite_info_s *_p_report_sat_info{nullptr}; ///< Pointer to uORB topic for satellite info - GPSSatelliteInfo *_sat_info{nullptr}; ///< Instance of GPS sat info data object - uORB::Publication _dump_communication_pub{ORB_ID(gps_dump)}; ///< uORB topic for dump GPS data - uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; ///< uORB topic for injected data to the receiver - uORB::PublicationMulti _report_gps_pos_pub{ORB_ID(sensor_gps)}; ///< uORB topic for gps position - uORB::PublicationMulti _report_sat_info_pub{ORB_ID(satellite_info)}; ///< uORB topic for satellite info - uORB::SubscriptionMultiArray _orb_inject_data_sub{ORB_ID::gps_inject_data}; ///< uORB topic about data to inject to the receiver - - // Reading and update rates - // NOTE: Both `_rate_vel` and `_rate_lat_lon` seem to be used in exactly the same way. - uint64_t _interval_rate_start{0}; ///< Start moment of measurement interval for position and velocity messages from the receiver - float _rate{0.0f}; ///< uORB position data publish rate (Hz) - uint8_t _rate_count_vel; ///< Velocity messages from receiver in current measurement interval - uint8_t _rate_count_lat_lon{}; ///< Position messages from receiver in current measurement interval - unsigned _last_rate_rtcm_injection_count{0}; ///< Counter for number of RTCM messages - float _rate_vel{0.0f}; ///< Velocity message rate from receiver in the last measurement interval (Hz) - float _rate_lat_lon{0.0f}; ///< Position message rate from receiver in the last measurement interval (Hz) - float _rate_rtcm_injection{0.0f}; ///< RTCM message injection rate (Hz) - unsigned _rate_reading{0}; ///< Byte reading rate (Hz) - unsigned _num_bytes_read{0}; ///< Number of bytes read in last measurement interval + sensor_gps_s _message_gps_state {}; ///< uORB topic for position + gps_dump_s *_message_data_to_receiver {nullptr}; ///< uORB topic for dumping data to the receiver + gps_dump_s *_message_data_from_receiver {nullptr}; ///< uORB topic for dumping data from the receiver + satellite_info_s *_message_satellite_info {nullptr}; ///< uORB topic for satellite info + uORB::PublicationMulti _sensor_gps_pub {ORB_ID(sensor_gps)}; ///< uORB publication for gps position + uORB::Publication _gps_dump_pub {ORB_ID(gps_dump)}; ///< uORB publication for dump GPS data + uORB::Publication _gps_inject_data_pub {ORB_ID(gps_inject_data)}; ///< uORB publication for injected data to the receiver + uORB::PublicationMulti _satellite_info_pub {ORB_ID(satellite_info)}; ///< uORB publication for satellite info + uORB::SubscriptionMultiArray _gps_inject_data_sub {ORB_ID::gps_inject_data}; ///< uORB subscription about data to inject to the receiver + + // Data about update frequencies of various bits of information like RTCM message injection frequency, received data rate... + hrt_abstime _current_interval_start_time {0}; ///< Start time of the current update measurement interval in us + uint16_t _last_interval_rtcm_injections {0}; ///< Nr of RTCM message injections in the last measurement interval + uint16_t _current_interval_rtcm_injections {0}; ///< Nr of RTCM message injections in the current measurement interval + uint32_t _last_interval_bytes_written {0}; ///< Nr of bytes written to the receiver in the last measurement interval + uint32_t _current_interval_bytes_written {0}; ///< Nr of bytes written to the receiver in the current measurement interval + uint32_t _last_interval_bytes_read {0}; ///< Nr of bytes read from the receiver in the last measurement interval + uint32_t _current_interval_bytes_read {0}; ///< Nr of bytes read from the receiver in the current measurement interval + MessageTracker _last_interval_messages {}; ///< Messages encountered in the last measurement interval + MessageTracker _current_interval_messages {}; ///< Messages encountered in the current measurement interval }; + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/util.cpp b/src/drivers/gnss/septentrio/util.cpp index 47d6b3923ab6..34bf02f58b89 100644 --- a/src/drivers/gnss/septentrio/util.cpp +++ b/src/drivers/gnss/septentrio/util.cpp @@ -31,9 +31,18 @@ * ****************************************************************************/ +/** + * @file util.cpp + * + * @author Thomas Frans +*/ + #include "util.h" -uint16_t sep_crc16(const uint8_t *data_p, uint32_t length) +namespace septentrio +{ + +uint16_t buffer_crc16(const uint8_t *data_p, uint32_t length) { uint8_t x; uint16_t crc = 0; @@ -46,3 +55,5 @@ uint16_t sep_crc16(const uint8_t *data_p, uint32_t length) return crc; } + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/util.h b/src/drivers/gnss/septentrio/util.h index e0ff5d0ef806..e9de9af50dff 100644 --- a/src/drivers/gnss/septentrio/util.h +++ b/src/drivers/gnss/septentrio/util.h @@ -31,9 +31,20 @@ * ****************************************************************************/ +/** + * @file util.h + * + * @author Thomas Frans + */ + #pragma once +namespace septentrio +{ + /** * @brief Calculate buffer CRC16 */ -uint16_t sep_crc16(const uint8_t *data_p, uint32_t length); +uint16_t buffer_crc16(const uint8_t *data_p, uint32_t length); + +} // namespace septentrio