diff --git a/boards/cuav/nora/default.px4board b/boards/cuav/nora/default.px4board index 2a0e40e37efe..8c3e304856c3 100644 --- a/boards/cuav/nora/default.px4board +++ b/boards/cuav/nora/default.px4board @@ -14,6 +14,7 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index e3c84b183895..480f896d67cf 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -15,6 +15,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y diff --git a/boards/cubepilot/cubeorangeplus/default.px4board b/boards/cubepilot/cubeorangeplus/default.px4board index d755eea8d814..e3fc787417c6 100644 --- a/boards/cubepilot/cubeorangeplus/default.px4board +++ b/boards/cubepilot/cubeorangeplus/default.px4board @@ -15,6 +15,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index 40301136e26a..fb49c90866cb 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -13,6 +13,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI085=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index f0753941e03d..b796d1bfbf49 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -10,6 +10,7 @@ CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y CONFIG_DRIVERS_IMU_ST_L3GD20=y diff --git a/boards/px4/fmu-v3/default.px4board b/boards/px4/fmu-v3/default.px4board index 5c24d0e731fb..96b71c0cf653 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/default.px4board @@ -15,6 +15,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y diff --git a/boards/px4/fmu-v4/default.px4board b/boards/px4/fmu-v4/default.px4board index ab106bdb7b90..e7906db52c85 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/default.px4board @@ -15,6 +15,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y 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/Kconfig b/src/drivers/gnss/Kconfig new file mode 100644 index 000000000000..6353836988b2 --- /dev/null +++ b/src/drivers/gnss/Kconfig @@ -0,0 +1 @@ +rsource "*/Kconfig" diff --git a/src/drivers/gnss/septentrio/CMakeLists.txt b/src/drivers/gnss/septentrio/CMakeLists.txt new file mode 100644 index 000000000000..62451fc9b1f3 --- /dev/null +++ b/src/drivers/gnss/septentrio/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE driver__septentrio + MAIN septentrio + COMPILE_FLAGS + # -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/Kconfig b/src/drivers/gnss/septentrio/Kconfig new file mode 100644 index 000000000000..5d3d52388587 --- /dev/null +++ b/src/drivers/gnss/septentrio/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_GNSS_SEPTENTRIO + bool "Septentrio GNSS receivers" + default n + ---help--- + Enable support for Septentrio receivers 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..c935c1263e44 --- /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_ERR(__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 new file mode 100644 index 000000000000..1b151f3618ba --- /dev/null +++ b/src/drivers/gnss/septentrio/module.yaml @@ -0,0 +1,206 @@ +module_name: Septentrio + +serial_config: + - command: set DUAL_GPS_ARGS "-e ${SERIAL_DEV} -g p:${BAUD_PARAM}" + port_config_param: + name: SEP_PORT2_CFG + 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 + default: GPS1 + label: GPS Port + +parameters: + - 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 + long: | + Heading offset angle for dual antenna GPS setups that support heading estimation. + + Set this to 0 if the antennas are parallel to the forward-facing direction + of the vehicle and the rover antenna is in front. + + The offset angle increases clockwise. + + Set this to 90 if the rover antenna is placed on the + right side of the vehicle and the moving base antenna is on the left side. + type: float + decimal: 3 + default: 0 + min: -360 + max: 360 + unit: deg + reboot_required: true + SEP_SAT_INFO: + description: + short: Enable sat info + long: | + Enable publication of satellite info (ORB_ID(satellite_info)) if possible. + type: boolean + default: 0 + reboot_required: true + SEP_PITCH_OFFS: + description: + short: Pitch offset for dual antenna GPS + long: | + Vertical offsets can be compensated for by adjusting the Pitch offset. + + Note that this can be interpreted as the "roll" angle in case the antennas are aligned along the perpendicular axis. + This occurs in situations where the two antenna ARPs may not be exactly at the same height in the vehicle reference frame. + Since pitch is defined as the right-handed rotation about the vehicle Y axis, + a situation where the main antenna is mounted lower than the aux antenna (assuming the default antenna setup) will result in a positive pitch. + type: float + decimal: 3 + default: 0 + min: -90 + max: 90 + unit: deg + reboot_required: true + SEP_DUMP_COMM: + description: + short: Log GPS communication data + long: | + Log raw communication between the driver and connected receivers. + For example, "To receiver" will log all commands and corrections sent by the driver to the receiver. + 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. + + 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: 1 + values: + 0: Default + 1: Moving base + reboot_required: true diff --git a/src/drivers/gnss/septentrio/rtcm.cpp b/src/drivers/gnss/septentrio/rtcm.cpp new file mode 100644 index 000000000000..007b2a168e33 --- /dev/null +++ b/src/drivers/gnss/septentrio/rtcm.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * 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 rtcm.cpp + * + * @author Thomas Frans +*/ + +#include "rtcm.h" + +#include +#include + +#include "module.h" + +namespace septentrio +{ + +namespace rtcm +{ + +Decoder::Decoder() +{ + reset(); +} + +Decoder::~Decoder() +{ + delete[] _message; +} + +Decoder::State Decoder::add_byte(uint8_t byte) +{ + 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; + } + + return _state; +} + +void Decoder::reset() +{ + if (_message) { + delete[] _message; + } + + _message = new uint8_t[INITIAL_BUFFER_LENGTH]; + _current_index = 0; + _message_length = 0; + _state = State::SearchingPreamble; +} + +uint16_t Decoder::parse_message_length() const +{ + if (!header_received()) { + return PX4_ERROR; + } + + return ((static_cast(_message[1]) & 3) << 8) | _message[2]; +} + +bool Decoder::header_received() const +{ + return _current_index >= HEADER_SIZE; +} + +uint16_t Decoder::received_bytes() const +{ + return _current_index; +} + +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 new file mode 100644 index 000000000000..672ac08da9e1 --- /dev/null +++ b/src/drivers/gnss/septentrio/rtcm.h @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * 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 rtcm.h + * + * @author Thomas Frans + */ + +#pragma once + +#include +#include + +namespace septentrio +{ + +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 Decoder +{ +public: + 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(); + + /** + * Add a byte to the current message. + * + * @param byte The new byte. + * + * @return true if message complete (use @message to get it) + */ + State add_byte(uint8_t b); + + /** + * @brief Reset the parser to a clean state. + */ + void reset(); + + uint8_t *message() const { return _message; } + + /** + * @brief Number of received bytes of the current message. + */ + uint16_t received_bytes() const; + + /** + * @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: + 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/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..487a40054c64 --- /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.0f * 10000000000.0f}; +constexpr double k_dnu_f8_value {-2.0 * 10000000000.0}; + +/// 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 new file mode 100644 index 000000000000..5c53db6098d5 --- /dev/null +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -0,0 +1,1751 @@ +/**************************************************************************** + * + * 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 septentrio.cpp + * + * Septentrio GNSS receiver driver + * + * @author Matej Franceskin + * @author Seppe Geuens + * @author Thomas Frans +*/ + +#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 device; + +namespace septentrio +{ + +/** + * RTC drift time when time synchronization is needed (in seconds). +*/ +constexpr int k_max_allowed_clock_drift = 5; + +/** + * If silence from the receiver for this time (ms), assume full data received. +*/ +constexpr int k_receiver_read_timeout = 2; + +/** + * The maximum allowed time for reading from the receiver. + */ +constexpr int k_max_receiver_read_timeout = 50; + +/** + * Minimum amount of bytes we try to read at one time from the receiver. +*/ +constexpr size_t k_min_receiver_read_bytes = 32; + +/** + * The baud rate of Septentrio receivers with factory default configuration. +*/ +constexpr uint32_t k_septentrio_receiver_default_baud_rate = 115200; + +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}; +uint32_t SeptentrioDriver::k_supported_baud_rates[] {0, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1500000}; +uint32_t SeptentrioDriver::k_default_baud_rate {230400}; +orb_advert_t SeptentrioDriver::k_mavlink_log_pub {nullptr}; + +SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate) : + Device(MODULE_NAME), + _instance(instance), + _chosen_baud_rate(baud_rate) +{ + strncpy(_port, device_path, sizeof(_port) - 1); + // Enforce null termination. + _port[sizeof(_port) - 1] = '\0'; + + int32_t enable_sat_info {0}; + get_parameter("SEP_SAT_INFO", &enable_sat_info); + + if (enable_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; + + if (_receiver_stream_log == _receiver_stream_main) { + mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Logging stream should be different from main stream"); + } + + 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); + + reset_gps_state_message(); +} + +SeptentrioDriver::~SeptentrioDriver() +{ + if (_instance == Instance::Main) { + if (await_second_instance_shutdown() == PX4_ERROR) { + SEP_ERR("Secondary instance shutdown timed out"); + } + } + + if (_message_data_from_receiver) { + delete _message_data_from_receiver; + } + + if (_message_data_to_receiver) { + delete _message_data_to_receiver; + } + + if (_message_satellite_info) { + delete _message_satellite_info; + } + + if (_rtcm_decoder) { + delete _rtcm_decoder; + } +} + +int SeptentrioDriver::print_status() +{ + SeptentrioDriver *secondary_instance = _secondary_instance.load(); + + switch (_instance) { + case Instance::Main: + PX4_INFO("Main GPS"); + break; + + case Instance::Secondary: + PX4_INFO(""); + PX4_INFO("Secondary GPS"); + break; + } + + PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _uart.getBaudrate()); + 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 (first_gps_uorb_message_created() && _state == State::ReceivingData) { + PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast(rtcm_injection_frequency())); + print_message(ORB_ID(sensor_gps), _message_gps_state); + } + + if (_instance == Instance::Main && secondary_instance) { + secondary_instance->print_status(); + } + + return 0; +} + +void SeptentrioDriver::run() +{ + while (!should_exit()) { + switch (_state) { + case State::OpeningSerialPort: { + _uart.setPort(_port); + + if (_uart.open()) { + _state = State::DetectingBaudRate; + + } else { + // Failed to open port, so wait a bit before trying again. + px4_usleep(200000); + } + + break; + } + + case State::DetectingBaudRate: { + static uint32_t expected_baud_rates[] = {k_septentrio_receiver_default_baud_rate, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000}; + expected_baud_rates[0] = _chosen_baud_rate != 0 ? _chosen_baud_rate : k_septentrio_receiver_default_baud_rate; + + if (detect_receiver_baud_rate(expected_baud_rates[_current_baud_rate_index], true)) { + if (set_baudrate(expected_baud_rates[_current_baud_rate_index]) == PX4_OK) { + _state = State::ConfiguringDevice; + + } else { + SEP_ERR("Setting local baud rate failed"); + } + + } else { + _current_baud_rate_index++; + + if (_current_baud_rate_index == sizeof(expected_baud_rates) / sizeof(expected_baud_rates[0])) { + _current_baud_rate_index = 0; + } + } + + break; + } + + case State::ConfiguringDevice: { + ConfigureResult result = configure(); + + if (!(static_cast(result) & static_cast(ConfigureResult::FailedCompletely))) { + if (static_cast(result) & static_cast(ConfigureResult::NoLogging)) { + mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Failed to configure receiver internal logging"); + } + + SEP_INFO("Automatic configuration finished"); + _state = State::ReceivingData; + + } else { + _state = State::DetectingBaudRate; + } + + break; + } + + case State::ReceivingData: { + int receive_result {0}; + + receive_result = receive(k_timeout_5hz); + + if (receive_result == -1) { + _state = State::DetectingBaudRate; + } + + if (_message_satellite_info && (receive_result & 2)) { + publish_satellite_info(); + } + + break; + } + + } + + reset_if_scheduled(); + + handle_inject_data_topic(); + + if (update_monitoring_interval_ended()) { + start_update_monitoring_interval(); + } + } + +} + +int SeptentrioDriver::task_spawn(int argc, char *argv[]) +{ + return task_spawn(argc, argv, Instance::Main); +} + +int SeptentrioDriver::task_spawn(int argc, char *argv[], Instance instance) +{ + px4_main_t entry_point; + static constexpr int k_task_stack_size = PX4_STACK_ADJUSTED(2048); + + if (instance == Instance::Main) { + entry_point = &run_trampoline; + + } else { + entry_point = &run_trampoline_secondary; + } + + px4_task_t task_id = px4_task_spawn_cmd("septentrio", + SCHED_DEFAULT, + SCHED_PRIORITY_SLOW_DRIVER, + k_task_stack_size, + entry_point, + (char *const *)argv); + + if (task_id < 0) { + // `_task_id` of module that hasn't been started before or has been stopped should already be -1. + // This is just to make sure. + _task_id = -1; + return -errno; + } + + if (instance == Instance::Main) { + _task_id = task_id; + } + + return 0; +} + +int SeptentrioDriver::run_trampoline_secondary(int argc, char *argv[]) +{ + // Get rid of the task name (first argument) + argc -= 1; + argv += 1; + + 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; +} + +SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[]) +{ + return instantiate(argc, argv, Instance::Main); +} + +SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance instance) +{ + ModuleArguments arguments {}; + SeptentrioDriver *gps {nullptr}; + + if (parse_cli_arguments(argc, argv, arguments) == PX4_ERROR) { + return nullptr; + } + + if (arguments.device_path_main && arguments.device_path_secondary + && strcmp(arguments.device_path_main, arguments.device_path_secondary) == 0) { + mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Device paths must be different"); + return nullptr; + } + + bool valid_chosen_baud_rate {false}; + + for (uint8_t i = 0; i < sizeof(k_supported_baud_rates) / sizeof(k_supported_baud_rates[0]); i++) { + switch (instance) { + case Instance::Main: + if (arguments.baud_rate_main == static_cast(k_supported_baud_rates[i])) { + valid_chosen_baud_rate = true; + } + + break; + + case Instance::Secondary: + if (arguments.baud_rate_secondary == static_cast(k_supported_baud_rates[i])) { + valid_chosen_baud_rate = true; + } + + break; + } + } + + if (!valid_chosen_baud_rate) { + mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Baud rate %d is unsupported, falling back to default %lu", + instance == Instance::Main ? arguments.baud_rate_main : arguments.baud_rate_secondary, k_default_baud_rate); + } + + if (instance == Instance::Main) { + if (Serial::validatePort(arguments.device_path_main)) { + gps = new SeptentrioDriver(arguments.device_path_main, instance, + valid_chosen_baud_rate ? arguments.baud_rate_main : k_default_baud_rate); + + } else { + PX4_ERR("Invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : ""); + } + + if (gps && arguments.device_path_secondary) { + task_spawn(argc, argv, Instance::Secondary); + + if (await_second_instance_startup() == PX4_ERROR) { + return nullptr; + } + } + + } else { + if (Serial::validatePort(arguments.device_path_secondary)) { + gps = new SeptentrioDriver(arguments.device_path_secondary, instance, + valid_chosen_baud_rate ? arguments.baud_rate_secondary : k_default_baud_rate); + + } else { + PX4_ERR("Invalid secondary device (-e) %s", arguments.device_path_secondary ? arguments.device_path_secondary : ""); + } + } + + return gps; +} + +// Called from outside driver thread. +// Return 0 on success, -1 otherwise. +int SeptentrioDriver::custom_command(int argc, char *argv[]) +{ + bool handled = false; + const char *failure_reason {"unknown command"}; + SeptentrioDriver *driver_instance; + + if (!is_running()) { + PX4_INFO("not running"); + return -1; + } + + driver_instance = get_instance(); + + if (argc >= 1 && strcmp(argv[0], "reset") == 0) { + if (argc == 2) { + ReceiverResetType type{ReceiverResetType::None}; + + if (strcmp(argv[1], "hot") == 0) { + type = ReceiverResetType::Hot; + + } else if (strcmp(argv[1], "warm") == 0) { + type = ReceiverResetType::Warm; + + } else if (strcmp(argv[1], "cold") == 0) { + type = ReceiverResetType::Cold; + + } else { + failure_reason = "unknown reset type"; + } + + if (type != ReceiverResetType::None) { + driver_instance->schedule_reset(type); + handled = true; + } + + } else { + failure_reason = "incorrect usage of reset command"; + } + } + + return handled ? 0 : print_usage(failure_reason); +} + +int SeptentrioDriver::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for Septentrio GNSS receivers. +It can automatically configure them and make their output available for the rest of the system. +A secondary receiver is supported for redundancy, logging and dual-receiver heading. +Septentrio receiver baud rates from 57600 to 1500000 are supported. +If others are used, the driver will use 230400 and give a warning. + +### Examples + +Use one receiver on port `/dev/ttyS0` and automatically configure it to use baud rate 230400: +$ septentrio start -d /dev/ttyS0 -b 230400 + +Use two receivers, the primary on port `/dev/ttyS3` and the secondary on `/dev/ttyS4`, +detect baud rate automatically and preserve them: +$ septentrio start -d /dev/ttyS3 -e /dev/ttyS4 + +Perform warm reset of the receivers: +$ gps reset warm +)DESCR_STR"); + PRINT_MODULE_USAGE_NAME("septentrio", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "Primary receiver port", false); + PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary receiver baud rate", true); + PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "", "Secondary receiver port", true); + PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary receiver baud rate", true); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver"); + PRINT_MODULE_USAGE_ARG("cold|warm|hot", "Specify reset type", false); + + return 0; +} + +int SeptentrioDriver::reset(ReceiverResetType type) +{ + bool res = false; + + force_input(); + + switch (type) { + case ReceiverResetType::Hot: + res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout_fast); + break; + + case ReceiverResetType::Warm: + res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout_fast); + break; + + case ReceiverResetType::Cold: + res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout_fast); + break; + + default: + break; + } + + if (res) { + return PX4_OK; + } else { + return PX4_ERROR; + } +} + +int SeptentrioDriver::force_input() +{ + 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; +} + +int SeptentrioDriver::await_second_instance_startup() +{ + 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; +} + +int SeptentrioDriver::await_second_instance_shutdown() +{ + 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 == Instance::Main && secondary_instance) { + secondary_instance->schedule_reset(reset_type); + } +} + +bool SeptentrioDriver::detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input) { + if (set_baudrate(baud_rate) != PX4_OK) { + return false; + } + + if (forced_input) { + force_input(); + } + + // Make sure that any weird data is "flushed" in the receiver. + (void)send_message("\n"); + + if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) { + SEP_INFO("Detected baud rate: %lu", baud_rate); + return true; + } + + return false; +} + +int SeptentrioDriver::detect_serial_port(char* const port_name) { + // Read buffer to get the COM port + 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 timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout_fast; + bool response_detected = false; + + // 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, k_receiver_ack_timeout_fast); + + if (read_result < 0) { + SEP_WARN("SBF read error"); + return PX4_ERROR; + } + + // 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'; + + char* port_name_address = strstr(buf, ">"); + + // Check if we found a port candidate. + if (buffer_offset > 4 && port_name_address != nullptr) { + size_t port_name_offset = reinterpret_cast(port_name_address) - reinterpret_cast(buf) - 4; + for (size_t i = 0; i < 4; i++) { + port_name[i] = buf[port_name_offset + i]; + } + // NOTE: This limits the ports to serial and USB ports only. Otherwise the detection doesn't work correctly. + if (strstr(port_name, "COM") != nullptr || strstr(port_name, "USB") != nullptr) { + response_detected = true; + break; + } + } + + if (buffer_offset + 1 >= sizeof(buf)) { + // Copy the last 3 bytes such that a half port isn't lost. + for (int i = 0; i < 4; i++) { + buf[i] = buf[sizeof(buf) - 4 + i]; + } + buffer_offset = 3; + } + } while (timeout_time > hrt_absolute_time()); + + if (!response_detected) { + SEP_WARN("No valid serial port detected"); + return PX4_ERROR; + } else { + SEP_INFO("Serial port found: %s", port_name); + return PX4_OK; + } +} + +SeptentrioDriver::ConfigureResult SeptentrioDriver::configure() +{ + char msg[k_max_command_size] {}; + char com_port[5] {}; + ConfigureResult result {ConfigureResult::OK}; + + // Passively detect receiver port. + if (detect_serial_port(com_port) != PX4_OK) { + SEP_WARN("CONFIG: failed port detection"); + return ConfigureResult::FailedCompletely; + } + + // 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 ConfigureResult::OK; + } + + // If user requested specific baud rate, set it now. Otherwise keep detected baud rate. + if (strstr(com_port, "COM") != nullptr && _chosen_baud_rate != 0) { + snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _chosen_baud_rate); + + if (!send_message(msg)) { + SEP_WARN("CONFIG: baud rate command write error"); + return ConfigureResult::FailedCompletely; + } + + // 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(2000000); + + if (set_baudrate(_chosen_baud_rate) != PX4_OK) { + SEP_WARN("CONFIG: failed local baud rate setting"); + return ConfigureResult::FailedCompletely; + } + + if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: failed remote baud rate setting"); + return ConfigureResult::FailedCompletely; + } + } + + // Delete all sbf outputs on current COM port to remove clutter data + snprintf(msg, sizeof(msg), k_command_clear_sbf, _receiver_stream_main, com_port); + + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: failed delete output"); + return ConfigureResult::FailedCompletely; // connection and/or baudrate detection failed + } + + // 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); + // Use a longer timeout as the `setSatelliteUsage` command acknowledges a bit slower on mosaic-H-based receivers. + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_slow)) { + SEP_WARN("CONFIG: Failed to configure constellation usage"); + return ConfigureResult::FailedCompletely; + } + } + + // 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}; + + 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; + } + + 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; + } + + 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_fast)) { + result = static_cast(static_cast(result) | static_cast(ConfigureResult::NoLogging)); + } + } else if (_receiver_stream_log == _receiver_stream_main) { + result = static_cast(static_cast(result) | static_cast(ConfigureResult::NoLogging)); + } + + // Define/inquire the type of data that the receiver should accept/send on a given connection descriptor + snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF"); + + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + return ConfigureResult::FailedCompletely; + } + + // Specify the offsets that the receiver applies to the computed attitude angles. + snprintf(msg, sizeof(msg), k_command_set_attitude_offset, static_cast(_heading_offset), static_cast(_pitch_offset)); + + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + return ConfigureResult::FailedCompletely; + } + + snprintf(msg, sizeof(msg), k_command_set_dynamics, "high"); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + return ConfigureResult::FailedCompletely; + } + + 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), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: Failed to configure SBF"); + return ConfigureResult::FailedCompletely; + } + + 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_fast)) { + SEP_WARN("CONFIG: 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_fast)) { + SEP_WARN("CONFIG: Failed to configure attitude source"); + } + } + } 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_WARN("CONFIG: Failed to configure attitude source"); + return ConfigureResult::FailedCompletely; + } + } + + return result; +} + +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 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; + 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 result; +} + +int SeptentrioDriver::process_message() +{ + int result = PX4_ERROR; + + switch (_active_decoder) { + case DecodingStatus::Searching: { + SEP_ERR("Can't process incomplete message!"); + result = PX4_ERROR; + break; + } + case DecodingStatus::SBF: { + using namespace sbf; + + 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; + + DOP dop; + + 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; + } + + break; + } + case BlockID::PVTGeodetic: { + using ModeType = PVTGeodetic::ModeType; + using Error = PVTGeodetic::Error; + + SEP_TRACE_PARSING("Processing PVTGeodetic SBF message"); + _current_interval_messages.pvt_geodetic = true; + + Header header; + PVTGeodetic pvt_geodetic; + + if (_sbf_decoder.parse(&header) == PX4_OK && _sbf_decoder.parse(&pvt_geodetic) == PX4_OK) { + switch (pvt_geodetic.mode_type) { + case ModeType::NoPVT: + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE; + break; + case ModeType::PVTWithSBAS: + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL; + break; + case ModeType::RTKFloat: + case ModeType::MovingBaseRTKFloat: + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT; + break; + case ModeType::RTKFixed: + case ModeType::MovingBaseRTKFixed: + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED; + break; + default: + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_3D; + break; + } + + // 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 (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.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; + } else { + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE; + } + + if (pvt_geodetic.nr_sv != PVTGeodetic::k_dnu_nr_sv) { + _message_gps_state.satellites_used = pvt_geodetic.nr_sv; + + 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; + } + + } else { + _message_gps_state.satellites_used = 0; + } + + /* 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; + + // 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; + _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); + + if (pvt_geodetic.cog > k_dnu_f4_value) { + _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; + } + +#endif + _message_gps_state.timestamp = hrt_absolute_time(); + result = PX4_OK; + } + + break; + } + + case BlockID::ReceiverStatus: { + SEP_TRACE_PARSING("Processing ReceiverStatus SBF message"); + + ReceiverStatus receiver_status; + + 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; + } + + break; + } + 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"); + + // 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; + + VelCovGeodetic vel_cov_geodetic; + + if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) { + if (vel_cov_geodetic.cov_ve_ve > k_dnu_f4_value && vel_cov_geodetic.cov_vn_vn > k_dnu_f4_value && vel_cov_geodetic.cov_vu_vu > k_dnu_f4_value) { + _message_gps_state.s_variance_m_s = math::max(math::max(vel_cov_geodetic.cov_ve_ve, vel_cov_geodetic.cov_vn_vn), vel_cov_geodetic.cov_vu_vu); + } + } + + break; + } + case BlockID::GEOIonoDelay: { + SEP_TRACE_PARSING("Processing GEOIonoDelay SBF message"); + break; + } + case BlockID::AttEuler: { + using Error = AttEuler::Error; + + SEP_TRACE_PARSING("Processing AttEuler SBF message"); + _current_interval_messages.att_euler = true; + + AttEuler att_euler; + + if (_sbf_decoder.parse(&att_euler) == PX4_OK && + !att_euler.error_not_requested && + att_euler.error_aux1 == Error::None && + att_euler.error_aux2 == Error::None && + att_euler.heading > k_dnu_f4_value) { + 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; + } + + _message_gps_state.heading = heading; + } + + break; + } + case BlockID::AttCovEuler: { + using Error = AttCovEuler::Error; + + SEP_TRACE_PARSING("Processing AttCovEuler SBF message"); + _current_interval_messages.att_cov_euler = true; + + AttCovEuler att_cov_euler; + + 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 && + att_cov_euler.cov_headhead > k_dnu_f4_value) { + _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] + } + + break; + } + } + + break; + } + case DecodingStatus::RTCMv3: { + SEP_TRACE_PARSING("Processing RTCMv3 message"); + publish_rtcm_corrections(_rtcm_decoder->message(), _rtcm_decoder->received_bytes()); + break; + } + } + + return result; +} + +bool SeptentrioDriver::send_message(const char *msg) +{ + PX4_DEBUG("Send MSG: %s", msg); + int length = strlen(msg); + + return (write(reinterpret_cast(msg), length) == length); +} + +bool SeptentrioDriver::send_message_and_wait_for_ack(const char *msg, const int timeout) +{ + if (!send_message(msg)) { + return false; + } + + // 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[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 { + int read_result = read(reinterpret_cast(buf), sizeof(buf), 50); + + if (read_result < 0) { + SEP_WARN("SBF read error"); + return false; + } + + 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 if (buf[i] == '$') { + // Special case makes sure we don't miss start of new response if that happened to be the character we weren't expecting next (e.g., `$R: ge$R: gecm`) + response_check_character = 1; + } else { + response_check_character = 0; + } + } + } while (timeout_time > hrt_absolute_time()); + + SEP_WARN("Response: timeout"); + return false; +} + +int SeptentrioDriver::receive(unsigned timeout) +{ + int ret = 0; + int handled = 0; + uint8_t buf[k_read_buffer_size]; + + // timeout additional to poll + hrt_abstime time_started = hrt_absolute_time(); + + while (true) { + // 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. + SEP_WARN("poll_or_read err"); + return -1; + + } else { + // Pass received bytes to the packet decoder. + for (int i = 0; i < ret; i++) { + handled |= parse_char(buf[i]); + } + } + + if (handled > 0) { + return handled; + } + + // abort after timeout if no useful packets received + if (time_started + timeout * 1000 < hrt_absolute_time()) { + PX4_DEBUG("timed out, returning"); + return -1; + } + } +} + +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) { + _current_interval_bytes_read += num_read; + if (should_dump_incoming()) { + dump_gps_data(buf, num_read, DataDirection::FromReceiver); + } + } + + return num_read; +} + +int SeptentrioDriver::poll_or_read(uint8_t *buf, size_t buf_length, int timeout) +{ + int read_timeout = math::min(k_max_receiver_read_timeout, timeout); + + return _uart.readAtLeast(buf, buf_length, math::min(k_min_receiver_read_bytes, buf_length), read_timeout); +} + +int SeptentrioDriver::write(const uint8_t* buf, size_t buf_length) +{ + ssize_t write_result = _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 SeptentrioDriver::initialize_communication_dump(DumpMode mode) +{ + if (mode == DumpMode::FromReceiver || mode == DumpMode::Both) { + _message_data_from_receiver = new gps_dump_s(); + + if (!_message_data_from_receiver) { + SEP_ERR("Failed to allocate incoming dump buffer"); + 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(); + + if (!_message_data_to_receiver) { + SEP_ERR("failed to allocated dump data"); + return PX4_ERROR; + } + + memset(_message_data_to_receiver, 0, sizeof(*_message_data_to_receiver)); + } + + if (mode != DumpMode::Disabled) { + _gps_dump_pub.advertise(); + } + + return PX4_OK; +} + +void SeptentrioDriver::reset_if_scheduled() +{ + ReceiverResetType reset_type = (ReceiverResetType)_scheduled_reset.load(); + + if (reset_type != ReceiverResetType::None) { + _scheduled_reset.store((int)ReceiverResetType::None); + int res = reset(reset_type); + + if (res == PX4_OK) { + SEP_INFO("Reset succeeded."); + } else { + SEP_INFO("Reset failed."); + } + } +} + +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 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; + gps_inject_data_s msg; + + // 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 < _gps_inject_data_sub.size(); instance++) { + const bool exists = _gps_inject_data_sub[instance].advertised(); + + if (exists) { + 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; + _selected_rtcm_instance = instance; + break; + } + } + } + } + } + + 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). + // 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. + const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH; + size_t num_injections = 0; + + do { + if (updated) { + num_injections++; + + // 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. + */ + write(msg.data, msg.len); + + ++_current_interval_rtcm_injections; + _last_rtcm_injection_time = hrt_absolute_time(); + } + } + + updated = _gps_inject_data_sub[_selected_rtcm_instance].update(&msg); + + } while (updated && num_injections < max_num_injections); +} + +void SeptentrioDriver::publish() +{ + _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(); + + _sensor_gps_pub.publish(_message_gps_state); + + if (_message_gps_state.spoofing_state != _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 = _message_gps_state.spoofing_state; + } + + if (_message_gps_state.jamming_state != _jamming_state) { + + 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 = _message_gps_state.jamming_state; + } +} + +void SeptentrioDriver::publish_satellite_info() +{ + if (_message_satellite_info) { + _satellite_info_pub.publish(*_message_satellite_info); + } +} + +bool SeptentrioDriver::first_gps_uorb_message_created() const +{ + return _message_gps_state.timestamp != 0; +} + +void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len) +{ + gps_inject_data_s gps_inject_data{}; + + gps_inject_data.timestamp = hrt_absolute_time(); + gps_inject_data.device_id = get_device_id(); + + size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0])); + + if (len > capacity) { + gps_inject_data.flags = 1; //LSB: 1=fragmented + + } else { + gps_inject_data.flags = 0; + } + + size_t written = 0; + + while (written < len) { + + gps_inject_data.len = len - written; + + if (gps_inject_data.len > capacity) { + gps_inject_data.len = capacity; + } + + memcpy(gps_inject_data.data, &data[written], gps_inject_data.len); + + _gps_inject_data_pub.publish(gps_inject_data); + + written = written + gps_inject_data.len; + } +} + +void SeptentrioDriver::dump_gps_data(const uint8_t *data, size_t len, DataDirection data_direction) +{ + 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; + + if (write_len > sizeof(dump_data->data) - dump_data->len) { + write_len = sizeof(dump_data->data) - dump_data->len; + } + + memcpy(dump_data->data + dump_data->len, data, write_len); + data += write_len; + dump_data->len += write_len; + len -= write_len; + + if (dump_data->len >= sizeof(dump_data->data)) { + if (data_direction == DataDirection::ToReceiver) { + dump_data->len |= 1 << 7; + } + + dump_data->timestamp = hrt_absolute_time(); + _gps_dump_pub.publish(*dump_data); + dump_data->len = 0; + } + } +} + +bool SeptentrioDriver::should_dump_incoming() const +{ + return _message_data_from_receiver != 0; +} + +bool SeptentrioDriver::should_dump_outgoing() const +{ + return _message_data_to_receiver != 0; +} + +void SeptentrioDriver::start_update_monitoring_interval() +{ + 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(); +} + +bool SeptentrioDriver::update_monitoring_interval_ended() const +{ + return current_monitoring_interval_duration() > k_update_monitoring_interval_duration; +} + +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 - real_time.tv_sec); + + 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 && receiver_configuration_healthy()) { + return true; + } + + return false; +} + +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 septentrio::SeptentrioDriver::main(argc, argv); +} diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h new file mode 100644 index 000000000000..ec203cd86186 --- /dev/null +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -0,0 +1,763 @@ +/**************************************************************************** + * + * 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 septentrio.h + * + * Septentrio GNSS receiver driver + * + * @author Matej Franceskin + * @author Seppe Geuens + * @author Thomas Frans +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "module.h" +#include "sbf/decoder.h" +#include "rtcm.h" + +using namespace time_literals; + +namespace septentrio +{ + +/** + * The parsed command line arguments for this module. + */ +struct ModuleArguments { + int baud_rate_main {0}; + int baud_rate_secondary {0}; + const char *device_path_main {nullptr}; + const char *device_path_secondary {nullptr}; +}; + +/** + * Which raw communication data to dump to the log file. +*/ +enum class DumpMode : int32_t { + Disabled = 0, + FromReceiver = 1, + ToReceiver = 2, + Both = 3, +}; + +/** + * Instance of the driver. +*/ +enum class Instance : uint8_t { + Main = 0, + Secondary, +}; + +/** + * 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. + */ + None, + + /** + * In hot start mode, the receiver was powered down only for a short time (4 hours or less), + * so that its ephemeris is still valid. Since the receiver doesn't need to download ephemeris + * again, this is the fastest startup method. + */ + Hot, + + /** + * In warm start mode, the receiver has approximate information for time, position, and coarse + * satellite position data (Almanac). In this mode, after power-up, the receiver normally needs + * to download ephemeris before it can calculate position and velocity data. + */ + Warm, + + /** + * In cold start mode, the receiver has no information from the last position at startup. + * Therefore, the receiver must search the full time and frequency space, and all possible + * satellite numbers. If a satellite signal is found, it is tracked to decode the ephemeris, + * whereas the other channels continue to search satellites. Once there is a sufficient number + * of satellites with valid ephemeris, the receiver can calculate position and velocity data. + */ + Cold +}; + +/** + * 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}; +}; + +/** + * 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: + 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[], Instance instance); + + /** + * @brief Secondary run trampoline to support two driver instances. + */ + static int run_trampoline_secondary(int argc, char *argv[]); + + /** @see ModuleBase */ + static SeptentrioDriver *instantiate(int argc, char *argv[]); + + static SeptentrioDriver *instantiate(int argc, char *argv[], Instance instance); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** + * @brief Reset the connected GPS receiver. + * + * @return `PX4_OK` on success, `PX4_ERROR` on otherwise + */ + int reset(ReceiverResetType type); + + /** + * @brief Force command input on the currently used port on the receiver. + * + * @return `PX4_OK` on success, `PX4_ERROR` otherwise + */ + int force_input(); + + /** + * Standard baud rates the driver can be started with. `0` means the driver matches baud rates but does not change them. + */ + static uint32_t k_supported_baud_rates[]; + + /** + * Default baud rate, used when the user requested an invalid baud rate. + */ + static uint32_t k_default_baud_rate; +private: + enum class State { + OpeningSerialPort, + DetectingBaudRate, + 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, + }; + + /** + * The result of trying to configure the receiver. + */ + enum class ConfigureResult : int32_t { + OK = 0, + FailedCompletely = 1 << 0, + NoLogging = 1 << 1, + }; + + /** + * Maximum timeout to wait for fast command acknowledgement by the receiver. + */ + static constexpr uint16_t k_receiver_ack_timeout_fast = 200; + + /** + * Maximum timeout to wait for slow command acknowledgement by the receiver. + * Might be the case for commands that send more output back as reply. + */ + static constexpr uint16_t k_receiver_ack_timeout_slow = 400; + + /** + * 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; + + /** + * uORB type to send messages to ground control stations. + */ + static orb_advert_t k_mavlink_log_pub; + + /** + * 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. + * + * @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(ReceiverResetType type); + + /** + * @brief Detect whether the receiver is running at the given baud rate. + * Does not preserve local baud rate! + * + * @param baud_rate The baud rate to check. + * @param force_input Choose whether the receiver forces input on the port. + * + * @return `true` if running at the baud rate, or `false` on error. + */ + bool detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input); + + /** + * @brief Try to detect the serial port used on the receiver side. + * + * @param port_name A string with a length of 5 to store the result + * + * @return `PX4_OK` on success, `PX4_ERROR` on error + */ + int detect_serial_port(char *const port_name); + + /** + * @brief Configure the attached receiver based on the user's parameters. + * + * 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 `ConfigureResult::OK` if configured, or error. + */ + ConfigureResult configure(); + + /** + * @brief Parse the next byte of a received message from the receiver. + * + * @return 0 = decoding, 1 = message handled, 2 = sat info message handled + */ + 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. + * + * @return -1 = error, 0 = ok, 1 = payload received completely + */ + int payload_rx_add(const uint8_t byte); + + /** + * @brief Parses incoming SBF blocks. + * + * @return bitfield: all 0 = no message handled, 1 = position handled, 2 = satellite info handled + */ + int payload_rx_done(); + + /** + * @brief Send a message. + * + * @return true on success, false on write error (errno set) + */ + [[nodiscard]] bool send_message(const char *msg); + + /** + * @brief Send a message and waits for acknowledge. + * + * @param msg The message to send to the receiver + * @param timeout The time before sending the message times out + * + * @return true on success, false on write error (errno set) or ack wait 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); + + /** + * @brief Read from the receiver. + * + * @param buf Data that is read + * @param buf_length Size of the buffer + * @param timeout Reading timeout + * + * @return 0 on nothing read or poll timeout, <0 on error and >0 on bytes read (nr of bytes) + */ + int read(uint8_t *buf, size_t buf_length, int timeout); + + /** + * @brief This is an abstraction for the poll on serial used. + * + * @param buf The read buffer + * @param buf_length Size of the read buffer + * @param timeout Read timeout in ms + * + * @return 0 on nothing read or poll timeout, <0 on error and >0 on bytes read (nr of bytes) + */ + int poll_or_read(uint8_t *buf, size_t buf_length, int timeout); + + /** + * @brief Write to the receiver. + * + * @param buf Data to be written + * @param buf_length Amount of bytes to be written + * + * @return the number of bytes written on success, or -1 otherwise + */ + int write(const uint8_t *buf, size_t buf_length); + + /** + * @brief Initialize uORB GPS logging and advertise the topic. + * + * @return `PX4_OK` on success, `PX4_ERROR` otherwise + */ + int initialize_communication_dump(DumpMode mode); + + /** + * @brief Reset the receiver if it was requested by the user. + */ + void reset_if_scheduled(); + + /** + * @brief Set the baudrate of the serial connection. + * + * @param baud The baud rate of the connection + * + * @return `PX4_OK` on success, `PX4_ERROR` on otherwise + */ + int set_baudrate(uint32_t baud); + + /** + * @brief Handle incoming messages on the "inject data" uORB topic and send them to the receiver. + */ + void handle_inject_data_topic(); + + /** + * @brief Send data to the receiver, such as RTCM injections. + * + * @param data The raw data to send to the device + * @param len The size of `data` + * + * @return `true` if all the data was written correctly, `false` otherwise + */ + inline bool inject_data(uint8_t *data, size_t len); + + /** + * @brief Publish new GPS data with uORB. + */ + void publish(); + + /** + * @brief Publish new GPS satellite data with uORB. + */ + void publish_satellite_info(); + + /** + * @brief Check whether the driver has created its first complete `SensorGPS` uORB message. + * + * @return `true` if the driver has created its first complete `SensorGPS` uORB message, `false` if still waiting. + */ + bool first_gps_uorb_message_created() const; + + /** + * @brief Publish RTCM corrections. + * + * @param data: The raw data to publish + * @param len: The size of `data` + */ + void publish_rtcm_corrections(uint8_t *data, size_t len); + + /** + * @brief Dump gps communication. + * + * @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, DataDirection data_direction); + + /** + * @brief Check whether we should dump incoming data. + * + * @return `true` when we should dump incoming data, `false` otherwise. + */ + bool should_dump_incoming() const; + + /** + * @brief Check whether we should dump outgoing data. + * + * @return `true` when we should dump outgoing data, `false` otherwise. + */ + bool should_dump_outgoing() const; + + /** + * @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; + + /** + * @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. + * + * This is checked by passively monitoring output from the receiver and validating whether it is what is + * expected. + * + * @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. + */ + static void set_clock(timespec rtc_gps_time); + + /** + * @brief Check whether the driver is operating correctly. + * + * The driver is operating correctly when it has fully configured the receiver and is actively receiving all the + * expected data. + * + * @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 _chosen_baud_rate {0}; ///< The baud rate requested by the user + 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 + uint8_t _current_baud_rate_index {0}; ///< Index of the current baud rate to check + + // 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 + 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 new file mode 100644 index 000000000000..34bf02f58b89 --- /dev/null +++ b/src/drivers/gnss/septentrio/util.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * 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 util.cpp + * + * @author Thomas Frans +*/ + +#include "util.h" + +namespace septentrio +{ + +uint16_t buffer_crc16(const uint8_t *data_p, uint32_t length) +{ + uint8_t x; + uint16_t crc = 0; + + while (length--) { + x = crc >> 8 ^ *data_p++; + x ^= x >> 4; + crc = static_cast((crc << 8) ^ (x << 12) ^ (x << 5) ^ x); + } + + return crc; +} + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/util.h b/src/drivers/gnss/septentrio/util.h new file mode 100644 index 000000000000..e9de9af50dff --- /dev/null +++ b/src/drivers/gnss/septentrio/util.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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 util.h + * + * @author Thomas Frans + */ + +#pragma once + +namespace septentrio +{ + +/** + * @brief Calculate buffer CRC16 + */ +uint16_t buffer_crc16(const uint8_t *data_p, uint32_t length); + +} // namespace septentrio diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index 9796d24fb3bc..74ee827283c3 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -51,9 +51,8 @@ px4_add_module( devices/src/nmea.cpp devices/src/unicore.cpp devices/src/crc.cpp - devices/src/sbf.cpp MODULE_CONFIG module.yaml DEPENDS git_gps_devices - ) + ) \ No newline at end of file diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fe6f82ec7fbc..dff88ecbdc27 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -73,7 +73,6 @@ # include "devices/src/mtk.h" # include "devices/src/femtomes.h" # include "devices/src/nmea.h" -# include "devices/src/sbf.h" #endif // CONSTRAINED_FLASH #include "devices/src/ubx.h" @@ -97,7 +96,6 @@ enum class gps_driver_mode_t { EMLIDREACH, FEMTOMES, NMEA, - SBF }; enum class gps_dump_comm_mode_t : int32_t { @@ -361,8 +359,6 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac case 5: _mode = gps_driver_mode_t::FEMTOMES; break; case 6: _mode = gps_driver_mode_t::NMEA; break; - - case 7: _mode = gps_driver_mode_t::SBF; break; #endif // CONSTRAINED_FLASH } } @@ -706,13 +702,6 @@ GPS::run() heading_offset = matrix::wrap_pi(math::radians(heading_offset)); } - handle = param_find("GPS_PITCH_OFFSET"); - float pitch_offset = 0.f; - - if (handle != PARAM_INVALID) { - param_get(handle, &pitch_offset); - } - int32_t gps_ubx_dynmodel = 7; // default to 7: airborne with <2g acceleration handle = param_find("GPS_UBX_DYNMODEL"); @@ -728,7 +717,8 @@ GPS::run() int32_t gps_ubx_mode = 0; param_get(handle, &gps_ubx_mode); - if (gps_ubx_mode == 1) { // heading + switch (gps_ubx_mode) { + case 1: // heading if (_instance == Instance::Main) { ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBase; @@ -736,10 +726,13 @@ GPS::run() ubx_mode = GPSDriverUBX::UBXMode::MovingBase; } - } else if (gps_ubx_mode == 2) { + break; + + case 2: ubx_mode = GPSDriverUBX::UBXMode::MovingBase; + break; - } else if (gps_ubx_mode == 3) { + case 3: if (_instance == Instance::Main) { ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1; @@ -747,11 +740,18 @@ GPS::run() ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1; } - } else if (gps_ubx_mode == 4) { + break; + + case 4: ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1; + break; - } else if (gps_ubx_mode == 5) { // rover with static base on Uart2 + case 5: // rover with static base on Uart2 ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUart2; + break; + + default: + break; } } @@ -875,11 +875,6 @@ GPS::run() _helper = new GPSDriverNMEA(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset); set_device_type(DRV_GPS_DEVTYPE_NMEA); break; - - case gps_driver_mode_t::SBF: - _helper = new GPSDriverSBF(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset, pitch_offset); - set_device_type(DRV_GPS_DEVTYPE_SBF); - break; #endif // CONSTRAINED_FLASH default: @@ -1056,11 +1051,8 @@ GPS::run() break; case gps_driver_mode_t::FEMTOMES: - _mode = gps_driver_mode_t::SBF; - break; - - case gps_driver_mode_t::SBF: case gps_driver_mode_t::NMEA: // skip NMEA for auto-detection to avoid false positive matching + #endif // CONSTRAINED_FLASH _mode = gps_driver_mode_t::UBX; px4_usleep(500000); // tried all possible drivers. Wait a bit before next round @@ -1120,10 +1112,6 @@ GPS::print_status() case gps_driver_mode_t::NMEA: PX4_INFO("protocol: NMEA"); - break; - - case gps_driver_mode_t::SBF: - PX4_INFO("protocol: SBF"); #endif // CONSTRAINED_FLASH default: @@ -1496,8 +1484,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) } else if (!strcmp(myoptarg, "nmea")) { mode = gps_driver_mode_t::NMEA; - } else if (!strcmp(myoptarg, "sbf")) { - mode = gps_driver_mode_t::SBF; #endif // CONSTRAINED_FLASH } else { PX4_ERR("unknown protocol: %s", myoptarg); @@ -1560,4 +1546,4 @@ int gps_main(int argc, char *argv[]) { return GPS::main(argc, argv); -} \ No newline at end of file +} diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c index 49b4da7065bf..5e5adbcfa9cd 100644 --- a/src/drivers/gps/params.c +++ b/src/drivers/gps/params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -152,8 +152,9 @@ PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0); * * The offset angle increases clockwise. * - * Set this to 90 if the rover (or Unicore primary) antenna is placed on the - * right side of the vehicle and the moving base antenna is on the left side. + * Set this to 90 if the rover (or Unicore primary, or Septentrio Mosaic Aux) + * antenna is placed on the right side of the vehicle and the moving base + * antenna is on the left side. * * (Note: the Unicore primary antenna is the one connected on the right as seen * from the top). @@ -168,24 +169,6 @@ PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0); */ PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f); -/** - * Pitch offset for dual antenna GPS - * - * Vertical offsets can be compensated for by adjusting the Pitch offset (Septentrio). - * - * Note that this can be interpreted as the "roll" angle in case the antennas are aligned along the perpendicular axis. This occurs in situations where the two antenna ARPs may not be exactly at the same height in the vehicle reference frame. Since pitch is defined as the right-handed rotation about the vehicle Y axis, a situation where the main antenna is mounted lower than the aux antenna (assuming the default antenna setup) will result in a positive pitch. - * - * - * @min -90 - * @max 90 - * @unit deg - * @reboot_required true - * @decimal 3 - * - * @group GPS - */ -PARAM_DEFINE_FLOAT(GPS_PITCH_OFFSET, 0.f); - /** * Protocol for Main GPS * @@ -202,7 +185,6 @@ PARAM_DEFINE_FLOAT(GPS_PITCH_OFFSET, 0.f); * @value 4 Emlid Reach * @value 5 Femtomes * @value 6 NMEA (generic) - * @value 7 Septentrio (SBF) * * @reboot_required true * @group GPS @@ -247,14 +229,16 @@ PARAM_DEFINE_INT32(GPS_2_PROTOCOL, 1); * 2 : Use Galileo * 3 : Use BeiDou * 4 : Use GLONASS + * 5 : Use NAVIC * * @min 0 - * @max 31 + * @max 63 * @bit 0 GPS (with QZSS) * @bit 1 SBAS * @bit 2 Galileo * @bit 3 BeiDou * @bit 4 GLONASS + * @bit 5 NAVIC * * @reboot_required true * @group GPS @@ -277,16 +261,18 @@ PARAM_DEFINE_INT32(GPS_1_GNSS, 0); * 2 : Use Galileo * 3 : Use BeiDou * 4 : Use GLONASS + * 5 : Use NAVIC * * @min 0 - * @max 31 + * @max 63 * @bit 0 GPS (with QZSS) * @bit 1 SBAS * @bit 2 Galileo * @bit 3 BeiDou * @bit 4 GLONASS + * @bit 5 NAVIC * * @reboot_required true * @group GPS */ -PARAM_DEFINE_INT32(GPS_2_GNSS, 0); +PARAM_DEFINE_INT32(GPS_2_GNSS, 0); \ No newline at end of file