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

A driver for EULER-NAV Baro-Inertial AHRS (#24534)

Browse Source 
* Create a dummy BAHRS driver

* Resolve compilation

* Switch back to cpp, fix compilation

* Create module.yaml

* Implement required module APIs and open serial port

* Revise info and error messages

* Poll serial port

* Push received bytes into the ring buffer

* Process data buffer (1)

* Process data buffer (2)

* Process data buffer (3)

* Process data buffer (4)

* Process data buffer (5)

* Process data buffer (6)

* Implement and use initialize() and deinitialize() methods

* Implement print_usage() and print_status()

* Collect all config constants in a class

* Put info about next found message into a class

* Print CRC failure count

* Remove unneeded print

* Add comments

* Disable EKF2, advertise vehicle attitude

* Decode and publish BAHRS signals (1)

* Run the driver as an additional source of sensor signals

* Add tiny noise to baro-inertial pressure signal

* Fix the sensor ID

* Add copyrights

* Fix formatting

* Remove redundant newline character

* Fix long parameter name

* Fix findings (1)

* Fix finding (2)

* Fix formatting

* Fix the timeout value

* Remove aliases

* Fix copyright

* Fix indent

* Comply with naming convention

* Rework comparison to false

* Reduce nesting (1)

* Reduce nesting (2)

* Reduce nesting (3)

* Fix BAHRS sensor ID
This commit is contained in:
fbaklanov 2025-09-04 19:31:36 +02:00 committed by GitHub
parent e6f60ef403
commit fc8e2021e7
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
9 changed files with 1165 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/drivers/drv_sensor.h
View File

@ -254,6 +254,7 @@
#define DRV_INS_DEVTYPE_ILABS 0xE9
#define DRV_INS_DEVTYPE_MICROSTRAIN 0xEA
#define DRV_INS_DEVTYPE_BAHRS 0xEB
#define DRV_DEVTYPE_UNUSED 0xff

1
src/drivers/ins/Kconfig
View File

@ -5,6 +5,7 @@ menu "Inertial Navigation Systems (INS)"
select DRIVERS_INS_VECTORNAV
select DRIVERS_INS_ILABS
select DRIVERS_INS_MICROSTRAIN
select DRIVERS_INS_EULERNAV_BAHRS
---help---
Enable default set of INS sensors
rsource "*/Kconfig"

44
src/drivers/ins/eulernav_bahrs/CMakeLists.txt Normal file
View File

@ -0,0 +1,44 @@
############################################################################
#
# Copyright (c) 2025 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 drivers__ins__eulerav_bahrs
MAIN eulernav_bahrs
INCLUDES
bahrs
SRCS
eulernav_bahrs_main.cpp
eulernav_driver.cpp
eulernav_driver.h
DEPENDS
ringbuffer
)

5
src/drivers/ins/eulernav_bahrs/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_INS_EULERNAV_BAHRS
bool "eulernav_bahrs"
default n
---help---
Enable support for EULER-NAV Baro-Inertial AHRS

371
src/drivers/ins/eulernav_bahrs/bahrs/CSerialProtocol.h Normal file
View File

@ -0,0 +1,371 @@
/****************************************************************************
*
* Copyright (c) 2025 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.
*
****************************************************************************/
/**
* The header is a modified version of a header from this repo:
* https://github.com/euler-nav/bahrs-oss
*
* Copyright 2023 AMS Advanced Air Mobility Sensors UG
*
* 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 of the copyright holder 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 HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <stdint.h>
#define PROTOCOL_WORD_LEN (4)
#define PADDING_SIZE(SPayloadType) (PROTOCOL_WORD_LEN - ((sizeof(SMessageHeader) + sizeof(SPayloadType)) % PROTOCOL_WORD_LEN))
#define BIT_VALID_HEIGHT (0x01)
#define BIT_VALID_VELOCITY_DOWN (0x02)
#define BIT_VALID_ROLL (0x04)
#define BIT_VALID_PITCH (0x08)
#define BIT_VALID_MAGNETIC_HEADING (0x10)
#define BIT_VALID_SPECIFIC_FORCE_X (0x01)
#define BIT_VALID_SPECIFIC_FORCE_Y (0x02)
#define BIT_VALID_SPECIFIC_FORCE_Z (0x04)
#define BIT_VALID_ANGULAR_RATE_X (0x08)
#define BIT_VALID_ANGULAR_RATE_Y (0x10)
#define BIT_VALID_ANGULAR_RATE_Z (0x20)
/**
* @brief The class that describes and implements the BAHRS serial protocol.
*/
class CSerialProtocol
{
public:
static constexpr uint8_t uMarker1_ { 0x4E }; ///< Sync byte 1, symbol 'N'
static constexpr uint8_t uMarker2_ { 0x45 }; ///< Sync char 2, symbol 'E'
static constexpr uint16_t uVersion_ { 2U }; ///< Protocol version
static constexpr float skfSpecificForceScale_ { 1.495384e-3F }; ///< Integer to float, +-5g range
static constexpr float skfAngularRateScale_ { 1.597921e-4F }; ///< Integer to float, +-300 deg/s range
static constexpr float skfHeightScale_ { 0.16784924F }; ///< Integer to float, -1000 to 10000 m range
static constexpr float skfHeighOffset_ { 1000.0F }; ///< An offset to convert unsigned integer to float
static constexpr float skfVelocityDownScale_ { 9.155413e-3F }; ///< Integer to float, -300 to 300 m/s range
static constexpr float skfAngleScale_ { 9.587526e-5F }; ///< Integer to float, -pi to pi or 0 to 2 pi range
// Signal ranges
static constexpr float skfMaxHeight_ { 10000.0F };
static constexpr float skfMinHeight_ { -1000.0F };
static constexpr float skfMaxVelocityDown_ { 300.0F };
static constexpr float skfMinVelocityDown_ { -300.0F };
enum class EMessageIds : uint8_t {
eInvalid = 0x00, ///< Invalid
eInertialData = 0x01, ///< Inertial data message
eNavigationData = 0x02, ///< Navigation data message
eAccuracy = 0x03, ///< Attitude accuracy information
eTimeOfNavigationData = 0x04, ///< "Time of navigation data" message
eTimeOfInertialData = 0x05, ///< "Time of inertial data" message
eTimeOfSyncPulse = 0x06, ///< "Time of the latest sync pulse" message
eSoftwareVersion = 0x0F, ///< "Software version" message
eDebugEventWriteToPort = 0xC0, ///< Debug information: SWC port data
eDebugEventRunnableCall = 0xC1, ///< Debug information: SWC API call
eTypeOpenDiagnostic = 0xF0, ///< Request to enter diagnostics mode
eTypeCloseDiagnostic = 0xF1, ///< Request to exit diagnostics mode
eTypeReadNVMPage = 0xF2, ///< Request to read NVM page
eTypeNVMPageData = 0xF3, ///< A message with NVM page data
eTypeAccept = 0xFF ///< Acknowledgment of diagnostics message reception
};
typedef uint32_t CrcType_t;
#pragma pack(push, 1)
/**
* @brief Message header struct.
*/
struct SMessageHeader {
uint8_t uMarker1_ { 0x4E };
uint8_t uMarker2_ { 0x45 };
uint16_t uVersion_ { CSerialProtocol::uVersion_ };
uint8_t uMsgType_ { 0U };
};
/**
* @brief Inertial data message payload.
*/
struct SInertialData {
uint8_t uSequenceCounter_ { 0U };
int16_t iSpecificForceX_ { 0 };
int16_t iSpecificForceY_ { 0 };
int16_t iSpecificForceZ_ { 0 };
int16_t iAngularRateX_ { 0 };
int16_t iAngularRateY_ { 0 };
int16_t iAngularRateZ_ { 0 };
uint8_t uValidity_ { 0U };
};
/**
* @brief Payload of the time of inertial data message.
*/
struct STimeOfInertialData {
uint8_t uSequenceCounter_ { 0U };
uint8_t uInertialDataSequenceCounter_ { 0U };
uint64_t uTimestampUs_ { 0U };
};
/**
* @brief Navigation data message payload.
*/
struct SNavigationData {
uint8_t uSequenceCounter_ { 0U };
uint16_t uPressureHeight_ { 0 };
int16_t iVelocityDown_ { 0 };
int16_t iRoll_ { 0 };
int16_t iPitch_ { 0 };
uint16_t uMagneticHeading_ { 0U };
uint8_t uValidity_ { 0 };
};
/**
* @brief Payload of the "time of navigation data" message.
*/
struct STimeOfNavigationData {
uint8_t uSequenceCounter_ { 0U };
uint8_t uNavigationDataSequenceCounter_ { 0U };
uint64_t uTimestampUs_ { 0U };
};
/**
* @brief Accuracy message payload.
*/
struct SAccuracyData {
uint8_t uSequenceCounter_ { 0U };
uint16_t uAttitudeStdN_ { 0U };
uint16_t uAttitudeStdE_ { 0U };
uint16_t uMagneticHeadingStd_ { 0U };
uint64_t uTimestampUs_ { 0U };
};
/**
* @brief Payload of the "time of the latest pulse" message.
*/
struct STimeOfLatestSyncPulse {
uint8_t uSequenceCounter_ { 0U };
uint64_t uTimestampUs_ { 0U };
};
/**
* @brief Payload of the "software version" message.
*/
struct SSoftwareVersionData {
char acProjectCode_[3] { '\0', '\0', '\0' };
uint16_t uMajor_ { 0U };
uint16_t uMinor_ { 0U };
};
/**
* @brief Partial data of the "write to port event".
* The struct does not include a variable size array of data written to a port.
*/
struct SWriteToPortEventPartialData {
uint8_t uPortId_ { 0U };
uint64_t uTimestampUs_ { 0U };
uint16_t uDataLen_ { 0U };
};
/**
* @brief Debug information on runnable call.
*/
struct SRunnableCallEventData {
uint8_t uRunnableId_ { 0U };
uint64_t uTimestampUs_ { 0U };
};
/**
* @brief Inertial data message.
*/
struct SInertialDataMessage {
SMessageHeader oHeader_ { CSerialProtocol::uMarker1_,
CSerialProtocol::uMarker2_,
CSerialProtocol::uVersion_,
static_cast<uint8_t>(EMessageIds::eInertialData) };
SInertialData oInertialData_;
uint8_t auPadding_[PADDING_SIZE(SInertialData)];
CrcType_t uCrc_ { 0U };
};
/**
* @brief Time of inertial data message.
*/
struct STimeOfInertialDataMessage {
SMessageHeader oHeader_ { CSerialProtocol::uMarker1_,
CSerialProtocol::uMarker2_,
CSerialProtocol::uVersion_,
static_cast<uint8_t>(EMessageIds::eTimeOfInertialData) };
STimeOfInertialData oTimeOfInertialData_;
uint8_t auPadding_[PADDING_SIZE(STimeOfInertialData)];
CrcType_t uCrc_ { 0U };
};
/**
* @brief Navigation data message.
*/
struct SNavigationDataMessage {
SMessageHeader oHeader_ { CSerialProtocol::uMarker1_,
CSerialProtocol::uMarker2_,
CSerialProtocol::uVersion_,
static_cast<uint8_t>(EMessageIds::eNavigationData) };
SNavigationData oNavigationData_;
uint8_t auPadding_[PADDING_SIZE(SNavigationData)];
CrcType_t uCrc_ { 0U };
};
/**
* @brief Navigation data accuracy message.
*/
struct SAccuracyDataMessage {
SMessageHeader oHeader_ { CSerialProtocol::uMarker1_,
CSerialProtocol::uMarker2_,
CSerialProtocol::uVersion_,
static_cast<uint8_t>(EMessageIds::eAccuracy) };
SAccuracyData oAccuracy_;
uint8_t auPadding_[PADDING_SIZE(SAccuracyData)];
CrcType_t uCrc_ { 0U };
};
/**
* @brief Time of navigation data message.
*/
struct STimeOfNavigationDataMessage {
SMessageHeader oHeader_ { CSerialProtocol::uMarker1_,
CSerialProtocol::uMarker2_,
CSerialProtocol::uVersion_,
static_cast<uint8_t>(EMessageIds::eTimeOfNavigationData) };
STimeOfNavigationData oTimeOfNavigationData_;
uint8_t auPadding_[PADDING_SIZE(STimeOfNavigationData)];
CrcType_t uCrc_ { 0U };
};
/**
* @brief Time of the latest sync pulse message.
*/
struct STimeOfLatestSyncPulseMessage {
SMessageHeader oHeader_ { CSerialProtocol::uMarker1_,
CSerialProtocol::uMarker2_,
CSerialProtocol::uVersion_,
static_cast<uint8_t>(EMessageIds::eTimeOfSyncPulse) };
STimeOfLatestSyncPulse oTimeOfLatestSyncPulse_;
uint8_t auPadding_[PADDING_SIZE(STimeOfLatestSyncPulse)];
CrcType_t uCrc_ { 0U };
};
/**
* @brief Software version message.
*/
struct SSoftwareVersionMessage {
SMessageHeader oHeader_ { CSerialProtocol::uMarker1_,
CSerialProtocol::uMarker2_,
CSerialProtocol::uVersion_,
static_cast<uint8_t>(EMessageIds::eSoftwareVersion) };
SSoftwareVersionData oSoftwareVersion_;
uint8_t auPadding_[PADDING_SIZE(SSoftwareVersionData)];
CrcType_t uCrc_ { 0U };
};
/**
* @brief A debug message with runnable call data.
*/
struct SRunnableCallEventDebugMessage {
SMessageHeader oHeader_{ CSerialProtocol::uMarker1_,
CSerialProtocol::uMarker2_,
CSerialProtocol::uVersion_,
static_cast<uint8_t>(EMessageIds::eDebugEventRunnableCall) };
SRunnableCallEventData oRunnableCallData_;
uint8_t auPadding_[PADDING_SIZE(oRunnableCallData_)];
CrcType_t uCrc_{ 0U };
};
/**
* @brief Diagnostic Mode Messages
****************************************************************************/
/**
* @brief Diagnostic message: Page request from NVM.
*/
struct SPacketReadNVMPage {
SMessageHeader oHeader_;
uint8_t uPageNumber { 0U };
CrcType_t uCrc32 { 0U };
};
/**
* @brief Diagnostic message: confirmation of request processing by the device.
*/
struct SPacketReceiveConfirmation {
SMessageHeader oHeader_ { CSerialProtocol::uMarker1_,
CSerialProtocol::uMarker2_,
CSerialProtocol::uVersion_,
static_cast<uint8_t>(EMessageIds::eTypeAccept) };
EMessageIds eMessageType; // message type to confirm.
uint8_t uStatus { 0U }; // 0 - OK, other values - error codes
CrcType_t uCrc32 { 0U };
};
#pragma pack(pop)
};

48
src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp Normal file
View File

@ -0,0 +1,48 @@
/****************************************************************************
*
* Copyright (c) 2025 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 eulernav_bahrs_main.cpp
* A driver module for EULER-NAV Baro-Inertial AHRS.
*
* @author Fedor Baklanov <fedor.baklanov@euler-nav.com>
*/
#include <px4_platform_common/log.h>
#include "eulernav_driver.h"
extern "C" __EXPORT int eulernav_bahrs_main(int argc, char *argv[])
{
EulerNavDriver::main(argc, argv);
return OK;
}

514
src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp Normal file
View File

@ -0,0 +1,514 @@
/****************************************************************************
*
* Copyright (c) 2025 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.
*
****************************************************************************/
#include "eulernav_driver.h"
#include <px4_platform_common/getopt.h>
#include <drivers/drv_sensor.h>
#include <matrix/Quaternion.hpp>
#include <matrix/Euler.hpp>
#include <atmosphere/atmosphere.h>
EulerNavDriver::EulerNavDriver(const char *device_name)
: ModuleParams{nullptr}
, _serial_port{device_name, 115200, ByteSize::EightBits, Parity::None, StopBits::One, FlowControl::Disabled}
, _data_buffer{}
, _px4_accel{DRV_INS_DEVTYPE_BAHRS}
, _px4_gyro{DRV_INS_DEVTYPE_BAHRS}
{
}
EulerNavDriver::~EulerNavDriver()
{
deinitialize();
}
int EulerNavDriver::task_spawn(int argc, char *argv[])
{
int task_id = px4_task_spawn_cmd("bahrs", SCHED_DEFAULT, SCHED_PRIORITY_FAST_DRIVER,
Config::TASK_STACK_SIZE, (px4_main_t)&run_trampoline, argv);
if (task_id < 0) {
_task_id = -1;
PX4_ERR("Failed to spawn task.");
} else {
_task_id = task_id;
}
return (_task_id < 0) ? 1 : 0;
}
EulerNavDriver *EulerNavDriver::instantiate(int argc, char *argv[])
{
int option_index = 1;
const char *option_arg{nullptr};
const char *device_name{nullptr};
while (true) {
int option{px4_getopt(argc, argv, "d:", &option_index, &option_arg)};
if (EOF == option) {
break;
}
switch (option) {
case 'd':
device_name = option_arg;
break;
default:
break;
}
}
auto *instance = new EulerNavDriver(device_name);
if (nullptr != instance) {
instance->initialize();
} else {
PX4_ERR("Failed to initialize EULER-NAV driver.");
}
return instance;
}
int EulerNavDriver::custom_command(int argc, char *argv[])
{
return print_usage("unrecognized command");
}
int EulerNavDriver::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Serial bus driver for the EULER-NAV Baro-Inertial AHRS.
### Examples
Attempt to start driver on a specified serial device.
$ eulernav_bahrs start -d /dev/ttyS1
Stop driver
$ eulernav_bahrs stop
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("eulernav_bahrs", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("ins");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print driver status");
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
return PX4_OK;
}
int EulerNavDriver::print_status()
{
if (_is_initialized)
{
PX4_INFO("Elapsed time: %llu [us].\n", hrt_elapsed_time(&_statistics.start_time));
PX4_INFO("Total bytes received: %lu.\n", _statistics.total_bytes_received);
PX4_INFO("Inertial messages received: %lu. Navigation messages received: %lu.\n",
_statistics.inertial_message_counter, _statistics.navigation_message_counter);
PX4_INFO("Failed CRC count: %lu.\n", _statistics.crc_failures);
}
else
{
PX4_INFO("Initialization failed. The driver is not running.\n");
}
return PX4_OK;
}
void EulerNavDriver::run()
{
_statistics.start_time = hrt_absolute_time();
while(!should_exit())
{
if (_is_initialized)
{
const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer, sizeof(_serial_read_buffer),
Config::MIN_BYTES_TO_READ, Config::SERIAL_READ_TIMEOUT_MS)};
_statistics.total_bytes_received += bytes_read;
if (bytes_read > 0)
{
if (!_data_buffer.push_back(_serial_read_buffer, bytes_read))
{
PX4_ERR("No space in data buffer");
}
}
processDataBuffer();
}
else
{
// The driver failed to initialize in the instantiate() method, so we exit the loop.
request_stop();
}
}
}
void EulerNavDriver::initialize()
{
if (!_is_initialized)
{
if (_serial_port.open())
{
PX4_INFO("Serial port opened successfully.");
_is_initialized = true;
}
else
{
PX4_ERR("Failed to open serial port");
}
if (_is_initialized)
{
if (!_data_buffer.allocate(Config::DATA_BUFFER_SIZE))
{
PX4_ERR("Failed to allocate data buffer");
_is_initialized = false;
}
}
if (_is_initialized)
{
_attitude_pub.advertise();
_barometer_pub.advertise();
}
}
}
void EulerNavDriver::deinitialize()
{
if (_serial_port.isOpen())
{
_serial_port.close();
}
_data_buffer.deallocate();
_attitude_pub.unadvertise();
_barometer_pub.unadvertise();
_is_initialized = false;
}
void EulerNavDriver::processDataBuffer()
{
static_assert(Config::MIN_MESSAGE_LENGTH >= (sizeof(CSerialProtocol::SMessageHeader) + sizeof(CSerialProtocol::CrcType_t)));
using EMessageIds = CSerialProtocol::EMessageIds;
while (_data_buffer.space_used() >= Config::MIN_MESSAGE_LENGTH)
{
if (!_next_message_info.is_detected)
{
_next_message_info.is_detected = findNextMessageHeader(_data_buffer);
if (!_next_message_info.is_detected)
{
// No message header found, wait for more bytes to arrive.
break;
}
if (!retrieveProtocolVersionAndMessageType(_data_buffer, _next_message_info.protocol_version, _next_message_info.message_code))
{
_next_message_info.is_detected = false;
}
}
if (_next_message_info.is_detected)
{
static_assert(sizeof(CSerialProtocol::SMessageHeader) < Config::MIN_MESSAGE_LENGTH);
const EMessageIds message_id{static_cast<EMessageIds>(_next_message_info.message_code)};
const int32_t message_length{getMessageLength(message_id)};
if ((message_length < 0) || (message_length < Config::MIN_MESSAGE_LENGTH) ||
(message_length > static_cast<int32_t>(sizeof(_message_storage))) || ((message_length % sizeof(uint32_t)) != 0U))
{
// The message is unknown, not supported, or does not fit into the temporary storage.
_next_message_info.is_detected = false;
continue;
}
const int32_t bytes_to_retrieve{message_length - static_cast<int32_t>(sizeof(CSerialProtocol::SMessageHeader))};
if (static_cast<int32_t>(_data_buffer.space_used()) < bytes_to_retrieve)
{
// Do nothing and wait for more bytes to arrive.
break;
}
// Get message from the data buffer
uint8_t* bytes{reinterpret_cast<uint8_t*>(_message_storage)};
bytes[0] = CSerialProtocol::uMarker1_;
bytes[1] = CSerialProtocol::uMarker2_;
bytes[2] = reinterpret_cast<uint8_t*>(&_next_message_info.protocol_version)[0];
bytes[3] = reinterpret_cast<uint8_t*>(&_next_message_info.protocol_version)[1];
bytes[4] = _next_message_info.message_code;
if (static_cast<size_t>(bytes_to_retrieve) == _data_buffer.pop_front(bytes + sizeof(CSerialProtocol::SMessageHeader), bytes_to_retrieve))
{
const uint32_t message_length_in_words{message_length / sizeof(uint32_t)};
const uint32_t actual_crc{crc32(_message_storage, message_length_in_words - 1)};
const uint32_t expected_crc = _message_storage[message_length_in_words - 1];
if (expected_crc != actual_crc)
{
++_statistics.crc_failures;
}
else
{
decodeMessageAndPublishData(bytes, message_id);
}
}
_next_message_info.is_detected = false;
}
}
}
bool EulerNavDriver::findNextMessageHeader(Ringbuffer& buffer)
{
bool result{false};
while (buffer.space_used() >= sizeof(CSerialProtocol::SMessageHeader))
{
uint8_t sync_byte{0U};
if ((1 == buffer.pop_front(&sync_byte, 1)) && (CSerialProtocol::uMarker1_ == sync_byte))
{
sync_byte = 0U;
if ((1 == buffer.pop_front(&sync_byte, 1)) && (CSerialProtocol::uMarker2_ == sync_byte))
{
result = true;
break;
}
}
}
return result;
}
bool EulerNavDriver::retrieveProtocolVersionAndMessageType(Ringbuffer& buffer, uint16_t& protocol_ver, uint8_t& message_code)
{
bool status{true};
auto bytes_to_pop{sizeof(protocol_ver)};
// Note: BAHRS uses little endian
if (bytes_to_pop != buffer.pop_front(reinterpret_cast<uint8_t*>(&protocol_ver), bytes_to_pop))
{
status = false;
}
if (status)
{
bytes_to_pop = 1;
if (bytes_to_pop != buffer.pop_front(&message_code, bytes_to_pop))
{
status = false;
}
}
return status;
}
void EulerNavDriver::decodeMessageAndPublishData(const uint8_t* data, CSerialProtocol::EMessageIds messsage_id)
{
switch (messsage_id)
{
case CSerialProtocol::EMessageIds::eInertialData:
handleInertialDataMessage(data);
break;
case CSerialProtocol::EMessageIds::eNavigationData:
handleNavigationDataMessage(data);
break;
default:
break;
}
}
void EulerNavDriver::handleInertialDataMessage(const uint8_t* data)
{
const CSerialProtocol::SInertialDataMessage* imu_msg{reinterpret_cast<const CSerialProtocol::SInertialDataMessage*>(data)};
if (nullptr != imu_msg)
{
const auto& imu_data{imu_msg->oInertialData_};
const bool accel_valid{((imu_data.uValidity_ & BIT_VALID_SPECIFIC_FORCE_X) > 0) &&
((imu_data.uValidity_ & BIT_VALID_SPECIFIC_FORCE_Y) > 0) &&
((imu_data.uValidity_ & BIT_VALID_SPECIFIC_FORCE_Z) > 0)};
const bool gyro_valid{((imu_data.uValidity_ & BIT_VALID_ANGULAR_RATE_X) > 0) &&
((imu_data.uValidity_ & BIT_VALID_ANGULAR_RATE_Y) > 0) &&
((imu_data.uValidity_ & BIT_VALID_ANGULAR_RATE_Z) > 0)};
const auto time = hrt_absolute_time();
if (accel_valid)
{
const float accel_x{CSerialProtocol::skfSpecificForceScale_ * static_cast<float>(imu_data.iSpecificForceX_)};
const float accel_y{CSerialProtocol::skfSpecificForceScale_ * static_cast<float>(imu_data.iSpecificForceY_)};
const float accel_z{CSerialProtocol::skfSpecificForceScale_ * static_cast<float>(imu_data.iSpecificForceZ_)};
_px4_accel.update(time, accel_x, accel_y, accel_z);
}
if (gyro_valid)
{
const float gyro_x{CSerialProtocol::skfAngularRateScale_ * static_cast<float>(imu_data.iAngularRateX_)};
const float gyro_y{CSerialProtocol::skfAngularRateScale_ * static_cast<float>(imu_data.iAngularRateY_)};
const float gyro_z{CSerialProtocol::skfAngularRateScale_ * static_cast<float>(imu_data.iAngularRateZ_)};
_px4_gyro.update(time, gyro_x, gyro_y, gyro_z);
}
++_statistics.inertial_message_counter;
}
}
void EulerNavDriver::handleNavigationDataMessage(const uint8_t* data)
{
const CSerialProtocol::SNavigationDataMessage* nav_msg{reinterpret_cast<const CSerialProtocol::SNavigationDataMessage*>(data)};
if (nullptr != nav_msg)
{
const auto& nav_data{nav_msg->oNavigationData_};
const bool roll_valid{(nav_data.uValidity_ & BIT_VALID_ROLL) > 0};
const bool pitch_valid{(nav_data.uValidity_ & BIT_VALID_PITCH) > 0};
const bool yaw_valid{(nav_data.uValidity_ & BIT_VALID_MAGNETIC_HEADING) > 0};
const auto time{hrt_absolute_time()};
if (roll_valid && pitch_valid && yaw_valid)
{
const float roll{CSerialProtocol::skfAngleScale_ * static_cast<float>(nav_data.iRoll_)};
const float pitch{CSerialProtocol::skfAngleScale_ * static_cast<float>(nav_data.iPitch_)};
const float yaw{CSerialProtocol::skfAngleScale_ * static_cast<float>(nav_data.uMagneticHeading_)};
const matrix::Quaternionf quat{matrix::Eulerf{roll, pitch, yaw}};
px4::msg::VehicleAttitude attitude{};
attitude.q[0] = quat(0);
attitude.q[1] = quat(1);
attitude.q[2] = quat(2);
attitude.q[3] = quat(3);
attitude.timestamp = time;
attitude.timestamp_sample = time;
_attitude_pub.publish(attitude);
}
const bool height_valid{(nav_data.uValidity_ & BIT_VALID_HEIGHT) > 0};
if (height_valid)
{
const float height{(CSerialProtocol::skfHeightScale_ * static_cast<float>(nav_data.uPressureHeight_)) - CSerialProtocol::skfHeighOffset_};
px4::msg::SensorBaro pressure{};
pressure.pressure = atmosphere::getPressureFromAltitude(height);
// EULER-NAV Baro-Inertial AHRS provides height estimate from a Kalman filter. It has got low noise and resolution
// of about 17 cm. It causes PX4 autopilot to mistakenly report that pressure signal is stale. In order to prevent
// the false alarms we add a small noise to the received height data.
if (_statistics.navigation_message_counter % 2U == 0)
{
pressure.pressure += 0.01F;
}
pressure.timestamp = time;
pressure.timestamp_sample = time;
pressure.device_id = DRV_INS_DEVTYPE_BAHRS;
pressure.temperature = NAN;
_barometer_pub.publish(pressure);
}
++_statistics.navigation_message_counter;
}
}
int32_t EulerNavDriver::getMessageLength(CSerialProtocol::EMessageIds messsage_id)
{
int message_length{-1};
switch (messsage_id)
{
case CSerialProtocol::EMessageIds::eInertialData:
message_length = sizeof(CSerialProtocol::SInertialDataMessage);
break;
case CSerialProtocol::EMessageIds::eNavigationData:
message_length = sizeof(CSerialProtocol::SNavigationDataMessage);
break;
default:
break;
}
return message_length;
}
uint32_t EulerNavDriver::crc32(const uint32_t* buffer, size_t length)
{
uint32_t crc = 0xFFFFFFFF;
for (size_t i = 0; i < length; ++i)
{
crc = crc ^ buffer[i];
for (uint8_t j = 0; j < 32; j++)
{
if (crc & 0x80000000)
{
crc = (crc << 1) ^ 0x04C11DB7;
}
else
{
crc = (crc << 1);
}
}
}
return crc;
}

174
src/drivers/ins/eulernav_bahrs/eulernav_driver.h Normal file
View File

@ -0,0 +1,174 @@
/****************************************************************************
*
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/Serial.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/vehicle_attitude.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <uORB/topics/sensor_baro.h>
#include <Ringbuffer.hpp>
#include <containers/Array.hpp>
#include "CSerialProtocol.h"
class EulerNavDriver : public ModuleBase<EulerNavDriver>, public ModuleParams
{
public:
/// @brief Class constructor
/// @param device_name Serial port to open
EulerNavDriver(const char *device_name);
~EulerNavDriver();
/// @brief Required by ModuleBase
static int task_spawn(int argc, char *argv[]);
/// @brief Required by ModuleBase
static EulerNavDriver *instantiate(int argc, char *argv[]);
/// @brief Required by ModuleBase
static int custom_command(int argc, char *argv[]);
/// @brief Required by ModuleBase
static int print_usage(const char *reason = nullptr);
/// @brief Overload of the method from the ModuleBase
int print_status() final;
/// @brief The main loop of the task.
void run() final;
private:
/// @brief Driver performance indicators
class Statistics
{
public:
uint32_t total_bytes_received{0U}; ///< Total number of received bytes
uint32_t inertial_message_counter{0U}; ///< Total number of received inertial data messages
uint32_t navigation_message_counter{0U}; ///< Total number of received navigation messages
uint32_t crc_failures{0U}; ///< CRC failure counter
hrt_abstime start_time{0U}; ///< Driver start time, [us]
};
/// @brief Configuration constants
class Config
{
public:
static constexpr uint32_t TASK_STACK_SIZE{2048}; ///< Driver task stack size
static constexpr uint32_t SERIAL_READ_BUFFER_SIZE{128}; ///< Buffer size for serial port read operations
static constexpr uint32_t MIN_BYTES_TO_READ{16}; ///< Minimum number of bytes to wait for when reading from a serial port
static constexpr uint32_t SERIAL_READ_TIMEOUT_MS{5}; ///< A timeout for serial port read operation
static constexpr uint32_t DATA_BUFFER_SIZE{512}; ///< Size of a ring buffer for storing RX data stream
static constexpr int32_t MIN_MESSAGE_LENGTH{12}; ///< Min length of a valid message. 5 bytes header + 4 bytes CRC + padding to 12 (multiple of 32 bit words)
};
/// @brief An object for grouping message-related attributes
class NextMessageInfo
{
public:
bool is_detected{false}; ///< A flag to indicate that the next message header was detected
uint16_t protocol_version{0U}; ///< Protocol version retrieved from the next message header
uint8_t message_code{0U}; ///< Message code retrieved from the next message header
};
/// @brief Perform initialization
/// If the driver is not initialized, the function does the following:
/// 1. Opens serial port
/// 2. Allocates a ring buffer for RX data stream
/// 3. Cleans up if any of the previous operations fails
/// 4. Sets the "is initialized" flag to true on success
void initialize();
/// @brief De-initialize
/// The function does the following:
/// 1. Close the serial port, if it is opened
/// 2. Deallocates the ring buffer
/// 3. Resets the "is initialized" flag to false
void deinitialize();
/// @brief Process data in the ring buffer.
/// Extracts and parses protocol messages.
void processDataBuffer();
/// @brief Searches the ring buffer for sync bytes.
/// @param buffer Ring buffer to search
/// @return True if sync bytes found, false if the number of bytes remaining in the buffer is less then message header length.
static bool findNextMessageHeader(Ringbuffer &buffer);
/// @brief Get protocol version and message code from the ring buffer.
/// @param buffer The buffer to process
/// @param protocol_ver Output protocol version
/// @param message_code Output message code
/// @return True on success, false on failure
static bool retrieveProtocolVersionAndMessageType(Ringbuffer &buffer, uint16_t &protocol_ver, uint8_t &message_code);
/// @brief Decode a message from BAHRS and publish its content.
/// @param data An array of message bytes
/// @param messsage_id Message ID
void decodeMessageAndPublishData(const uint8_t *data, CSerialProtocol::EMessageIds messsage_id);
/// @brief Decode and publish IMU data.
/// @param data Inertial message data bytes
void handleInertialDataMessage(const uint8_t *data);
/// @brief Decode and publish vehicle attitude and pressure height.
/// @param data Navigation message data bytes
void handleNavigationDataMessage(const uint8_t *data);
/// @brief Get message length by message ID
/// @param messsage_id Query message ID
/// @return Message length, or -1 if the message ID is unknown or not supported.
static int32_t getMessageLength(CSerialProtocol::EMessageIds messsage_id);
/// @brief Perform CRC
/// @param buf Data buffer pointer
/// @param len Number of words to include in the CRC.
/// @return CRC value
static uint32_t crc32(const uint32_t *buf, size_t len);
device::Serial _serial_port; ///< Serial port object to read data from
Ringbuffer _data_buffer; ///< A buffer for RX data stream
uint8_t _serial_read_buffer[Config::SERIAL_READ_BUFFER_SIZE]; ///< A buffer for serial port read operation
uint32_t _message_storage[8]; ///< A temporary storage for BAHRS messages being extracted
NextMessageInfo _next_message_info{}; ///< Attributes of the next message detected in the data buffer
Statistics _statistics{}; ///< Driver performance indicators
bool _is_initialized{false}; ///< Initialization flag
PX4Accelerometer _px4_accel; ///< Accelerometer sensor object for publishing acceleration data
PX4Gyroscope _px4_gyro; ///< Gyroscope sensor object for publishing angular rate data
uORB::PublicationMulti<px4::msg::VehicleAttitude> _attitude_pub{ORB_ID(vehicle_attitude)}; ///< Vehicle attitude publisher
uORB::PublicationMulti<px4::msg::SensorBaro> _barometer_pub{ORB_ID(sensor_baro)}; ///< Pressure data publisher
};

7
src/drivers/ins/eulernav_bahrs/module.yaml Normal file
View File

@ -0,0 +1,7 @@
module_name: EULER-NAV BAHRS
serial_config:
- command: eulernav_bahrs start -d ${SERIAL_DEV}
port_config_param:
name: SENS_BAHRS_CFG
group: Sensors