fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/gps: extract Septentrio into new standalone drivers/gnss/septentrio module (#22904)

Browse Source 
Having a generic interface over the GPS drivers makes dedicated
functionality for each driver harder. Move the Septentrio driver into
its own module under the `gnss` driver directory, and let it have its
own parameters for only the functionality it requires. This also helps
with adding new features because they only need to be implemented for
the driver that wants it, simplifying testing.
This commit is contained in:
Thomas Frans 2024-06-17 18:25:24 +02:00 committed by GitHub
parent f9160853fa
commit cd4c495377
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
27 changed files with 4028 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/cuav/nora/default.px4board
View File

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

1
boards/cubepilot/cubeorange/default.px4board
View File

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

1
boards/cubepilot/cubeorangeplus/default.px4board
View File

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

1
boards/mro/pixracerpro/default.px4board
View File

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

1
boards/px4/fmu-v2/default.px4board
View File

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

1
boards/px4/fmu-v3/default.px4board
View File

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

1
boards/px4/fmu-v4/default.px4board
View File

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

12
msg/GpsDump.msg
View File

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

9
msg/SensorGps.msg
View File

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

1
src/drivers/gnss/Kconfig Normal file
View File

@ -0,0 +1 @@
rsource "*/Kconfig"

50
src/drivers/gnss/septentrio/CMakeLists.txt Normal file
View File

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

5
src/drivers/gnss/septentrio/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_GNSS_SEPTENTRIO
bool "Septentrio GNSS receivers"
default n
---help---
Enable support for Septentrio receivers

6
src/drivers/gnss/septentrio/README.md Normal file
View File

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

80
src/drivers/gnss/septentrio/module.h Normal file
View File

@ -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 <px4_platform_common/log.h>
#ifdef DEBUG_BUILD
#ifndef SEP_LOG_ERROR
#define SEP_LOG_ERROR
#endif
#ifndef SEP_LOG_WARN
#define SEP_LOG_WARN
#endif
#ifndef SEP_LOG_INFO
#define SEP_LOG_INFO
#endif
#endif
#ifdef SEP_LOG_ERROR
#define SEP_ERR(...) {PX4_WARN(__VA_ARGS__);}
#else
#define SEP_ERR(...) {}
#endif
#ifdef SEP_LOG_WARN
#define SEP_WARN(...) {PX4_WARN(__VA_ARGS__);}
#else
#define SEP_WARN(...) {}
#endif
#ifdef SEP_LOG_INFO
#define SEP_INFO(...) {PX4_INFO(__VA_ARGS__);}
#else
#define SEP_INFO(...) {}
#endif
#ifdef SEP_LOG_TRACE_PARSING
#define SEP_TRACE_PARSING(...) {PX4_DEBUG(__VA_ARGS__);}
#else
#define SEP_TRACE_PARSING(...) {}
#endif

205
src/drivers/gnss/septentrio/module.yaml Normal file
View File

@ -0,0 +1,205 @@
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: 0
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: |
Dump raw communication data from and to the connected receiver to the log file.
type: enum
default: 0
min: 0
max: 3
values:
0: Disabled
1: From receiver
2: To receiver
3: Both
SEP_AUTO_CONFIG:
description:
short: Toggle automatic receiver configuration
long: |
By default, the receiver is automatically configured. Sometimes it may be used for multiple purposes.
If the offered parameters aren't sufficient, this parameter can be disabled to have full control of the receiver configuration.
A good way to use this is to enable automatic configuration, let the receiver be configured, and then disable it to make manual adjustments.
type: boolean
default: true
reboot_required: true
SEP_CONST_USAGE:
description:
short: Usage of different constellations
long: |
Choice of which constellations the receiver should use for PVT computation.
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

162
src/drivers/gnss/septentrio/rtcm.cpp Normal file
View File

@ -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 <cstring>
#include <px4_platform_common/log.h>
#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<uint16_t>(_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

132
src/drivers/gnss/septentrio/rtcm.h Normal file
View File

@ -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 <cstdint>
#include <math.h>
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

252
src/drivers/gnss/septentrio/sbf/decoder.cpp Normal file
View File

@ -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 <cstring>
#include <mathlib/math/Limits.hpp>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
#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<uint8_t *>(&_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<const uint8_t *>(&_message) + 4, _message.header.length - 4);
}
} // namespace sbf
} // namespace septentrio

233
src/drivers/gnss/septentrio/sbf/decoder.h Normal file
View File

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

353
src/drivers/gnss/septentrio/sbf/messages.h Normal file
View File

@ -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 <cstddef>
#include <cstdint>
namespace septentrio
{
namespace sbf
{
constexpr char k_sync1 {'$'};
constexpr char k_sync2 {'@'};
// Do-Not-Use values for fields in SBF blocks. The receiver can set certain fields to these values to signal that they are invalid.
// Most fields of a certain type will use these values (u2 representing a value often has DNU value of 65535).
// For the ones that do not use these, custom ones can be specified together with the blocks.
constexpr uint32_t k_dnu_u4_value {4294967295};
constexpr uint32_t k_dnu_u4_bitfield {0};
constexpr uint16_t k_dnu_u2_value {65535};
constexpr uint16_t k_dnu_u2_bitfield {0};
constexpr float k_dnu_f4_value {-2 * 10000000000};
constexpr double k_dnu_f8_value {-2 * 10000000000};
/// Maximum size of all expected messages.
/// This needs to be bigger than the maximum size of all declared SBF blocks so that `memcpy()` can safely copy from the decoding buffer using this value into messages.
constexpr size_t k_max_message_size {300};
/// Maximum expected number of sub-blocks.
constexpr uint8_t k_max_quality_indicators {9};
constexpr uint8_t k_max_rfband_blocks {4};
/**
* IDs of all the available SBF messages that we care about.
*/
enum class BlockID : uint16_t {
Invalid = 0,
DOP = 4001,
PVTGeodetic = 4007,
ReceiverStatus = 4014,
QualityInd = 4082,
RFStatus = 4092,
GALAuthStatus = 4245,
VelCovGeodetic = 5908,
EndOfPVT = 5921,
GEOIonoDelay = 5933,
AttEuler = 5938,
AttCovEuler = 5939,
};
#pragma pack(push, 1)
/**
* Common SBF message header.
*/
struct Header {
uint8_t sync1;
uint8_t sync2;
uint16_t crc;
BlockID id_number: 13;
uint16_t id_revision: 3;
uint16_t length;
uint32_t tow;
uint16_t wnc;
};
struct DOP {
uint8_t nr_sv;
uint8_t reserved;
uint16_t p_dop;
uint16_t t_dop;
uint16_t h_dop;
uint16_t v_dop;
float hpl;
float vpl;
};
struct PVTGeodetic {
static constexpr uint8_t k_dnu_nr_sv {255};
enum class ModeType : uint8_t {
NoPVT = 0,
StandAlonePVT = 1,
DifferentialPVT = 2,
FixedLocation = 3,
RTKFixed = 4,
RTKFloat = 5,
PVTWithSBAS = 6,
MovingBaseRTKFixed = 7,
MovingBaseRTKFloat = 8,
PrecisePointPositioning = 10,
};
enum class Error : uint8_t {
None = 0,
InsufficientMeasurements = 1,
InsufficientEphemerides = 2,
DOPTooLarge = 3,
SquaredResidualSumTooLarge = 4,
NoConvergence = 5,
InsufficientMeasurementsAfterOutlierRejection = 6,
ExportLawsForbidPositionOutput = 7,
InsufficientDifferentialCorrections = 8,
MissingBaseStationCoordinates = 9,
MissingRequiredRTKFixedPosition = 10,
};
ModeType mode_type: 4;
uint8_t mode_reserved: 2;
uint8_t mode_base_fixed: 1;
uint8_t mode_2d: 1;
Error error;
double latitude;
double longitude;
double height;
float undulation;
float vn;
float ve;
float vu;
float cog;
double rx_clk_bias;
float rx_clk_drift;
uint8_t time_system;
uint8_t datum;
uint8_t nr_sv;
uint8_t wa_corr_info;
uint16_t reference_id;
uint16_t mean_corr_age;
uint32_t signal_info;
uint8_t alert_flag;
uint8_t nr_bases;
uint16_t ppp_info;
uint16_t latency;
uint16_t h_accuracy;
uint16_t v_accuracy;
};
struct ReceiverStatus {
uint8_t cpu_load;
uint8_t ext_error_siserror: 1;
uint8_t ext_error_diff_corr_error: 1;
uint8_t ext_error_ext_sensor_error: 1;
uint8_t ext_error_setup_error: 1;
uint8_t ext_error_reserved: 4;
uint32_t uptime;
uint32_t rx_state_reserved1: 1;
uint32_t rx_state_active_antenna: 1;
uint32_t rx_state_ext_freq: 1;
uint32_t rx_state_ext_time: 1;
uint32_t rx_state_wn_set: 1;
uint32_t rx_state_tow_set: 1;
uint32_t rx_state_fine_time: 1;
uint32_t rx_state_internal_disk_activity: 1;
uint32_t rx_state_internal_disk_full: 1;
uint32_t rx_state_internal_disk_mounted: 1;
uint32_t rx_state_int_ant: 1;
uint32_t rx_state_refout_locked: 1;
uint32_t rx_state_reserved2: 1;
uint32_t rx_state_external_disk_activity: 1;
uint32_t rx_state_external_disk_full: 1;
uint32_t rx_state_external_disk_mounted: 1;
uint32_t rx_state_pps_in_cal: 1;
uint32_t rx_state_diff_corr_in: 1;
uint32_t rx_state_internet: 1;
uint32_t rx_state_reserved3: 13;
uint32_t rx_error_reserved1: 3;
uint32_t rx_error_software: 1;
uint32_t rx_error_watchdog: 1;
uint32_t rx_error_antenna: 1;
uint32_t rx_error_congestion: 1;
uint32_t rx_error_reserved2: 1;
uint32_t rx_error_missed_event: 1;
uint32_t rx_error_cpu_overload: 1;
uint32_t rx_error_invalid_config: 1;
uint32_t rx_error_out_of_geofence: 1;
uint32_t rx_error_reserved3: 22;
uint8_t n;
uint8_t sb_length;
uint8_t cmd_count;
uint8_t temperature;
};
struct QualityIndicator {
enum class Type : uint8_t {
Overall = 0,
GNSSSignalsMainAntenna = 1,
GNSSSignalsAuxAntenna = 2,
RFPowerMainAntenna = 11,
RFPowerAuxAntenna = 12,
CPUHeadroom = 21,
OCXOStability = 25,
BaseStationMeasurements = 30,
RTKPostProcessing = 31,
};
Type type: 8;
uint16_t value: 4;
uint16_t reserved: 4;
};
struct QualityInd {
uint8_t n;
uint8_t reserved1;
// Only support the currently available indicators for now so we don't have to allocate.
QualityIndicator indicators[k_max_quality_indicators];
};
struct RFBand {
uint32_t frequency;
uint16_t bandwidth;
uint8_t info_mode: 4;
uint8_t info_reserved: 2;
uint8_t info_antenna_id: 2;
};
struct RFStatus {
uint8_t n;
uint8_t sb_length;
uint8_t flags_inauthentic_gnss_signals: 1;
uint8_t flags_inauthentic_navigation_message: 1;
uint8_t flags_reserved: 6;
uint8_t reserved[3];
RFBand rf_band[k_max_rfband_blocks];
};
struct GALAuthStatus {
uint16_t osnma_status_status: 3;
uint16_t osnma_status_initialization_progress: 8;
uint16_t osnma_status_trusted_time_source: 3;
uint16_t osnma_status_merkle_tree_busy: 1;
uint16_t osnma_status_reserved: 1;
float trusted_time_delta;
uint64_t gal_active_mask;
uint64_t gal_authentic_mask;
uint64_t gps_active_mask;
uint64_t gps_authentic_mask;
};
struct VelCovGeodetic {
uint8_t mode_type: 4;
uint8_t mode_reserved: 2;
uint8_t mode_base_fixed: 1;
uint8_t mode_2d: 1;
uint8_t error;
float cov_vn_vn;
float cov_ve_ve;
float cov_vu_vu;
float cov_dt_dt;
float cov_vn_ve;
float cov_vn_vu;
float cov_vn_dt;
float cov_ve_vu;
float cov_ve_dt;
float cov_vu_dt;
};
struct IDC {
uint8_t igp_mask_no;
uint8_t givei;
uint8_t reserved[2];
float vertical_delay;
};
struct GEOIonoDelay {
uint8_t prn;
uint8_t bandnbr;
uint8_t iodi;
uint8_t n;
uint8_t sb_length;
uint8_t reserved;
IDC idc[4];
};
struct AttEuler {
enum class Error : uint8_t {
None = 0,
InsufficientMeasurements = 1,
};
uint8_t nr_sv;
Error error_aux1: 2;
Error error_aux2: 2;
uint8_t error_reserved: 3;
uint8_t error_not_requested: 1;
uint16_t mode;
uint16_t reserved;
float heading;
float pitch;
float roll;
float pitch_dot;
float roll_dot;
float heading_dot;
};
struct AttCovEuler {
enum class Error : uint8_t {
None = 0,
InsufficientMeasurements = 1,
};
uint8_t reserved;
Error error_aux1: 2;
Error error_aux2: 2;
uint8_t error_reserved: 3;
uint8_t error_not_requested: 1;
float cov_headhead;
float cov_pitchpitch;
float cov_rollroll;
float cov_headpitch;
float cov_headroll;
float cov_pitchroll;
};
#pragma pack(pop)
} // namespace sbf
} // namespace septentrio

1702
src/drivers/gnss/septentrio/septentrio.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

715
src/drivers/gnss/septentrio/septentrio.h Normal file
View File

@ -0,0 +1,715 @@
/****************************************************************************
*
* 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 <Matej.Franceskin@gmail.com>
* @author <a href="https://github.com/SeppeG">Seppe Geuens</a>
* @author Thomas Frans
*/
#pragma once
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/Serial.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/satellite_info.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/gps_dump.h>
#include <uORB/topics/gps_inject_data.h>
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/parameters/param.h>
#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<SeptentrioDriver>, 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();
private:
enum class State {
OpeningSerialPort,
ConfiguringDevice,
ReceivingData,
};
/**
* The current decoder that data has to be fed to.
*/
enum class DecodingStatus {
Searching,
SBF,
RTCMv3,
};
enum class ReceiveResult {
ReadError,
Timeout,
Receiving,
MessageAvailable,
};
/**
* Maximum timeout to wait for command acknowledgement by the receiver.
*/
static constexpr uint16_t k_receiver_ack_timeout = 200;
/**
* Duration of one update monitoring interval in us.
* This should be longer than the time it takes for all *positioning* SBF messages to be sent once by the receiver!
* Otherwise the driver will assume the receiver configuration isn't healthy because it doesn't see all blocks in time.
*/
static constexpr hrt_abstime k_update_monitoring_interval_duration = 5_s;
/**
* 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 the current baud rate used by the receiver on the connected port.
*
* @param force_input Choose whether the receiver forces input on the port
*
* @return The detected baud rate on success, or `0` on error
*/
uint32_t detect_receiver_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 `PX4_OK` on success, `PX4_ERROR` otherwise.
*/
int 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 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.
*
* @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.
*
* @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<typename T>
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<int> _scheduled_reset {static_cast<int>(ReceiverResetType::None)}; ///< The type of receiver reset that is scheduled
DumpMode _dump_communication_mode {DumpMode::Disabled}; ///< GPS communication dump mode
device::Serial _uart {}; ///< Serial UART port for communication with the receiver
char _port[20] {}; ///< The path of the used serial device
hrt_abstime _last_rtcm_injection_time {0}; ///< Time of last RTCM injection
uint8_t _selected_rtcm_instance {0}; ///< uORB instance that is being used for RTCM corrections
uint8_t _spoofing_state {0}; ///< Receiver spoofing state
uint8_t _jamming_state {0}; ///< Receiver jamming state
const Instance _instance {Instance::Main}; ///< The receiver that this instance of the driver controls
uint32_t _baud_rate {0};
static px4::atomic<SeptentrioDriver *> _secondary_instance;
hrt_abstime _sleep_end {0}; ///< End time for sleeping
State _resume_state {State::OpeningSerialPort}; ///< Resume state after sleep
// Module configuration
float _heading_offset {0.0f}; ///< The heading offset given by the `SEP_YAW_OFFS` parameter
float _pitch_offset {0.0f}; ///< The pitch offset given by the `SEP_PITCH_OFFS` parameter
uint32_t _receiver_stream_main {k_default_main_stream}; ///< The main output stream for the receiver given by the `SEP_STREAM_MAIN` parameter
uint32_t _receiver_stream_log {k_default_log_stream}; ///< The log output stream for the receiver given by the `SEP_STREAM_LOG` parameter
SBFOutputFrequency _sbf_output_frequency {SBFOutputFrequency::Hz5_0}; ///< Output frequency of the main SBF blocks given by the `SEP_OUTP_HZ` parameter
ReceiverLogFrequency _receiver_logging_frequency {ReceiverLogFrequency::Hz1_0}; ///< Logging frequency of the receiver given by the `SEP_LOG_HZ` parameter
ReceiverLogLevel _receiver_logging_level {ReceiverLogLevel::Default}; ///< Logging level of the receiver given by the `SEP_LOG_LEVEL` parameter
bool _receiver_logging_overwrite {0}; ///< Logging overwrite behavior, given by the `SEP_LOG_FORCE` parameter
bool _automatic_configuration {true}; ///< Automatic configuration of the receiver given by the `SEP_AUTO_CONFIG` parameter
ReceiverSetup _receiver_setup {ReceiverSetup::Default}; ///< Purpose of the receivers, given by the `SEP_HARDW_SETUP` parameter
int32_t _receiver_constellation_usage {0}; ///< Constellation usage in PVT computation given by the `SEP_CONST_USAGE` parameter
// Decoding and parsing
DecodingStatus _active_decoder {DecodingStatus::Searching}; ///< Currently active decoder that new data has to be fed into
sbf::Decoder _sbf_decoder {}; ///< SBF message decoder
rtcm::Decoder *_rtcm_decoder {nullptr}; ///< RTCM message decoder
// uORB topics and subscriptions
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_s> _sensor_gps_pub {ORB_ID(sensor_gps)}; ///< uORB publication for gps position
uORB::Publication<gps_dump_s> _gps_dump_pub {ORB_ID(gps_dump)}; ///< uORB publication for dump GPS data
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub {ORB_ID(gps_inject_data)}; ///< uORB publication for injected data to the receiver
uORB::PublicationMulti<satellite_info_s> _satellite_info_pub {ORB_ID(satellite_info)}; ///< uORB publication for satellite info
uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _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

59
src/drivers/gnss/septentrio/util.cpp Normal file
View File

@ -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<uint16_t>((crc << 8) ^ (x << 12) ^ (x << 5) ^ x);
}
return crc;
}
} // namespace septentrio

50
src/drivers/gnss/septentrio/util.h Normal file
View File

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

3
src/drivers/gps/CMakeLists.txt
View File

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

27
src/drivers/gps/gps.cpp
View File

@ -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");
@ -886,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:
@ -1068,11 +1052,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
@ -1132,10 +1113,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:
@ -1508,8 +1485,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);

21
src/drivers/gps/params.c
View File

@ -169,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
*
@ -203,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
@ -294,4 +275,4 @@ PARAM_DEFINE_INT32(GPS_1_GNSS, 0);
* @reboot_required true
* @group GPS
*/
PARAM_DEFINE_INT32(GPS_2_GNSS, 0);
PARAM_DEFINE_INT32(GPS_2_GNSS, 0);