mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-12 23:41:35 +08:00
Revert "drivers/gnss/septentrio : 1.15-compatible extracted septentrio driver, with new features and fixes (#23386)"
This reverts commit 4ea83484c17b064921ddd9574880ef0d997383d7.
This commit is contained in:
Ramon Roche
@@ -1 +0,0 @@
|
||||
rsource "*/Kconfig"
|
||||
@@ -1,50 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
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
|
||||
)
|
||||
@@ -1,5 +0,0 @@
|
||||
menuconfig DRIVERS_GNSS_SEPTENTRIO
|
||||
bool "Septentrio GNSS receivers"
|
||||
default n
|
||||
---help---
|
||||
Enable support for Septentrio receivers
|
||||
@@ -1,6 +0,0 @@
|
||||
# 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.
|
||||
@@ -1,80 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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_ERR(__VA_ARGS__);}
|
||||
#else
|
||||
#define SEP_ERR(...) {}
|
||||
#endif
|
||||
|
||||
#ifdef SEP_LOG_WARN
|
||||
#define SEP_WARN(...) {PX4_WARN(__VA_ARGS__);}
|
||||
#else
|
||||
#define SEP_WARN(...) {}
|
||||
#endif
|
||||
|
||||
#ifdef SEP_LOG_INFO
|
||||
#define SEP_INFO(...) {PX4_INFO(__VA_ARGS__);}
|
||||
#else
|
||||
#define SEP_INFO(...) {}
|
||||
#endif
|
||||
|
||||
#ifdef SEP_LOG_TRACE_PARSING
|
||||
#define SEP_TRACE_PARSING(...) {PX4_DEBUG(__VA_ARGS__);}
|
||||
#else
|
||||
#define SEP_TRACE_PARSING(...) {}
|
||||
#endif
|
||||
@@ -1,206 +0,0 @@
|
||||
module_name: Septentrio
|
||||
|
||||
serial_config:
|
||||
- command: set DUAL_GPS_ARGS "-e ${SERIAL_DEV} -g p:${BAUD_PARAM}"
|
||||
port_config_param:
|
||||
name: SEP_PORT2_CFG
|
||||
group: Septentrio
|
||||
label: Secondary GPS port
|
||||
- command: septentrio start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} ${DUAL_GPS_ARGS}
|
||||
port_config_param:
|
||||
name: SEP_PORT1_CFG
|
||||
group: Septentrio
|
||||
default: GPS1
|
||||
label: GPS Port
|
||||
|
||||
parameters:
|
||||
- group: Septentrio
|
||||
definitions:
|
||||
SEP_STREAM_MAIN:
|
||||
description:
|
||||
short: Main stream used during automatic configuration
|
||||
long: |
|
||||
The stream the autopilot sets up on the receiver to output the main data.
|
||||
|
||||
Set this to another value if the default stream is already used for another purpose.
|
||||
type: int32
|
||||
min: 1
|
||||
max: 10
|
||||
default: 1
|
||||
reboot_required: true
|
||||
SEP_STREAM_LOG:
|
||||
description:
|
||||
short: Logging stream used during automatic configuration
|
||||
long: |
|
||||
The stream the autopilot sets up on the receiver to output the logging data.
|
||||
|
||||
Set this to another value if the default stream is already used for another purpose.
|
||||
type: int32
|
||||
min: 1
|
||||
max: 10
|
||||
default: 2
|
||||
reboot_required: true
|
||||
SEP_OUTP_HZ:
|
||||
description:
|
||||
short: Output frequency of main SBF blocks
|
||||
long: |
|
||||
The output frequency of the main SBF blocks needed for PVT information.
|
||||
type: enum
|
||||
min: 0
|
||||
max: 3
|
||||
default: 1
|
||||
values:
|
||||
0: 5 Hz
|
||||
1: 10 Hz
|
||||
2: 20 Hz
|
||||
3: 25 Hz
|
||||
reboot_required: true
|
||||
SEP_YAW_OFFS:
|
||||
description:
|
||||
short: Heading/Yaw offset for dual antenna GPS
|
||||
long: |
|
||||
Heading offset angle for dual antenna GPS setups that support heading estimation.
|
||||
|
||||
Set this to 0 if the antennas are parallel to the forward-facing direction
|
||||
of the vehicle and the rover antenna is in front.
|
||||
|
||||
The offset angle increases clockwise.
|
||||
|
||||
Set this to 90 if the rover antenna is placed on the
|
||||
right side of the vehicle and the moving base antenna is on the left side.
|
||||
type: float
|
||||
decimal: 3
|
||||
default: 0
|
||||
min: -360
|
||||
max: 360
|
||||
unit: deg
|
||||
reboot_required: true
|
||||
SEP_SAT_INFO:
|
||||
description:
|
||||
short: Enable sat info
|
||||
long: |
|
||||
Enable publication of satellite info (ORB_ID(satellite_info)) if possible.
|
||||
type: boolean
|
||||
default: 0
|
||||
reboot_required: true
|
||||
SEP_PITCH_OFFS:
|
||||
description:
|
||||
short: Pitch offset for dual antenna GPS
|
||||
long: |
|
||||
Vertical offsets can be compensated for by adjusting the Pitch offset.
|
||||
|
||||
Note that this can be interpreted as the "roll" angle in case the antennas are aligned along the perpendicular axis.
|
||||
This occurs in situations where the two antenna ARPs may not be exactly at the same height in the vehicle reference frame.
|
||||
Since pitch is defined as the right-handed rotation about the vehicle Y axis,
|
||||
a situation where the main antenna is mounted lower than the aux antenna (assuming the default antenna setup) will result in a positive pitch.
|
||||
type: float
|
||||
decimal: 3
|
||||
default: 0
|
||||
min: -90
|
||||
max: 90
|
||||
unit: deg
|
||||
reboot_required: true
|
||||
SEP_DUMP_COMM:
|
||||
description:
|
||||
short: Log GPS communication data
|
||||
long: |
|
||||
Log raw communication between the driver and connected receivers.
|
||||
For example, "To receiver" will log all commands and corrections sent by the driver to the receiver.
|
||||
type: enum
|
||||
default: 0
|
||||
min: 0
|
||||
max: 3
|
||||
values:
|
||||
0: Disabled
|
||||
1: From receiver
|
||||
2: To receiver
|
||||
3: Both
|
||||
SEP_AUTO_CONFIG:
|
||||
description:
|
||||
short: Toggle automatic receiver configuration
|
||||
long: |
|
||||
By default, the receiver is automatically configured. Sometimes it may be used for multiple purposes.
|
||||
If the offered parameters aren't sufficient, this parameter can be disabled to have full control of the receiver configuration.
|
||||
A good way to use this is to enable automatic configuration, let the receiver be configured, and then disable it to make manual adjustments.
|
||||
type: boolean
|
||||
default: true
|
||||
reboot_required: true
|
||||
SEP_CONST_USAGE:
|
||||
description:
|
||||
short: Usage of different constellations
|
||||
long: |
|
||||
Choice of which constellations the receiver should use for PVT computation.
|
||||
|
||||
When this is 0, the constellation usage isn't changed.
|
||||
type: bitmask
|
||||
bit:
|
||||
0: GPS
|
||||
1: GLONASS
|
||||
2: Galileo
|
||||
3: SBAS
|
||||
4: BeiDou
|
||||
default: 0
|
||||
min: 0
|
||||
max: 63
|
||||
reboot_required: true
|
||||
SEP_LOG_HZ:
|
||||
description:
|
||||
short: Logging frequency for the receiver
|
||||
long: |
|
||||
Select the frequency at which the connected receiver should log data to its internal storage.
|
||||
type: enum
|
||||
default: 0
|
||||
min: 0
|
||||
max: 10
|
||||
values:
|
||||
0: Disabled
|
||||
1: 0.1 Hz
|
||||
2: 0.2 Hz
|
||||
3: 0.5 Hz
|
||||
4: 1 Hz
|
||||
5: 2 Hz
|
||||
6: 5 Hz
|
||||
7: 10 Hz
|
||||
8: 20 Hz
|
||||
9: 25 Hz
|
||||
10: 50 Hz
|
||||
reboot_required: true
|
||||
SEP_LOG_LEVEL:
|
||||
description:
|
||||
short: Logging level for the receiver
|
||||
long: |
|
||||
Select the level of detail that needs to be logged by the receiver.
|
||||
type: enum
|
||||
default: 2
|
||||
min: 0
|
||||
max: 3
|
||||
values:
|
||||
0: Lite
|
||||
1: Basic
|
||||
2: Default
|
||||
3: Full
|
||||
reboot_required: true
|
||||
SEP_LOG_FORCE:
|
||||
description:
|
||||
short: Whether to overwrite or add to existing logging
|
||||
long: |
|
||||
When the receiver is already set up to log data, this decides whether extra logged data should be added or overwrite existing data.
|
||||
type: boolean
|
||||
default: false
|
||||
reboot_required: true
|
||||
SEP_HARDW_SETUP:
|
||||
description:
|
||||
short: Setup and expected use of the hardware
|
||||
long: |
|
||||
Setup and expected use of the hardware.
|
||||
|
||||
- Default: Use two receivers as completely separate instances.
|
||||
- Moving base: Use two receivers in a rover & moving base setup for heading.
|
||||
type: enum
|
||||
default: 0
|
||||
min: 0
|
||||
max: 1
|
||||
values:
|
||||
0: Default
|
||||
1: Moving base
|
||||
reboot_required: true
|
||||
@@ -1,162 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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
|
||||
@@ -1,132 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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
|
||||
@@ -1,252 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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
|
||||
@@ -1,233 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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
|
||||
@@ -1,353 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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.0f * 10000000000.0f};
|
||||
constexpr double k_dnu_f8_value {-2.0 * 10000000000.0};
|
||||
|
||||
/// Maximum size of all expected messages.
|
||||
/// This needs to be bigger than the maximum size of all declared SBF blocks so that `memcpy()` can safely copy from the decoding buffer using this value into messages.
|
||||
constexpr size_t k_max_message_size {300};
|
||||
|
||||
/// Maximum expected number of sub-blocks.
|
||||
constexpr uint8_t k_max_quality_indicators {9};
|
||||
constexpr uint8_t k_max_rfband_blocks {4};
|
||||
|
||||
/**
|
||||
* IDs of all the available SBF messages that we care about.
|
||||
*/
|
||||
enum class BlockID : uint16_t {
|
||||
Invalid = 0,
|
||||
DOP = 4001,
|
||||
PVTGeodetic = 4007,
|
||||
ReceiverStatus = 4014,
|
||||
QualityInd = 4082,
|
||||
RFStatus = 4092,
|
||||
GALAuthStatus = 4245,
|
||||
VelCovGeodetic = 5908,
|
||||
EndOfPVT = 5921,
|
||||
GEOIonoDelay = 5933,
|
||||
AttEuler = 5938,
|
||||
AttCovEuler = 5939,
|
||||
};
|
||||
|
||||
#pragma pack(push, 1)
|
||||
|
||||
/**
|
||||
* Common SBF message header.
|
||||
*/
|
||||
struct Header {
|
||||
uint8_t sync1;
|
||||
uint8_t sync2;
|
||||
uint16_t crc;
|
||||
BlockID id_number: 13;
|
||||
uint16_t id_revision: 3;
|
||||
uint16_t length;
|
||||
uint32_t tow;
|
||||
uint16_t wnc;
|
||||
};
|
||||
|
||||
struct DOP {
|
||||
uint8_t nr_sv;
|
||||
uint8_t reserved;
|
||||
uint16_t p_dop;
|
||||
uint16_t t_dop;
|
||||
uint16_t h_dop;
|
||||
uint16_t v_dop;
|
||||
float hpl;
|
||||
float vpl;
|
||||
};
|
||||
|
||||
struct PVTGeodetic {
|
||||
static constexpr uint8_t k_dnu_nr_sv {255};
|
||||
enum class ModeType : uint8_t {
|
||||
NoPVT = 0,
|
||||
StandAlonePVT = 1,
|
||||
DifferentialPVT = 2,
|
||||
FixedLocation = 3,
|
||||
RTKFixed = 4,
|
||||
RTKFloat = 5,
|
||||
PVTWithSBAS = 6,
|
||||
MovingBaseRTKFixed = 7,
|
||||
MovingBaseRTKFloat = 8,
|
||||
PrecisePointPositioning = 10,
|
||||
};
|
||||
enum class Error : uint8_t {
|
||||
None = 0,
|
||||
InsufficientMeasurements = 1,
|
||||
InsufficientEphemerides = 2,
|
||||
DOPTooLarge = 3,
|
||||
SquaredResidualSumTooLarge = 4,
|
||||
NoConvergence = 5,
|
||||
InsufficientMeasurementsAfterOutlierRejection = 6,
|
||||
ExportLawsForbidPositionOutput = 7,
|
||||
InsufficientDifferentialCorrections = 8,
|
||||
MissingBaseStationCoordinates = 9,
|
||||
MissingRequiredRTKFixedPosition = 10,
|
||||
};
|
||||
ModeType mode_type: 4;
|
||||
uint8_t mode_reserved: 2;
|
||||
uint8_t mode_base_fixed: 1;
|
||||
uint8_t mode_2d: 1;
|
||||
Error error;
|
||||
double latitude;
|
||||
double longitude;
|
||||
double height;
|
||||
float undulation;
|
||||
float vn;
|
||||
float ve;
|
||||
float vu;
|
||||
float cog;
|
||||
double rx_clk_bias;
|
||||
float rx_clk_drift;
|
||||
uint8_t time_system;
|
||||
uint8_t datum;
|
||||
uint8_t nr_sv;
|
||||
uint8_t wa_corr_info;
|
||||
uint16_t reference_id;
|
||||
uint16_t mean_corr_age;
|
||||
uint32_t signal_info;
|
||||
uint8_t alert_flag;
|
||||
uint8_t nr_bases;
|
||||
uint16_t ppp_info;
|
||||
uint16_t latency;
|
||||
uint16_t h_accuracy;
|
||||
uint16_t v_accuracy;
|
||||
};
|
||||
|
||||
struct ReceiverStatus {
|
||||
uint8_t cpu_load;
|
||||
uint8_t ext_error_siserror: 1;
|
||||
uint8_t ext_error_diff_corr_error: 1;
|
||||
uint8_t ext_error_ext_sensor_error: 1;
|
||||
uint8_t ext_error_setup_error: 1;
|
||||
uint8_t ext_error_reserved: 4;
|
||||
uint32_t uptime;
|
||||
uint32_t rx_state_reserved1: 1;
|
||||
uint32_t rx_state_active_antenna: 1;
|
||||
uint32_t rx_state_ext_freq: 1;
|
||||
uint32_t rx_state_ext_time: 1;
|
||||
uint32_t rx_state_wn_set: 1;
|
||||
uint32_t rx_state_tow_set: 1;
|
||||
uint32_t rx_state_fine_time: 1;
|
||||
uint32_t rx_state_internal_disk_activity: 1;
|
||||
uint32_t rx_state_internal_disk_full: 1;
|
||||
uint32_t rx_state_internal_disk_mounted: 1;
|
||||
uint32_t rx_state_int_ant: 1;
|
||||
uint32_t rx_state_refout_locked: 1;
|
||||
uint32_t rx_state_reserved2: 1;
|
||||
uint32_t rx_state_external_disk_activity: 1;
|
||||
uint32_t rx_state_external_disk_full: 1;
|
||||
uint32_t rx_state_external_disk_mounted: 1;
|
||||
uint32_t rx_state_pps_in_cal: 1;
|
||||
uint32_t rx_state_diff_corr_in: 1;
|
||||
uint32_t rx_state_internet: 1;
|
||||
uint32_t rx_state_reserved3: 13;
|
||||
uint32_t rx_error_reserved1: 3;
|
||||
uint32_t rx_error_software: 1;
|
||||
uint32_t rx_error_watchdog: 1;
|
||||
uint32_t rx_error_antenna: 1;
|
||||
uint32_t rx_error_congestion: 1;
|
||||
uint32_t rx_error_reserved2: 1;
|
||||
uint32_t rx_error_missed_event: 1;
|
||||
uint32_t rx_error_cpu_overload: 1;
|
||||
uint32_t rx_error_invalid_config: 1;
|
||||
uint32_t rx_error_out_of_geofence: 1;
|
||||
uint32_t rx_error_reserved3: 22;
|
||||
uint8_t n;
|
||||
uint8_t sb_length;
|
||||
uint8_t cmd_count;
|
||||
uint8_t temperature;
|
||||
};
|
||||
|
||||
struct QualityIndicator {
|
||||
enum class Type : uint8_t {
|
||||
Overall = 0,
|
||||
GNSSSignalsMainAntenna = 1,
|
||||
GNSSSignalsAuxAntenna = 2,
|
||||
RFPowerMainAntenna = 11,
|
||||
RFPowerAuxAntenna = 12,
|
||||
CPUHeadroom = 21,
|
||||
OCXOStability = 25,
|
||||
BaseStationMeasurements = 30,
|
||||
RTKPostProcessing = 31,
|
||||
};
|
||||
Type type: 8;
|
||||
uint16_t value: 4;
|
||||
uint16_t reserved: 4;
|
||||
};
|
||||
|
||||
struct QualityInd {
|
||||
uint8_t n;
|
||||
uint8_t reserved1;
|
||||
// Only support the currently available indicators for now so we don't have to allocate.
|
||||
QualityIndicator indicators[k_max_quality_indicators];
|
||||
};
|
||||
|
||||
struct RFBand {
|
||||
uint32_t frequency;
|
||||
uint16_t bandwidth;
|
||||
uint8_t info_mode: 4;
|
||||
uint8_t info_reserved: 2;
|
||||
uint8_t info_antenna_id: 2;
|
||||
};
|
||||
|
||||
struct RFStatus {
|
||||
uint8_t n;
|
||||
uint8_t sb_length;
|
||||
uint8_t flags_inauthentic_gnss_signals: 1;
|
||||
uint8_t flags_inauthentic_navigation_message: 1;
|
||||
uint8_t flags_reserved: 6;
|
||||
uint8_t reserved[3];
|
||||
RFBand rf_band[k_max_rfband_blocks];
|
||||
};
|
||||
|
||||
struct GALAuthStatus {
|
||||
uint16_t osnma_status_status: 3;
|
||||
uint16_t osnma_status_initialization_progress: 8;
|
||||
uint16_t osnma_status_trusted_time_source: 3;
|
||||
uint16_t osnma_status_merkle_tree_busy: 1;
|
||||
uint16_t osnma_status_reserved: 1;
|
||||
float trusted_time_delta;
|
||||
uint64_t gal_active_mask;
|
||||
uint64_t gal_authentic_mask;
|
||||
uint64_t gps_active_mask;
|
||||
uint64_t gps_authentic_mask;
|
||||
};
|
||||
|
||||
struct VelCovGeodetic {
|
||||
uint8_t mode_type: 4;
|
||||
uint8_t mode_reserved: 2;
|
||||
uint8_t mode_base_fixed: 1;
|
||||
uint8_t mode_2d: 1;
|
||||
uint8_t error;
|
||||
float cov_vn_vn;
|
||||
float cov_ve_ve;
|
||||
float cov_vu_vu;
|
||||
float cov_dt_dt;
|
||||
float cov_vn_ve;
|
||||
float cov_vn_vu;
|
||||
float cov_vn_dt;
|
||||
float cov_ve_vu;
|
||||
float cov_ve_dt;
|
||||
float cov_vu_dt;
|
||||
};
|
||||
|
||||
struct IDC {
|
||||
uint8_t igp_mask_no;
|
||||
uint8_t givei;
|
||||
uint8_t reserved[2];
|
||||
float vertical_delay;
|
||||
};
|
||||
|
||||
struct GEOIonoDelay {
|
||||
uint8_t prn;
|
||||
uint8_t bandnbr;
|
||||
uint8_t iodi;
|
||||
uint8_t n;
|
||||
uint8_t sb_length;
|
||||
uint8_t reserved;
|
||||
IDC idc[4];
|
||||
};
|
||||
|
||||
struct AttEuler {
|
||||
enum class Error : uint8_t {
|
||||
None = 0,
|
||||
InsufficientMeasurements = 1,
|
||||
};
|
||||
uint8_t nr_sv;
|
||||
Error error_aux1: 2;
|
||||
Error error_aux2: 2;
|
||||
uint8_t error_reserved: 3;
|
||||
uint8_t error_not_requested: 1;
|
||||
uint16_t mode;
|
||||
uint16_t reserved;
|
||||
float heading;
|
||||
float pitch;
|
||||
float roll;
|
||||
float pitch_dot;
|
||||
float roll_dot;
|
||||
float heading_dot;
|
||||
};
|
||||
|
||||
struct AttCovEuler {
|
||||
enum class Error : uint8_t {
|
||||
None = 0,
|
||||
InsufficientMeasurements = 1,
|
||||
};
|
||||
uint8_t reserved;
|
||||
Error error_aux1: 2;
|
||||
Error error_aux2: 2;
|
||||
uint8_t error_reserved: 3;
|
||||
uint8_t error_not_requested: 1;
|
||||
float cov_headhead;
|
||||
float cov_pitchpitch;
|
||||
float cov_rollroll;
|
||||
float cov_headpitch;
|
||||
float cov_headroll;
|
||||
float cov_pitchroll;
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
} // namespace sbf
|
||||
|
||||
} // namespace septentrio
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,763 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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/uORB.h>
|
||||
#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();
|
||||
|
||||
/**
|
||||
* Standard baud rates the driver can be started with. `0` means the driver matches baud rates but does not change them.
|
||||
*/
|
||||
static uint32_t k_supported_baud_rates[];
|
||||
|
||||
/**
|
||||
* Default baud rate, used when the user requested an invalid baud rate.
|
||||
*/
|
||||
static uint32_t k_default_baud_rate;
|
||||
private:
|
||||
enum class State {
|
||||
OpeningSerialPort,
|
||||
DetectingBaudRate,
|
||||
ConfiguringDevice,
|
||||
ReceivingData,
|
||||
};
|
||||
|
||||
/**
|
||||
* The current decoder that data has to be fed to.
|
||||
*/
|
||||
enum class DecodingStatus {
|
||||
Searching,
|
||||
SBF,
|
||||
RTCMv3,
|
||||
};
|
||||
|
||||
enum class ReceiveResult {
|
||||
ReadError,
|
||||
Timeout,
|
||||
Receiving,
|
||||
MessageAvailable,
|
||||
};
|
||||
|
||||
/**
|
||||
* The result of trying to configure the receiver.
|
||||
*/
|
||||
enum class ConfigureResult : int32_t {
|
||||
OK = 0,
|
||||
FailedCompletely = 1 << 0,
|
||||
NoLogging = 1 << 1,
|
||||
};
|
||||
|
||||
/**
|
||||
* Maximum timeout to wait for fast command acknowledgement by the receiver.
|
||||
*/
|
||||
static constexpr uint16_t k_receiver_ack_timeout_fast = 200;
|
||||
|
||||
/**
|
||||
* Maximum timeout to wait for slow command acknowledgement by the receiver.
|
||||
* Might be the case for commands that send more output back as reply.
|
||||
*/
|
||||
static constexpr uint16_t k_receiver_ack_timeout_slow = 400;
|
||||
|
||||
/**
|
||||
* Duration of one update monitoring interval in us.
|
||||
* This should be longer than the time it takes for all *positioning* SBF messages to be sent once by the receiver!
|
||||
* Otherwise the driver will assume the receiver configuration isn't healthy because it doesn't see all blocks in time.
|
||||
*/
|
||||
static constexpr hrt_abstime k_update_monitoring_interval_duration = 5_s;
|
||||
|
||||
/**
|
||||
* uORB type to send messages to ground control stations.
|
||||
*/
|
||||
static orb_advert_t k_mavlink_log_pub;
|
||||
|
||||
/**
|
||||
* The default stream for output of PVT data.
|
||||
*/
|
||||
static constexpr uint8_t k_default_main_stream = 1;
|
||||
|
||||
/**
|
||||
* The default stream for output of logging data.
|
||||
*/
|
||||
static constexpr uint8_t k_default_log_stream = 2;
|
||||
|
||||
/**
|
||||
* @brief Parse command line arguments for this module.
|
||||
*
|
||||
* @param argc Number of arguments.
|
||||
* @param argv The arguments.
|
||||
* @param arguments The parsed arguments.
|
||||
*
|
||||
* @return `PX4_OK` on success, `PX4_ERROR` on a parsing error.
|
||||
*/
|
||||
static int parse_cli_arguments(int argc, char *argv[], ModuleArguments &arguments);
|
||||
|
||||
/**
|
||||
* @brief Wait for the second instance to properly start up.
|
||||
*
|
||||
* @return `PX4_OK` once the second instance has started, or `PX4_ERROR` if timed out waiting.
|
||||
*/
|
||||
static int await_second_instance_startup();
|
||||
|
||||
/**
|
||||
* @brief Wait for the second instance to properly shut down.
|
||||
*
|
||||
* @return `PX4_OK` once the second instance has shut down, or `PX4_ERROR` if timed out waiting.
|
||||
*/
|
||||
int await_second_instance_shutdown();
|
||||
|
||||
/**
|
||||
* @brief Schedule a reset of the connected receiver.
|
||||
*/
|
||||
void schedule_reset(ReceiverResetType type);
|
||||
|
||||
/**
|
||||
* @brief Detect whether the receiver is running at the given baud rate.
|
||||
* Does not preserve local baud rate!
|
||||
*
|
||||
* @param baud_rate The baud rate to check.
|
||||
* @param force_input Choose whether the receiver forces input on the port.
|
||||
*
|
||||
* @return `true` if running at the baud rate, or `false` on error.
|
||||
*/
|
||||
bool detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input);
|
||||
|
||||
/**
|
||||
* @brief Try to detect the serial port used on the receiver side.
|
||||
*
|
||||
* @param port_name A string with a length of 5 to store the result
|
||||
*
|
||||
* @return `PX4_OK` on success, `PX4_ERROR` on error
|
||||
*/
|
||||
int detect_serial_port(char *const port_name);
|
||||
|
||||
/**
|
||||
* @brief Configure the attached receiver based on the user's parameters.
|
||||
*
|
||||
* If the user has disabled automatic configuration, only execute the steps that do not touch the receiver (e.g., baud rate detection, port detection...).
|
||||
*
|
||||
* @return `ConfigureResult::OK` if configured, or error.
|
||||
*/
|
||||
ConfigureResult configure();
|
||||
|
||||
/**
|
||||
* @brief Parse the next byte of a received message from the receiver.
|
||||
*
|
||||
* @return 0 = decoding, 1 = message handled, 2 = sat info message handled
|
||||
*/
|
||||
int parse_char(const uint8_t byte);
|
||||
|
||||
/**
|
||||
* @brief Process a fully received message from the receiver.
|
||||
*
|
||||
* @return `PX4_OK` on message handled, `PX4_ERROR` on error.
|
||||
*/
|
||||
int process_message();
|
||||
|
||||
/**
|
||||
* @brief Add payload rx byte.
|
||||
*
|
||||
* @return -1 = error, 0 = ok, 1 = payload received completely
|
||||
*/
|
||||
int payload_rx_add(const uint8_t byte);
|
||||
|
||||
/**
|
||||
* @brief Parses incoming SBF blocks.
|
||||
*
|
||||
* @return bitfield: all 0 = no message handled, 1 = position handled, 2 = satellite info handled
|
||||
*/
|
||||
int payload_rx_done();
|
||||
|
||||
/**
|
||||
* @brief Send a message.
|
||||
*
|
||||
* @return true on success, false on write error (errno set)
|
||||
*/
|
||||
[[nodiscard]] bool send_message(const char *msg);
|
||||
|
||||
/**
|
||||
* @brief Send a message and waits for acknowledge.
|
||||
*
|
||||
* @param msg The message to send to the receiver
|
||||
* @param timeout The time before sending the message times out
|
||||
*
|
||||
* @return true on success, false on write error (errno set) or ack wait timeout
|
||||
*/
|
||||
[[nodiscard]] bool send_message_and_wait_for_ack(const char *msg, const int timeout);
|
||||
|
||||
/**
|
||||
* @brief Receive incoming messages.
|
||||
*
|
||||
* @param timeout Maximum time to wait for new data in ms, after which we return.
|
||||
*
|
||||
* @return -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
|
||||
*/
|
||||
int receive(unsigned timeout);
|
||||
|
||||
/**
|
||||
* @brief Read from the receiver.
|
||||
*
|
||||
* @param buf Data that is read
|
||||
* @param buf_length Size of the buffer
|
||||
* @param timeout Reading timeout
|
||||
*
|
||||
* @return 0 on nothing read or poll timeout, <0 on error and >0 on bytes read (nr of bytes)
|
||||
*/
|
||||
int read(uint8_t *buf, size_t buf_length, int timeout);
|
||||
|
||||
/**
|
||||
* @brief This is an abstraction for the poll on serial used.
|
||||
*
|
||||
* @param buf The read buffer
|
||||
* @param buf_length Size of the read buffer
|
||||
* @param timeout Read timeout in ms
|
||||
*
|
||||
* @return 0 on nothing read or poll timeout, <0 on error and >0 on bytes read (nr of bytes)
|
||||
*/
|
||||
int poll_or_read(uint8_t *buf, size_t buf_length, int timeout);
|
||||
|
||||
/**
|
||||
* @brief Write to the receiver.
|
||||
*
|
||||
* @param buf Data to be written
|
||||
* @param buf_length Amount of bytes to be written
|
||||
*
|
||||
* @return the number of bytes written on success, or -1 otherwise
|
||||
*/
|
||||
int write(const uint8_t *buf, size_t buf_length);
|
||||
|
||||
/**
|
||||
* @brief Initialize uORB GPS logging and advertise the topic.
|
||||
*
|
||||
* @return `PX4_OK` on success, `PX4_ERROR` otherwise
|
||||
*/
|
||||
int initialize_communication_dump(DumpMode mode);
|
||||
|
||||
/**
|
||||
* @brief Reset the receiver if it was requested by the user.
|
||||
*/
|
||||
void reset_if_scheduled();
|
||||
|
||||
/**
|
||||
* @brief Set the baudrate of the serial connection.
|
||||
*
|
||||
* @param baud The baud rate of the connection
|
||||
*
|
||||
* @return `PX4_OK` on success, `PX4_ERROR` on otherwise
|
||||
*/
|
||||
int set_baudrate(uint32_t baud);
|
||||
|
||||
/**
|
||||
* @brief Handle incoming messages on the "inject data" uORB topic and send them to the receiver.
|
||||
*/
|
||||
void handle_inject_data_topic();
|
||||
|
||||
/**
|
||||
* @brief Send data to the receiver, such as RTCM injections.
|
||||
*
|
||||
* @param data The raw data to send to the device
|
||||
* @param len The size of `data`
|
||||
*
|
||||
* @return `true` if all the data was written correctly, `false` otherwise
|
||||
*/
|
||||
inline bool inject_data(uint8_t *data, size_t len);
|
||||
|
||||
/**
|
||||
* @brief Publish new GPS data with uORB.
|
||||
*/
|
||||
void publish();
|
||||
|
||||
/**
|
||||
* @brief Publish new GPS satellite data with uORB.
|
||||
*/
|
||||
void publish_satellite_info();
|
||||
|
||||
/**
|
||||
* @brief Check whether the driver has created its first complete `SensorGPS` uORB message.
|
||||
*
|
||||
* @return `true` if the driver has created its first complete `SensorGPS` uORB message, `false` if still waiting.
|
||||
*/
|
||||
bool first_gps_uorb_message_created() const;
|
||||
|
||||
/**
|
||||
* @brief Publish RTCM corrections.
|
||||
*
|
||||
* @param data: The raw data to publish
|
||||
* @param len: The size of `data`
|
||||
*/
|
||||
void publish_rtcm_corrections(uint8_t *data, size_t len);
|
||||
|
||||
/**
|
||||
* @brief Dump gps communication.
|
||||
*
|
||||
* @param data The raw data of the message.
|
||||
* @param len The size of `data`.
|
||||
* @param data_direction The type of data, either incoming or outgoing.
|
||||
*/
|
||||
void dump_gps_data(const uint8_t *data, size_t len, DataDirection data_direction);
|
||||
|
||||
/**
|
||||
* @brief Check whether we should dump incoming data.
|
||||
*
|
||||
* @return `true` when we should dump incoming data, `false` otherwise.
|
||||
*/
|
||||
bool should_dump_incoming() const;
|
||||
|
||||
/**
|
||||
* @brief Check whether we should dump outgoing data.
|
||||
*
|
||||
* @return `true` when we should dump outgoing data, `false` otherwise.
|
||||
*/
|
||||
bool should_dump_outgoing() const;
|
||||
|
||||
/**
|
||||
* @brief Start a new update frequency monitoring interval.
|
||||
*/
|
||||
void start_update_monitoring_interval();
|
||||
|
||||
/**
|
||||
* @brief Check whether the current update monitoring interval should end.
|
||||
*
|
||||
* @return `true` if a new interval should be started, or `false` if the current interval is still valid.
|
||||
*/
|
||||
bool update_monitoring_interval_ended() const;
|
||||
|
||||
/**
|
||||
* @brief Get the duration of the current update frequency monitoring interval.
|
||||
*
|
||||
* @return The duration of the current interval in us.
|
||||
*/
|
||||
hrt_abstime current_monitoring_interval_duration() const;
|
||||
|
||||
/**
|
||||
* @brief Calculate RTCM message injection frequency for the current measurement interval.
|
||||
*
|
||||
* @return The RTCM message injection frequency for the current measurement interval in Hz.
|
||||
*/
|
||||
float rtcm_injection_frequency() const;
|
||||
|
||||
/**
|
||||
* @brief Calculate output data rate to the receiver for the current measurement interval.
|
||||
*
|
||||
* @return The output data rate for the current measurement interval in B/s.
|
||||
*/
|
||||
uint32_t output_data_rate() const;
|
||||
|
||||
/**
|
||||
* @brief Calculate input data rate from the receiver for the current measurement interval.
|
||||
*
|
||||
* @return The input data rate for the current measurement interval in B/s.
|
||||
*/
|
||||
uint32_t input_data_rate() const;
|
||||
|
||||
/**
|
||||
* @brief Check whether the current receiver configuration is likely healthy.
|
||||
*
|
||||
* This is checked by passively monitoring output from the receiver and validating whether it is what is
|
||||
* expected.
|
||||
*
|
||||
* @return `true` if the receiver is operating correctly, `false` if it needs to be reconfigured.
|
||||
*/
|
||||
bool receiver_configuration_healthy() const;
|
||||
|
||||
/**
|
||||
* @brief Convert from microseconds to seconds.
|
||||
*
|
||||
* @return `us` converted into seconds.
|
||||
*/
|
||||
static float us_to_s(uint64_t us);
|
||||
|
||||
/**
|
||||
* @brief Check if the system clock needs to be updated with new time obtained from the receiver.
|
||||
*
|
||||
* Setting the clock on Nuttx temporarily pauses interrupts. Therefore it should only be set if it is absolutely necessary.
|
||||
*
|
||||
* @return `true` if the clock should be update, `false` if the clock is still accurate enough.
|
||||
*/
|
||||
static bool clock_needs_update(timespec real_time);
|
||||
|
||||
/**
|
||||
* @brief Used to set the system clock accurately.
|
||||
*
|
||||
* This does not guarantee that the clock will be set.
|
||||
*
|
||||
* @param time The current time.
|
||||
*/
|
||||
static void set_clock(timespec rtc_gps_time);
|
||||
|
||||
/**
|
||||
* @brief Check whether the driver is operating correctly.
|
||||
*
|
||||
* The driver is operating correctly when it has fully configured the receiver and is actively receiving all the
|
||||
* expected data.
|
||||
*
|
||||
* @return `true` if the driver is working as expected, `false` otherwise.
|
||||
*/
|
||||
bool is_healthy() const;
|
||||
|
||||
/**
|
||||
* @brief Reset the GPS state uORB message for reuse.
|
||||
*/
|
||||
void reset_gps_state_message();
|
||||
|
||||
/**
|
||||
* @brief Get the parameter with the given name into `value`.
|
||||
*
|
||||
* @param name The name of the parameter.
|
||||
* @param value The value in which to store the parameter.
|
||||
*
|
||||
* @return `PX4_OK` on success, or `PX4_ERROR` if the parameter wasn't found.
|
||||
*/
|
||||
static uint32_t get_parameter(const char *name, int32_t *value);
|
||||
|
||||
/**
|
||||
* @brief Get the parameter with the given name into `value`.
|
||||
*
|
||||
* @param name The name of the parameter.
|
||||
* @param value The value in which to store the parameter.
|
||||
*
|
||||
* @return `PX4_OK` on success, or `PX4_ERROR` if the parameter wasn't found.
|
||||
*/
|
||||
static uint32_t get_parameter(const char *name, float *value);
|
||||
|
||||
/**
|
||||
* @brief Don't use this, use the other parameter functions instead!
|
||||
*/
|
||||
template<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 _chosen_baud_rate {0}; ///< The baud rate requested by the user
|
||||
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
|
||||
uint8_t _current_baud_rate_index {0}; ///< Index of the current baud rate to check
|
||||
|
||||
// Decoding and parsing
|
||||
DecodingStatus _active_decoder {DecodingStatus::Searching}; ///< Currently active decoder that new data has to be fed into
|
||||
sbf::Decoder _sbf_decoder {}; ///< SBF message decoder
|
||||
rtcm::Decoder *_rtcm_decoder {nullptr}; ///< RTCM message decoder
|
||||
|
||||
// uORB topics and subscriptions
|
||||
sensor_gps_s _message_gps_state {}; ///< uORB topic for position
|
||||
gps_dump_s *_message_data_to_receiver {nullptr}; ///< uORB topic for dumping data to the receiver
|
||||
gps_dump_s *_message_data_from_receiver {nullptr}; ///< uORB topic for dumping data from the receiver
|
||||
satellite_info_s *_message_satellite_info {nullptr}; ///< uORB topic for satellite info
|
||||
uORB::PublicationMulti<sensor_gps_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
|
||||
@@ -1,59 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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
|
||||
@@ -1,50 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @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
|
||||
Reference in New Issue
Block a user