Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 13:02:25 +08:00
Code Issues Actions 4 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
27 changed files with 4028 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/cuav/nora/default.px4board
+1
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
boards/cubepilot/cubeorange/default.px4board
+1
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
boards/cubepilot/cubeorangeplus/default.px4board
+1
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
boards/mro/pixracerpro/default.px4board
+1
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
boards/px4/fmu-v2/default.px4board
+1
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
boards/px4/fmu-v3/default.px4board
+1
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
boards/px4/fmu-v4/default.px4board
+1
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
msg/GpsDump.msg
+5 -7
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
msg/SensorGps.msg
+8 -1
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)
src/drivers/gnss/Kconfig
+1
View File
@@ -0,0 +1 @@
rsource "*/Kconfig"
src/drivers/gnss/septentrio/CMakeLists.txt
+50
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
)
src/drivers/gnss/septentrio/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_GNSS_SEPTENTRIO
bool "Septentrio GNSS receivers"
default n
---help---
Enable support for Septentrio receivers
src/drivers/gnss/septentrio/README.md
+6
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.
src/drivers/gnss/septentrio/module.h
+80
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
src/drivers/gnss/septentrio/module.yaml
+205
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
src/drivers/gnss/septentrio/rtcm.cpp
+162
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
src/drivers/gnss/septentrio/rtcm.h
+132
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
src/drivers/gnss/septentrio/sbf/decoder.cpp
+252
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
src/drivers/gnss/septentrio/sbf/decoder.h
+233
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
src/drivers/gnss/septentrio/sbf/messages.h
+353
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
src/drivers/gnss/septentrio/septentrio.cpp
+1702
View File
File diff suppressed because it is too large Load Diff
src/drivers/gnss/septentrio/septentrio.h
+715
View File
File diff suppressed because it is too large Load Diff
src/drivers/gnss/septentrio/util.cpp
+59
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
src/drivers/gnss/septentrio/util.h
+50
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
src/drivers/gps/CMakeLists.txt
+1 -2
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
)
)
src/drivers/gps/gps.cpp
+1 -26
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);
src/drivers/gps/params.c
+1 -20
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);