diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 2d943397ea..32eadbb067 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -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 diff --git a/src/drivers/ins/Kconfig b/src/drivers/ins/Kconfig index e5f5f6324a..70db3d9fcb 100644 --- a/src/drivers/ins/Kconfig +++ b/src/drivers/ins/Kconfig @@ -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" diff --git a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt new file mode 100644 index 0000000000..8d40252de1 --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt @@ -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 + ) diff --git a/src/drivers/ins/eulernav_bahrs/Kconfig b/src/drivers/ins/eulernav_bahrs/Kconfig new file mode 100644 index 0000000000..80c32787dd --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_INS_EULERNAV_BAHRS + bool "eulernav_bahrs" + default n + ---help--- + Enable support for EULER-NAV Baro-Inertial AHRS diff --git a/src/drivers/ins/eulernav_bahrs/bahrs/CSerialProtocol.h b/src/drivers/ins/eulernav_bahrs/bahrs/CSerialProtocol.h new file mode 100644 index 0000000000..8814f2a5fc --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/bahrs/CSerialProtocol.h @@ -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 + +#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(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(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(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(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(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(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(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(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(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) + +}; diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp new file mode 100644 index 0000000000..674db6dbf1 --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp @@ -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 + */ + +#include +#include "eulernav_driver.h" + +extern "C" __EXPORT int eulernav_bahrs_main(int argc, char *argv[]) +{ + EulerNavDriver::main(argc, argv); + return OK; +} diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp new file mode 100644 index 0000000000..1592308d76 --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -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 +#include +#include +#include +#include + +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(_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(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(sizeof(CSerialProtocol::SMessageHeader))}; + + if (static_cast(_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(_message_storage)}; + + bytes[0] = CSerialProtocol::uMarker1_; + bytes[1] = CSerialProtocol::uMarker2_; + bytes[2] = reinterpret_cast(&_next_message_info.protocol_version)[0]; + bytes[3] = reinterpret_cast(&_next_message_info.protocol_version)[1]; + bytes[4] = _next_message_info.message_code; + + if (static_cast(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(&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(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(imu_data.iSpecificForceX_)}; + const float accel_y{CSerialProtocol::skfSpecificForceScale_ * static_cast(imu_data.iSpecificForceY_)}; + const float accel_z{CSerialProtocol::skfSpecificForceScale_ * static_cast(imu_data.iSpecificForceZ_)}; + + _px4_accel.update(time, accel_x, accel_y, accel_z); + } + + if (gyro_valid) + { + const float gyro_x{CSerialProtocol::skfAngularRateScale_ * static_cast(imu_data.iAngularRateX_)}; + const float gyro_y{CSerialProtocol::skfAngularRateScale_ * static_cast(imu_data.iAngularRateY_)}; + const float gyro_z{CSerialProtocol::skfAngularRateScale_ * static_cast(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(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(nav_data.iRoll_)}; + const float pitch{CSerialProtocol::skfAngleScale_ * static_cast(nav_data.iPitch_)}; + const float yaw{CSerialProtocol::skfAngleScale_ * static_cast(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(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; +} diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h new file mode 100644 index 0000000000..851b8d63b1 --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "CSerialProtocol.h" + +class EulerNavDriver : public ModuleBase, 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 _attitude_pub{ORB_ID(vehicle_attitude)}; ///< Vehicle attitude publisher + uORB::PublicationMulti _barometer_pub{ORB_ID(sensor_baro)}; ///< Pressure data publisher +}; diff --git a/src/drivers/ins/eulernav_bahrs/module.yaml b/src/drivers/ins/eulernav_bahrs/module.yaml new file mode 100644 index 0000000000..38ff4a5f1b --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/module.yaml @@ -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