From e6c90784a4ac077630cb45474764cdfe1afdc86b Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Fri, 13 Dec 2024 12:45:29 +0100 Subject: [PATCH 01/21] Create a dummy BAHRS driver --- src/drivers/ins/Kconfig | 1 + src/drivers/ins/eulernav_bahrs/CMakeLists.txt | 39 +++++++++++++++ src/drivers/ins/eulernav_bahrs/Kconfig | 5 ++ .../eulernav_bahrs/eulernav_bahrs_main.cpp | 49 +++++++++++++++++++ 4 files changed, 94 insertions(+) create mode 100644 src/drivers/ins/eulernav_bahrs/CMakeLists.txt create mode 100644 src/drivers/ins/eulernav_bahrs/Kconfig create mode 100644 src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp diff --git a/src/drivers/ins/Kconfig b/src/drivers/ins/Kconfig index 5c215e3cca15..2d54833af4c6 100644 --- a/src/drivers/ins/Kconfig +++ b/src/drivers/ins/Kconfig @@ -3,6 +3,7 @@ menu "Inertial Navigation Systems (INS)" bool "All INS sensors" default n select DRIVERS_INS_VECTORNAV + 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 000000000000..094ab1b0d8a3 --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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 drivers__ins__eulerav_bahrs + MAIN eulernav_bahrs_main + SRCS + eulernav_bahrs_main.cpp + DEPENDS + ) diff --git a/src/drivers/ins/eulernav_bahrs/Kconfig b/src/drivers/ins/eulernav_bahrs/Kconfig new file mode 100644 index 000000000000..80c32787dd66 --- /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/eulernav_bahrs_main.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp new file mode 100644 index 000000000000..bd87699df32f --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * 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 px4_simple_app.c + * Minimal application example for PX4 autopilot + * + * @author Example User + */ + +#include + +__EXPORT int eulernav_bahrs_main(int argc, char *argv[]); + +int eulernav_main(int argc, char *argv[]) +{ + PX4_INFO("Hello Sky!"); + return OK; +} From eb44d50b7210fa6d4c3a47cd1e4619b35181d224 Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Fri, 13 Dec 2024 13:03:00 +0100 Subject: [PATCH 02/21] Resolve compilation --- src/drivers/ins/eulernav_bahrs/CMakeLists.txt | 4 ++-- .../{eulernav_bahrs_main.cpp => eulernav_bahrs_main.c} | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) rename src/drivers/ins/eulernav_bahrs/{eulernav_bahrs_main.cpp => eulernav_bahrs_main.c} (97%) diff --git a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt index 094ab1b0d8a3..d2bed51fb2c8 100644 --- a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt +++ b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt @@ -32,8 +32,8 @@ ############################################################################ px4_add_module( MODULE drivers__ins__eulerav_bahrs - MAIN eulernav_bahrs_main + MAIN eulernav_bahrs SRCS - eulernav_bahrs_main.cpp + eulernav_bahrs_main.c DEPENDS ) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.c similarity index 97% rename from src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp rename to src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.c index bd87699df32f..016776b4f31f 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.c @@ -42,7 +42,7 @@ __EXPORT int eulernav_bahrs_main(int argc, char *argv[]); -int eulernav_main(int argc, char *argv[]) +int eulernav_bahrs_main(int argc, char *argv[]) { PX4_INFO("Hello Sky!"); return OK; From e3ee153203e8b3b85cd05bd75b07df7563d7875f Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Fri, 13 Dec 2024 13:29:30 +0100 Subject: [PATCH 03/21] Switch back to cpp, fix compilation --- src/drivers/ins/eulernav_bahrs/CMakeLists.txt | 2 +- .../{eulernav_bahrs_main.c => eulernav_bahrs_main.cpp} | 6 ++---- 2 files changed, 3 insertions(+), 5 deletions(-) rename src/drivers/ins/eulernav_bahrs/{eulernav_bahrs_main.c => eulernav_bahrs_main.cpp} (93%) diff --git a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt index d2bed51fb2c8..3132b5b0a7e3 100644 --- a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt +++ b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt @@ -34,6 +34,6 @@ px4_add_module( MODULE drivers__ins__eulerav_bahrs MAIN eulernav_bahrs SRCS - eulernav_bahrs_main.c + eulernav_bahrs_main.cpp DEPENDS ) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.c b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp similarity index 93% rename from src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.c rename to src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp index 016776b4f31f..72b2de548e85 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.c +++ b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp @@ -40,10 +40,8 @@ #include -__EXPORT int eulernav_bahrs_main(int argc, char *argv[]); - -int eulernav_bahrs_main(int argc, char *argv[]) +extern "C" __EXPORT int eulernav_bahrs_main(int argc, char *argv[]) { - PX4_INFO("Hello Sky!"); + PX4_INFO("This is an EULER-NAV Baro-Inertial AHRS driver!"); return OK; } From 559c26cefdb74895187332b470bfef7430024674 Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Wed, 18 Dec 2024 19:24:44 +0100 Subject: [PATCH 04/21] Create module.yaml --- src/drivers/ins/eulernav_bahrs/module.yaml | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 src/drivers/ins/eulernav_bahrs/module.yaml diff --git a/src/drivers/ins/eulernav_bahrs/module.yaml b/src/drivers/ins/eulernav_bahrs/module.yaml new file mode 100644 index 000000000000..b470d03a1bec --- /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_EN_BAHRS_CFG + group: Sensors From dab846e2765d38ff533d9bdc17fd0b89bf083c25 Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Thu, 19 Dec 2024 11:58:29 +0100 Subject: [PATCH 05/21] Implement required module APIs and open serial port --- src/drivers/ins/eulernav_bahrs/CMakeLists.txt | 2 + .../eulernav_bahrs/eulernav_bahrs_main.cpp | 8 +- .../ins/eulernav_bahrs/eulernav_driver.cpp | 97 +++++++++++++++++++ .../ins/eulernav_bahrs/eulernav_driver.h | 33 +++++++ 4 files changed, 137 insertions(+), 3 deletions(-) create mode 100644 src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp create mode 100644 src/drivers/ins/eulernav_bahrs/eulernav_driver.h diff --git a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt index 3132b5b0a7e3..184cc1c19f2e 100644 --- a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt +++ b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt @@ -35,5 +35,7 @@ px4_add_module( MAIN eulernav_bahrs SRCS eulernav_bahrs_main.cpp + eulernav_driver.cpp + eulernav_driver.h DEPENDS ) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp index 72b2de548e85..03205f1b0d8f 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp @@ -32,16 +32,18 @@ ****************************************************************************/ /** - * @file px4_simple_app.c - * Minimal application example for PX4 autopilot + * @file eulernav_bahrs_main.cpp + * A driver module for EULER-NAV Baro-Inertial AHRS. * - * @author Example User + * @author Fedor Baklanov */ #include +#include "eulernav_driver.h" extern "C" __EXPORT int eulernav_bahrs_main(int argc, char *argv[]) { PX4_INFO("This is an EULER-NAV Baro-Inertial AHRS driver!"); + 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 000000000000..981431ccc767 --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -0,0 +1,97 @@ +#include "eulernav_driver.h" +#include + +EulerNavDriver::EulerNavDriver(const char* device_name) : + _serial_port{device_name, 115200, ByteSize::EightBits, Parity::None, StopBits::One, FlowControl::Disabled} +{ + _serial_port.open(); + + if (_serial_port.isOpen()) + { + PX4_INFO("Serial port opened successfully."); + _is_initialized = true; + } + else + { + PX4_INFO("Failed to open serial port"); + _is_initialized = false; + } +} + +EulerNavDriver::~EulerNavDriver() +{ + if (_serial_port.isOpen()) + { + _serial_port.close(); + } +} + +int EulerNavDriver::task_spawn(int argc, char *argv[]) +{ + int task_id = px4_task_spawn_cmd("bahrs", SCHED_DEFAULT, SCHED_PRIORITY_FAST_DRIVER, + _task_stack_size, (px4_main_t)&run_trampoline, argv); + + PX4_INFO("Task spawn."); + + if (task_id < 0) + { + _task_id = -1; + PX4_INFO("Task spawn failed."); + } + else + { + _task_id = task_id; + PX4_INFO("Task spawn succeeded."); + } + + 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}; + + PX4_INFO("Task instantiate."); + + 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; + } + } + + return new EulerNavDriver(device_name); +} + +int EulerNavDriver::custom_command(int argc, char *argv[]) +{ + return print_usage("unrecognized command"); +} + +int EulerNavDriver::print_usage(const char *reason) +{ + return 0; +} + +void EulerNavDriver::run() +{ + while(false == should_exit()) + { + px4_usleep(1000000); + PX4_INFO("Running the EulerNavDriver::run..."); + } +} 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 000000000000..83ad28aa3b32 --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include + +class EulerNavDriver : public ModuleBase +{ +public: + 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); + + void run() final; + +private: + static constexpr int _task_stack_size{2048}; + device::Serial _serial_port; + bool _is_initialized{false}; +}; + From 8c5baed1fb900bf31f65fd510c58e066a0fd980d Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Thu, 19 Dec 2024 19:24:29 +0100 Subject: [PATCH 06/21] Revise info and error messages --- src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp | 2 +- src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp | 9 ++------- src/drivers/ins/eulernav_bahrs/eulernav_driver.h | 1 + 3 files changed, 4 insertions(+), 8 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp index 03205f1b0d8f..918a10db2ffd 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp @@ -43,7 +43,7 @@ extern "C" __EXPORT int eulernav_bahrs_main(int argc, char *argv[]) { - PX4_INFO("This is an EULER-NAV Baro-Inertial AHRS driver!"); + PX4_INFO("Starting EULER-NAV BAHRS driver..."); 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 index 981431ccc767..7dfb130c6cd6 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -13,7 +13,7 @@ EulerNavDriver::EulerNavDriver(const char* device_name) : } else { - PX4_INFO("Failed to open serial port"); + PX4_ERR("Failed to open serial port"); _is_initialized = false; } } @@ -31,17 +31,14 @@ int EulerNavDriver::task_spawn(int argc, char *argv[]) int task_id = px4_task_spawn_cmd("bahrs", SCHED_DEFAULT, SCHED_PRIORITY_FAST_DRIVER, _task_stack_size, (px4_main_t)&run_trampoline, argv); - PX4_INFO("Task spawn."); - if (task_id < 0) { _task_id = -1; - PX4_INFO("Task spawn failed."); + PX4_ERR("Failed to spawn task."); } else { _task_id = task_id; - PX4_INFO("Task spawn succeeded."); } return (_task_id < 0) ? 1 : 0; @@ -53,8 +50,6 @@ EulerNavDriver* EulerNavDriver::instantiate(int argc, char *argv[]) const char* option_arg{nullptr}; const char* device_name{nullptr}; - PX4_INFO("Task instantiate."); - while (true) { int option{px4_getopt(argc, argv, "d:", &option_index, &option_arg)}; diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 83ad28aa3b32..3e55d268d79f 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -23,6 +23,7 @@ class EulerNavDriver : public ModuleBase /// @brief Required by ModuleBase static int print_usage(const char *reason = nullptr); + /// @brief The main loop of the task. void run() final; private: From c689ef0fa5321e1812cd79dab78726a02500f1e1 Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Thu, 19 Dec 2024 20:14:55 +0100 Subject: [PATCH 07/21] Poll serial port --- src/drivers/ins/eulernav_bahrs/CMakeLists.txt | 1 + src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp | 10 +++++++++- src/drivers/ins/eulernav_bahrs/eulernav_driver.h | 11 ++++++++++- 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt index 184cc1c19f2e..f373660b7b9d 100644 --- a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt +++ b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt @@ -38,4 +38,5 @@ px4_add_module( eulernav_driver.cpp eulernav_driver.h DEPENDS + ringbuffer ) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index 7dfb130c6cd6..b6f5e1f9ac2e 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -29,7 +29,7 @@ EulerNavDriver::~EulerNavDriver() int EulerNavDriver::task_spawn(int argc, char *argv[]) { int task_id = px4_task_spawn_cmd("bahrs", SCHED_DEFAULT, SCHED_PRIORITY_FAST_DRIVER, - _task_stack_size, (px4_main_t)&run_trampoline, argv); + TASK_STACK_SIZE, (px4_main_t)&run_trampoline, argv); if (task_id < 0) { @@ -88,5 +88,13 @@ void EulerNavDriver::run() { px4_usleep(1000000); PX4_INFO("Running the EulerNavDriver::run..."); + + // The declaration of readAtLeast() suggests that the timeout is in microseconds, + // but it seems to be a bug, as readAtLeast() forwards the value to another method + // that expects milliseconds. + const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer.begin(), _serial_read_buffer.capacity(), + MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_MS)}; + + (void)bytes_read; } } diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 3e55d268d79f..4467821189a0 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -3,6 +3,8 @@ #include #include #include +#include +#include class EulerNavDriver : public ModuleBase { @@ -27,8 +29,15 @@ class EulerNavDriver : public ModuleBase void run() final; private: - static constexpr int _task_stack_size{2048}; + static constexpr int TASK_STACK_SIZE{2048}; + static constexpr int SERIAL_READ_BUFFER_SIZE{128}; + static constexpr int MIN_BYTES_TO_READ{16}; + static constexpr int SERIAL_READ_TIMEOUT_MS{5}; + device::Serial _serial_port; + Ringbuffer _data_buffer; + px4::Array _serial_read_buffer; + bool _is_initialized{false}; }; From 0032aca2beabb9fe2c1f8ffd34e240a92ad63eae Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Thu, 19 Dec 2024 20:34:15 +0100 Subject: [PATCH 08/21] Push received bytes into the ring buffer --- .../ins/eulernav_bahrs/eulernav_driver.cpp | 30 +++++++++++++++++-- .../ins/eulernav_bahrs/eulernav_driver.h | 3 ++ 2 files changed, 30 insertions(+), 3 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index b6f5e1f9ac2e..c689d475d6fb 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -1,8 +1,9 @@ #include "eulernav_driver.h" #include -EulerNavDriver::EulerNavDriver(const char* device_name) : - _serial_port{device_name, 115200, ByteSize::EightBits, Parity::None, StopBits::One, FlowControl::Disabled} +EulerNavDriver::EulerNavDriver(const char* device_name) + : _serial_port{device_name, 115200, ByteSize::EightBits, Parity::None, StopBits::One, FlowControl::Disabled} + , _data_buffer{} { _serial_port.open(); @@ -16,6 +17,15 @@ EulerNavDriver::EulerNavDriver(const char* device_name) : PX4_ERR("Failed to open serial port"); _is_initialized = false; } + + if (_is_initialized) + { + if (false == _data_buffer.allocate(DATA_BUFFER_SIZE)) + { + PX4_ERR("Failed to allocate data buffer"); + _is_initialized = false; + } + } } EulerNavDriver::~EulerNavDriver() @@ -24,6 +34,8 @@ EulerNavDriver::~EulerNavDriver() { _serial_port.close(); } + + _data_buffer.deallocate(); } int EulerNavDriver::task_spawn(int argc, char *argv[]) @@ -95,6 +107,18 @@ void EulerNavDriver::run() const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer.begin(), _serial_read_buffer.capacity(), MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_MS)}; - (void)bytes_read; + if (bytes_read > 0) + { + if (false == _data_buffer.push_back(_serial_read_buffer.begin(), _serial_read_buffer.size())) + { + PX4_ERR("No space in data buffer"); + } + } + + processDataBuffer(); } } + +void EulerNavDriver::processDataBuffer() +{ +} diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 4467821189a0..70a87d172803 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -33,6 +33,9 @@ class EulerNavDriver : public ModuleBase static constexpr int SERIAL_READ_BUFFER_SIZE{128}; static constexpr int MIN_BYTES_TO_READ{16}; static constexpr int SERIAL_READ_TIMEOUT_MS{5}; + static constexpr int DATA_BUFFER_SIZE{512}; + + void processDataBuffer(); device::Serial _serial_port; Ringbuffer _data_buffer; From 60d8b432a83aa6e4c19e694ff7991140f8cd1c63 Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Sun, 22 Dec 2024 16:50:59 +0100 Subject: [PATCH 09/21] Process data buffer (1) --- src/drivers/ins/eulernav_bahrs/CMakeLists.txt | 2 + .../eulernav_bahrs/bahrs/CSerialProtocol.h | 330 ++++++++++++++++++ .../ins/eulernav_bahrs/eulernav_driver.cpp | 26 ++ .../ins/eulernav_bahrs/eulernav_driver.h | 4 + 4 files changed, 362 insertions(+) create mode 100644 src/drivers/ins/eulernav_bahrs/bahrs/CSerialProtocol.h diff --git a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt index f373660b7b9d..e803bf4d7bba 100644 --- a/src/drivers/ins/eulernav_bahrs/CMakeLists.txt +++ b/src/drivers/ins/eulernav_bahrs/CMakeLists.txt @@ -33,6 +33,8 @@ px4_add_module( MODULE drivers__ins__eulerav_bahrs MAIN eulernav_bahrs + INCLUDES + bahrs SRCS eulernav_bahrs_main.cpp eulernav_driver.cpp 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 000000000000..adb75ecac67a --- /dev/null +++ b/src/drivers/ins/eulernav_bahrs/bahrs/CSerialProtocol.h @@ -0,0 +1,330 @@ +/** + * @file CSerialProtocol.h + * @brief Declaration of the BAHRS serial protocol + * @author Fedor Baklanov + * @date 07 June 2022 + */ + +#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) + +/** + * @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_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index c689d475d6fb..b28f98c46d18 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -1,5 +1,6 @@ #include "eulernav_driver.h" #include +#include "CSerialProtocol.h" EulerNavDriver::EulerNavDriver(const char* device_name) : _serial_port{device_name, 115200, ByteSize::EightBits, Parity::None, StopBits::One, FlowControl::Disabled} @@ -121,4 +122,29 @@ void EulerNavDriver::run() void EulerNavDriver::processDataBuffer() { + static_assert(MIN_MESSAGE_LENGTH >= (sizeof(CSerialProtocol::SMessageHeader) + sizeof(CSerialProtocol::CrcType_t))); + + // We start from the first element and search for a message header. We remove + // bytes from the buffer until a header is found. Then we parse the found message + // and remove the corresponding data bytes. If a full message has not been received + // yet, we leave the message bytes in the buffer and wait for more data to arrive. + + while (_data_buffer.space_used() >= MIN_MESSAGE_LENGTH) + { + uint8_t sync_byte{0U}; + + _data_buffer.pop_front(&sync_byte, 1); + + if (CSerialProtocol::uMarker1_ == sync_byte) + { + sync_byte = 0U; + _data_buffer.pop_front(&sync_byte, 1); + + if (CSerialProtocol::uMarker2_ == sync_byte) + { + + } + } + } + } diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 70a87d172803..7b22c8838ee3 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -35,11 +35,15 @@ class EulerNavDriver : public ModuleBase static constexpr int SERIAL_READ_TIMEOUT_MS{5}; static constexpr int DATA_BUFFER_SIZE{512}; + // Min length of a valid message. 5 bytes header + 4 bytes CRC + padding to 12 (multiple of 32 bit words) + static constexpr int MIN_MESSAGE_LENGTH{12}; + void processDataBuffer(); device::Serial _serial_port; Ringbuffer _data_buffer; px4::Array _serial_read_buffer; + px4::Array _message_storage; bool _is_initialized{false}; }; From 5c8d511f9550589a8e53e7d812b7efe47107ee4f Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Mon, 23 Dec 2024 10:55:22 +0100 Subject: [PATCH 10/21] Process data buffer (2) --- .../ins/eulernav_bahrs/eulernav_driver.cpp | 65 +++++++++++++++++-- .../ins/eulernav_bahrs/eulernav_driver.h | 2 + 2 files changed, 61 insertions(+), 6 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index b28f98c46d18..b85174f06c34 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -132,19 +132,72 @@ void EulerNavDriver::processDataBuffer() while (_data_buffer.space_used() >= MIN_MESSAGE_LENGTH) { uint8_t sync_byte{0U}; + uint16_t protocol_ver{0U}; + uint8_t message_code{0U}; + bool error{false}; - _data_buffer.pop_front(&sync_byte, 1); - - if (CSerialProtocol::uMarker1_ == sync_byte) + if (0 == _data_buffer.pop_front(&sync_byte, 1)) { - sync_byte = 0U; - _data_buffer.pop_front(&sync_byte, 1); + error = true; + } - if (CSerialProtocol::uMarker2_ == sync_byte) + if (false == error) + { + if (CSerialProtocol::uMarker1_ == sync_byte) { + sync_byte = 0U; + + if (0 == _data_buffer.pop_front(&sync_byte, 1)) + { + error = true; + } + + if (false == error) + { + if (CSerialProtocol::uMarker2_ == sync_byte) + { + error = retrieveProtocolVersionAndMessageType(_data_buffer, protocol_ver, message_code); + } + } + } + } + if (false == error) + { + switch (static_cast(message_code)) + { + case CSerialProtocol::EMessageIds::eInertialData: + break; + case CSerialProtocol::EMessageIds::eNavigationData: + break; + default: + break; } } } } + +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; +} diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 7b22c8838ee3..d189d1886e03 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -40,6 +40,8 @@ class EulerNavDriver : public ModuleBase void processDataBuffer(); + static bool retrieveProtocolVersionAndMessageType(Ringbuffer& buffer, uint16_t& protocol_ver, uint8_t& message_code); + device::Serial _serial_port; Ringbuffer _data_buffer; px4::Array _serial_read_buffer; From 63b77546f8c680241da547ffd007db9f323c8951 Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Fri, 27 Dec 2024 20:37:22 +0100 Subject: [PATCH 11/21] Process data buffer (3) --- .../ins/eulernav_bahrs/eulernav_driver.cpp | 92 +++++++++++++++++-- .../ins/eulernav_bahrs/eulernav_driver.h | 7 +- 2 files changed, 90 insertions(+), 9 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index b85174f06c34..cc014271c31a 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -1,6 +1,5 @@ #include "eulernav_driver.h" #include -#include "CSerialProtocol.h" EulerNavDriver::EulerNavDriver(const char* device_name) : _serial_port{device_name, 115200, ByteSize::EightBits, Parity::None, StopBits::One, FlowControl::Disabled} @@ -123,6 +122,7 @@ void EulerNavDriver::run() void EulerNavDriver::processDataBuffer() { static_assert(MIN_MESSAGE_LENGTH >= (sizeof(CSerialProtocol::SMessageHeader) + sizeof(CSerialProtocol::CrcType_t))); + using EMessageIds = CSerialProtocol::EMessageIds; // We start from the first element and search for a message header. We remove // bytes from the buffer until a header is found. Then we parse the found message @@ -164,14 +164,47 @@ void EulerNavDriver::processDataBuffer() if (false == error) { - switch (static_cast(message_code)) + const auto message_id{static_cast(message_code)}; + const auto message_length{getMessageLength(message_id)}; + + if (message_length > 0) { - case CSerialProtocol::EMessageIds::eInertialData: - break; - case CSerialProtocol::EMessageIds::eNavigationData: - break; - default: - break; + const auto bytes_to_retrieve{message_length - sizeof(CSerialProtocol::SMessageHeader)}; + + if ((bytes_to_retrieve > 0) && + (_data_buffer.space_used() >= bytes_to_retrieve) && + (bytes_to_retrieve + sizeof(CSerialProtocol::SMessageHeader)) <= _message_storage.capacity()) + { + uint8_t* bytes = reinterpret_cast(_message_storage.begin()); + bytes[0] = CSerialProtocol::uMarker1_; + bytes[1] = CSerialProtocol::uMarker2_; + bytes[2] = reinterpret_cast(&protocol_ver)[0]; + bytes[3] = reinterpret_cast(&protocol_ver)[1]; + bytes[4] = message_code; + + if (bytes_to_retrieve != _data_buffer.pop_front(bytes + 5, bytes_to_retrieve)) + { + error = true; + } + + if (false == error) + { + uint32_t message_length_in_words{message_length / sizeof(uint32_t)}; + const uint32_t actual_crc{crc32(_message_storage.begin(), message_length_in_words - 1)}; + const uint32_t expected_crc = _message_storage[message_length_in_words - 1]; + + if (expected_crc != actual_crc) + { + error = true; + } + + } + + if (false == error) + { + // decode message + } + } } } } @@ -201,3 +234,46 @@ bool EulerNavDriver::retrieveProtocolVersionAndMessageType(Ringbuffer& buffer, u return status; } + +int 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 index d189d1886e03..0c1aaaaa62a8 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -5,6 +5,7 @@ #include #include #include +#include "CSerialProtocol.h" class EulerNavDriver : public ModuleBase { @@ -42,10 +43,14 @@ class EulerNavDriver : public ModuleBase static bool retrieveProtocolVersionAndMessageType(Ringbuffer& buffer, uint16_t& protocol_ver, uint8_t& message_code); + static int getMessageLength(CSerialProtocol::EMessageIds messsage_id); + + static uint32_t crc32(const uint32_t* buf, size_t len); + device::Serial _serial_port; Ringbuffer _data_buffer; px4::Array _serial_read_buffer; - px4::Array _message_storage; + px4::Array _message_storage; bool _is_initialized{false}; }; From be62668326537dff9e9bf6231e61e2900f8d98ef Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Sat, 28 Dec 2024 22:16:57 +0100 Subject: [PATCH 12/21] Process data buffer (4) --- .../ins/eulernav_bahrs/eulernav_driver.cpp | 156 ++++++++++++------ .../ins/eulernav_bahrs/eulernav_driver.h | 4 +- 2 files changed, 112 insertions(+), 48 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index cc014271c31a..1ab3b26e09db 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -104,16 +104,26 @@ void EulerNavDriver::run() // The declaration of readAtLeast() suggests that the timeout is in microseconds, // but it seems to be a bug, as readAtLeast() forwards the value to another method // that expects milliseconds. - const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer.begin(), _serial_read_buffer.capacity(), - MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_MS)}; + /*const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer.begin(), _serial_read_buffer.capacity(), + MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_MS)};*/ - if (bytes_read > 0) + uint8_t test_data[] = {0x4e, 0x45, 0x02, 0x00, 0x01, 0x34, 0xce, 0xff, 0xd3, 0xff, 0x70, 0xe6, 0x05, 0x00, 0xfa, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x32, 0x7d, 0x13, 0x65, \ + 0x4e, 0x45, 0x02, 0x00, 0x02, 0x9a, 0xb0, 0x22, 0xfe, 0xff, 0x3b, 0x00, 0xa3, 0xff, 0x30, 0xc8, 0x1f, 0x00, 0x00, 0x00, 0x82, 0x98, 0xa3, 0x42, \ + 0x4e, 0x45, 0x02, 0x00, 0x01, 0x35, 0xce, 0xff, 0xe2, 0xff, 0x65, 0xe6, 0x00, 0x00, 0xf9, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x1d, 0x14, 0xf1, 0xf0, \ + 0x4e, 0x45, 0x02, 0x00, 0x01, 0x36, 0xcf, 0xff, 0xda, 0xff, 0x6e, 0xe6, 0x00, 0x00, 0xf8, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x9f, 0x1b, 0x2b, 0xd1 + }; + + _data_buffer.push_back(test_data, sizeof(test_data)); + + printf("Data buffer size: %d\n", _data_buffer.space_used()); + + /*if (bytes_read > 0) { if (false == _data_buffer.push_back(_serial_read_buffer.begin(), _serial_read_buffer.size())) { PX4_ERR("No space in data buffer"); } - } + }*/ processDataBuffer(); } @@ -129,6 +139,8 @@ void EulerNavDriver::processDataBuffer() // and remove the corresponding data bytes. If a full message has not been received // yet, we leave the message bytes in the buffer and wait for more data to arrive. + bool message_header_found{false}; + while (_data_buffer.space_used() >= MIN_MESSAGE_LENGTH) { uint8_t sync_byte{0U}; @@ -136,76 +148,128 @@ void EulerNavDriver::processDataBuffer() uint8_t message_code{0U}; bool error{false}; - if (0 == _data_buffer.pop_front(&sync_byte, 1)) + if (false == message_header_found) { - error = true; - } - - if (false == error) - { - if (CSerialProtocol::uMarker1_ == sync_byte) + if (0 == _data_buffer.pop_front(&sync_byte, 1)) { - sync_byte = 0U; + error = true; + } - if (0 == _data_buffer.pop_front(&sync_byte, 1)) + if (false == error) + { + if (CSerialProtocol::uMarker1_ == sync_byte) { - error = true; - } + sync_byte = 0U; + printf("Found sync byte 1.\n"); - if (false == error) - { - if (CSerialProtocol::uMarker2_ == sync_byte) + if (0 == _data_buffer.pop_front(&sync_byte, 1)) + { + error = true; + } + + if (false == error) { - error = retrieveProtocolVersionAndMessageType(_data_buffer, protocol_ver, message_code); + if (CSerialProtocol::uMarker2_ == sync_byte) + { + if (false == retrieveProtocolVersionAndMessageType(_data_buffer, protocol_ver, message_code)) + { + error = true; + } + + printf("Found sync byte 2.\n"); + + if (false == error) + { + printf("Message found. Protocol version: %d. Message code: %d.\n", protocol_ver, message_code); + message_header_found = true; + } + else + { + printf("Failed to retrieve message attributes.\n"); + } + } } } } } - if (false == error) + if (message_header_found) { - const auto message_id{static_cast(message_code)}; - const auto message_length{getMessageLength(message_id)}; + const EMessageIds message_id{static_cast(message_code)}; + const int32_t message_length{getMessageLength(message_id)}; if (message_length > 0) { - const auto bytes_to_retrieve{message_length - sizeof(CSerialProtocol::SMessageHeader)}; + const uint32_t bytes_to_retrieve{static_cast(message_length) - sizeof(CSerialProtocol::SMessageHeader)}; + printf("Starting to parse the message.\n"); - if ((bytes_to_retrieve > 0) && - (_data_buffer.space_used() >= bytes_to_retrieve) && - (bytes_to_retrieve + sizeof(CSerialProtocol::SMessageHeader)) <= _message_storage.capacity()) + if (_data_buffer.space_used() >= bytes_to_retrieve) { - uint8_t* bytes = reinterpret_cast(_message_storage.begin()); - bytes[0] = CSerialProtocol::uMarker1_; - bytes[1] = CSerialProtocol::uMarker2_; - bytes[2] = reinterpret_cast(&protocol_ver)[0]; - bytes[3] = reinterpret_cast(&protocol_ver)[1]; - bytes[4] = message_code; - - if (bytes_to_retrieve != _data_buffer.pop_front(bytes + 5, bytes_to_retrieve)) + if ((bytes_to_retrieve > 0) && + (bytes_to_retrieve + sizeof(CSerialProtocol::SMessageHeader)) <= sizeof(_message_storage)) { - error = true; - } + printf("Preparing CRC.\n"); + uint8_t* bytes = reinterpret_cast(_message_storage); + bytes[0] = CSerialProtocol::uMarker1_; + bytes[1] = CSerialProtocol::uMarker2_; + bytes[2] = reinterpret_cast(&protocol_ver)[0]; + bytes[3] = reinterpret_cast(&protocol_ver)[1]; + bytes[4] = message_code; - if (false == error) - { - uint32_t message_length_in_words{message_length / sizeof(uint32_t)}; - const uint32_t actual_crc{crc32(_message_storage.begin(), message_length_in_words - 1)}; - const uint32_t expected_crc = _message_storage[message_length_in_words - 1]; + printf("Bytes to retrieve: %lu.\n.", bytes_to_retrieve); - if (expected_crc != actual_crc) + if (bytes_to_retrieve != _data_buffer.pop_front(bytes + 5, bytes_to_retrieve)) { error = true; } - } + printf("Message:\n"); - if (false == error) - { - // decode message + for (int i = 0; i < message_length; ++i) + { + printf("%X, ", bytes[i]); + } + printf("\n"); + + if (false == error) + { + 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) + { + error = true; + printf("CRC FAILED for: message code: %d.\n", message_code); + printf("Message length in words: %lu.\n", message_length_in_words); + printf("Expected CRC: %lu. Actual CRC: %lu.\n", expected_crc, actual_crc); + } + else + { + printf("CRC PASSED for: message code: %d.\n", message_code); + } + + } + + if (false == error) + { + // decode message + } } + + message_header_found = false; + } + else + { + // wait for more data to arrive } + } + else + { + message_header_found = false; + } + } } diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 0c1aaaaa62a8..6ea0894c03f5 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -49,8 +49,8 @@ class EulerNavDriver : public ModuleBase device::Serial _serial_port; Ringbuffer _data_buffer; - px4::Array _serial_read_buffer; - px4::Array _message_storage; + uint8_t _serial_read_buffer[SERIAL_READ_BUFFER_SIZE]; + uint32_t _message_storage[8]; bool _is_initialized{false}; }; From fd65f94cb50988178c29bca561d721aa30c5905e Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Thu, 2 Jan 2025 17:58:43 +0100 Subject: [PATCH 13/21] Process data buffer (5) --- .../ins/eulernav_bahrs/eulernav_driver.cpp | 248 ++++++++++-------- .../ins/eulernav_bahrs/eulernav_driver.h | 9 +- 2 files changed, 143 insertions(+), 114 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index 1ab3b26e09db..be5075734d4a 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -104,26 +104,52 @@ void EulerNavDriver::run() // The declaration of readAtLeast() suggests that the timeout is in microseconds, // but it seems to be a bug, as readAtLeast() forwards the value to another method // that expects milliseconds. - /*const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer.begin(), _serial_read_buffer.capacity(), - MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_MS)};*/ + const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer, sizeof(_serial_read_buffer), + MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_MS)}; - uint8_t test_data[] = {0x4e, 0x45, 0x02, 0x00, 0x01, 0x34, 0xce, 0xff, 0xd3, 0xff, 0x70, 0xe6, 0x05, 0x00, 0xfa, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x32, 0x7d, 0x13, 0x65, \ - 0x4e, 0x45, 0x02, 0x00, 0x02, 0x9a, 0xb0, 0x22, 0xfe, 0xff, 0x3b, 0x00, 0xa3, 0xff, 0x30, 0xc8, 0x1f, 0x00, 0x00, 0x00, 0x82, 0x98, 0xa3, 0x42, \ - 0x4e, 0x45, 0x02, 0x00, 0x01, 0x35, 0xce, 0xff, 0xe2, 0xff, 0x65, 0xe6, 0x00, 0x00, 0xf9, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x1d, 0x14, 0xf1, 0xf0, \ - 0x4e, 0x45, 0x02, 0x00, 0x01, 0x36, 0xcf, 0xff, 0xda, 0xff, 0x6e, 0xe6, 0x00, 0x00, 0xf8, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x9f, 0x1b, 0x2b, 0xd1 - }; - - _data_buffer.push_back(test_data, sizeof(test_data)); + if (bytes_read > 0) + { + if (false == _data_buffer.push_back(_serial_read_buffer, bytes_read)) + { + PX4_ERR("No space in data buffer"); + } + } - printf("Data buffer size: %d\n", _data_buffer.space_used()); +#ifdef EULERNAV_BAHRS_PARSER_DEBUG + static int counter = 0; - /*if (bytes_read > 0) + if (counter < 6) { - if (false == _data_buffer.push_back(_serial_read_buffer.begin(), _serial_read_buffer.size())) + uint8_t test_data_part_1[] = { 0x05, 0xce, 0xF0, 0x00, 0xab, 0xcd, 0x12, // Rubbish + 0x4e, 0x45, 0x02, 0x00, 0x01, 0x34, 0xce, 0xff, 0xd3, 0xff, 0x70, 0xe6, 0x05, 0x00, 0xfa, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x32, 0x7d, 0x13, 0x65, // Inertial message + 0x4e, 0x45, 0x02, 0x00, 0x02, 0x9a, 0xb0, 0x22, 0xfe, 0xff, 0x3b, 0x00, 0xa3, 0xff, 0x30, 0xc8, 0x1f, 0x00, 0x00, 0x00, 0x82, 0x98, 0xa3, 0x42, // Navigation message + 0x4e, 0x45, 0x02, 0x00, 0x01, 0x35, 0xce, 0xff, 0xe2, 0xff // Inertial message part 1 + }; + + uint8_t test_data_part_2[] = { 0x65, 0xe6, 0x00, 0x00, 0xf9, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x1d, 0x14, 0xf1, 0xf0, // Inertial message part 2 + 0x4e, 0x45, 0x02, 0x00, 0x01, 0x36, 0xcf, 0xff, 0xda, 0xff, 0x6e, 0xe6, 0x00, 0x00, 0xf8, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x9f, 0x1b, 0x2b, 0xd1, // Inertial message + 0x00, 0x4e, 0xcd, 0x34, 0x77, 0x86, 0xFF // Rubbish + }; + + uint8_t* data_to_push{nullptr}; + size_t byte_count_to_push{0U}; + + if (counter % 2 == 0) { - PX4_ERR("No space in data buffer"); + data_to_push = test_data_part_1; + byte_count_to_push = sizeof(test_data_part_1); + } + else + { + data_to_push = test_data_part_2; + byte_count_to_push = sizeof(test_data_part_2); } - }*/ + + _data_buffer.push_back(data_to_push, byte_count_to_push); + printf("Data buffer size: %d\n", _data_buffer.space_used()); + ++counter; + } +#endif // EULERNAV_BAHRS_PARSER_DEBUG processDataBuffer(); } @@ -134,145 +160,121 @@ void EulerNavDriver::processDataBuffer() static_assert(MIN_MESSAGE_LENGTH >= (sizeof(CSerialProtocol::SMessageHeader) + sizeof(CSerialProtocol::CrcType_t))); using EMessageIds = CSerialProtocol::EMessageIds; - // We start from the first element and search for a message header. We remove - // bytes from the buffer until a header is found. Then we parse the found message - // and remove the corresponding data bytes. If a full message has not been received - // yet, we leave the message bytes in the buffer and wait for more data to arrive. - - bool message_header_found{false}; - while (_data_buffer.space_used() >= MIN_MESSAGE_LENGTH) { - uint8_t sync_byte{0U}; - uint16_t protocol_ver{0U}; - uint8_t message_code{0U}; - bool error{false}; - - if (false == message_header_found) + if (false == _next_message_detected) { - if (0 == _data_buffer.pop_front(&sync_byte, 1)) - { - error = true; - } + _next_message_detected = findNextMessageHeader(_data_buffer); - if (false == error) + if (_next_message_detected) { - if (CSerialProtocol::uMarker1_ == sync_byte) + if (false == retrieveProtocolVersionAndMessageType(_data_buffer, _next_message_protocol_version, _next_message_code)) { - sync_byte = 0U; - printf("Found sync byte 1.\n"); - - if (0 == _data_buffer.pop_front(&sync_byte, 1)) - { - error = true; - } - - if (false == error) - { - if (CSerialProtocol::uMarker2_ == sync_byte) - { - if (false == retrieveProtocolVersionAndMessageType(_data_buffer, protocol_ver, message_code)) - { - error = true; - } - - printf("Found sync byte 2.\n"); - - if (false == error) - { - printf("Message found. Protocol version: %d. Message code: %d.\n", protocol_ver, message_code); - message_header_found = true; - } - else - { - printf("Failed to retrieve message attributes.\n"); - } - } - } + _next_message_detected = false; } } } - if (message_header_found) + if (_next_message_detected) { - const EMessageIds message_id{static_cast(message_code)}; + const EMessageIds message_id{static_cast(_next_message_code)}; const int32_t message_length{getMessageLength(message_id)}; - if (message_length > 0) + if (message_length < 0) { - const uint32_t bytes_to_retrieve{static_cast(message_length) - sizeof(CSerialProtocol::SMessageHeader)}; - printf("Starting to parse the message.\n"); - - if (_data_buffer.space_used() >= bytes_to_retrieve) - { - if ((bytes_to_retrieve > 0) && - (bytes_to_retrieve + sizeof(CSerialProtocol::SMessageHeader)) <= sizeof(_message_storage)) - { - printf("Preparing CRC.\n"); - uint8_t* bytes = reinterpret_cast(_message_storage); - bytes[0] = CSerialProtocol::uMarker1_; - bytes[1] = CSerialProtocol::uMarker2_; - bytes[2] = reinterpret_cast(&protocol_ver)[0]; - bytes[3] = reinterpret_cast(&protocol_ver)[1]; - bytes[4] = message_code; + // The message is unknown or not supported + _next_message_detected = false; + } - printf("Bytes to retrieve: %lu.\n.", bytes_to_retrieve); + if (_next_message_detected) + { + const int32_t bytes_to_retrieve{message_length - static_cast(sizeof(CSerialProtocol::SMessageHeader))}; - if (bytes_to_retrieve != _data_buffer.pop_front(bytes + 5, bytes_to_retrieve)) - { - error = true; - } + if (bytes_to_retrieve < 0) + { + PX4_ERR("Message length is less than the message header length for the protocol version %u, message code %u.\n", _next_message_protocol_version, _next_message_code); + _next_message_detected = false; + } + else if ((bytes_to_retrieve + static_cast(sizeof(CSerialProtocol::SMessageHeader))) > static_cast(sizeof(_message_storage))) + { + PX4_ERR("Protocol version %u, message code %u: message storage is too small.\n", _next_message_protocol_version, _next_message_code); + _next_message_detected = false; + } + else if (static_cast(_data_buffer.space_used()) < bytes_to_retrieve) + { + // Do nothing and wait for more bytes to arrive. + } + else + { + // Get message from the data buffer + uint8_t* bytes{reinterpret_cast(_message_storage)}; - printf("Message:\n"); + bytes[0] = CSerialProtocol::uMarker1_; + bytes[1] = CSerialProtocol::uMarker2_; + bytes[2] = reinterpret_cast(&_next_message_protocol_version)[0]; + bytes[3] = reinterpret_cast(&_next_message_protocol_version)[1]; + bytes[4] = _next_message_code; - for (int i = 0; i < message_length; ++i) + if (static_cast(bytes_to_retrieve) == _data_buffer.pop_front(bytes + sizeof(CSerialProtocol::SMessageHeader), bytes_to_retrieve)) + { + if ((message_length % sizeof(uint32_t)) != 0U) { - printf("%X, ", bytes[i]); + PX4_ERR("Protocol version %u, message code %u: message length is not a multiple of 32 bit words.\n", _next_message_protocol_version, _next_message_code); } - printf("\n"); - - if (false == error) + else { - uint32_t message_length_in_words{message_length / sizeof(uint32_t)}; + 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) { - error = true; - printf("CRC FAILED for: message code: %d.\n", message_code); - printf("Message length in words: %lu.\n", message_length_in_words); - printf("Expected CRC: %lu. Actual CRC: %lu.\n", expected_crc, actual_crc); + PX4_INFO("Protocol version %u, message code %u: CRC failed.\n", _next_message_protocol_version, _next_message_code); } else { - printf("CRC PASSED for: message code: %d.\n", message_code); + decodeMessageAndPublishData(bytes, message_id); } - - } - - if (false == error) - { - // decode message } } - message_header_found = false; + _next_message_detected = false; } - else + } + + } + } + +} + +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)) + { + if (CSerialProtocol::uMarker1_ == sync_byte) + { + sync_byte = 0U; + + if (1 == buffer.pop_front(&sync_byte, 1)) { - // wait for more data to arrive + if (CSerialProtocol::uMarker2_ == sync_byte) + { + result = true; + break; + } } } - else - { - message_header_found = false; - } - } } + return result; } bool EulerNavDriver::retrieveProtocolVersionAndMessageType(Ringbuffer& buffer, uint16_t& protocol_ver, uint8_t& message_code) @@ -299,7 +301,27 @@ bool EulerNavDriver::retrieveProtocolVersionAndMessageType(Ringbuffer& buffer, u return status; } -int EulerNavDriver::getMessageLength(CSerialProtocol::EMessageIds messsage_id) +void EulerNavDriver::decodeMessageAndPublishData(const uint8_t* data, CSerialProtocol::EMessageIds messsage_id) +{ + static uint32_t inertial_message_counter{0U}; + static uint32_t navigation_message_counter{0U}; + + switch (messsage_id) + { + case CSerialProtocol::EMessageIds::eInertialData: + ++inertial_message_counter; + break; + case CSerialProtocol::EMessageIds::eNavigationData: + ++navigation_message_counter; + break; + default: + break; + } + + PX4_INFO("Inertial messages received: %lu. Navigation messages received: %lu.\n", inertial_message_counter, navigation_message_counter); +} + +int32_t EulerNavDriver::getMessageLength(CSerialProtocol::EMessageIds messsage_id) { int message_length{-1}; diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 6ea0894c03f5..850ca9acc505 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -41,9 +41,13 @@ class EulerNavDriver : public ModuleBase void processDataBuffer(); + static bool findNextMessageHeader(Ringbuffer& buffer); + static bool retrieveProtocolVersionAndMessageType(Ringbuffer& buffer, uint16_t& protocol_ver, uint8_t& message_code); - static int getMessageLength(CSerialProtocol::EMessageIds messsage_id); + static void decodeMessageAndPublishData(const uint8_t* data, CSerialProtocol::EMessageIds messsage_id); + + static int32_t getMessageLength(CSerialProtocol::EMessageIds messsage_id); static uint32_t crc32(const uint32_t* buf, size_t len); @@ -51,6 +55,9 @@ class EulerNavDriver : public ModuleBase Ringbuffer _data_buffer; uint8_t _serial_read_buffer[SERIAL_READ_BUFFER_SIZE]; uint32_t _message_storage[8]; + bool _next_message_detected{false}; + uint16_t _next_message_protocol_version{0U}; + uint8_t _next_message_code{0U}; bool _is_initialized{false}; }; From c577940f8f4f417e194474a766222b41067823df Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Fri, 3 Jan 2025 17:47:47 +0100 Subject: [PATCH 14/21] Process data buffer (6) --- .../ins/eulernav_bahrs/eulernav_driver.cpp | 74 ++++++++----------- .../ins/eulernav_bahrs/eulernav_driver.h | 13 +++- 2 files changed, 42 insertions(+), 45 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index be5075734d4a..cf0963197f85 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -96,16 +96,16 @@ int EulerNavDriver::print_usage(const char *reason) void EulerNavDriver::run() { + const auto start_time{hrt_absolute_time()}; + auto time_of_previous_statistics_print{start_time}; + while(false == should_exit()) { - px4_usleep(1000000); - PX4_INFO("Running the EulerNavDriver::run..."); - - // The declaration of readAtLeast() suggests that the timeout is in microseconds, - // but it seems to be a bug, as readAtLeast() forwards the value to another method - // that expects milliseconds. +#ifndef EULERNAV_BAHRS_PARSER_DEBUG const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer, sizeof(_serial_read_buffer), - MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_MS)}; + MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_US)}; + + _statistics._total_bytes_read += bytes_read; if (bytes_read > 0) { @@ -114,8 +114,7 @@ void EulerNavDriver::run() PX4_ERR("No space in data buffer"); } } - -#ifdef EULERNAV_BAHRS_PARSER_DEBUG +#else static int counter = 0; if (counter < 6) @@ -152,6 +151,13 @@ void EulerNavDriver::run() #endif // EULERNAV_BAHRS_PARSER_DEBUG processDataBuffer(); + + if (hrt_elapsed_time(&time_of_previous_statistics_print) >= STATISTICS_PRINT_PERIOD) + { + PX4_INFO("Elapsed time: %llu [us]. Total bytes received: %lu.\n", hrt_elapsed_time(&start_time), _statistics._total_bytes_read); + PX4_INFO("Inertial messages received: %lu. Navigation messages received: %lu.\n", _statistics._inertial_message_counter, _statistics._navigation_message_counter); + time_of_previous_statistics_print = hrt_absolute_time(); + } } } @@ -177,12 +183,15 @@ void EulerNavDriver::processDataBuffer() if (_next_message_detected) { + static_assert(sizeof(CSerialProtocol::SMessageHeader) < MIN_MESSAGE_LENGTH); + const EMessageIds message_id{static_cast(_next_message_code)}; const int32_t message_length{getMessageLength(message_id)}; - if (message_length < 0) + if ((message_length < 0) || (message_length < MIN_MESSAGE_LENGTH) || + (message_length > static_cast(sizeof(_message_storage))) || ((message_length % sizeof(uint32_t)) != 0U)) { - // The message is unknown or not supported + // The message is unknown, not supported, or does not fit into the temporary storage. _next_message_detected = false; } @@ -190,19 +199,10 @@ void EulerNavDriver::processDataBuffer() { const int32_t bytes_to_retrieve{message_length - static_cast(sizeof(CSerialProtocol::SMessageHeader))}; - if (bytes_to_retrieve < 0) - { - PX4_ERR("Message length is less than the message header length for the protocol version %u, message code %u.\n", _next_message_protocol_version, _next_message_code); - _next_message_detected = false; - } - else if ((bytes_to_retrieve + static_cast(sizeof(CSerialProtocol::SMessageHeader))) > static_cast(sizeof(_message_storage))) - { - PX4_ERR("Protocol version %u, message code %u: message storage is too small.\n", _next_message_protocol_version, _next_message_code); - _next_message_detected = false; - } - else if (static_cast(_data_buffer.space_used()) < bytes_to_retrieve) + if (static_cast(_data_buffer.space_used()) < bytes_to_retrieve) { // Do nothing and wait for more bytes to arrive. + break; } else { @@ -217,24 +217,17 @@ void EulerNavDriver::processDataBuffer() if (static_cast(bytes_to_retrieve) == _data_buffer.pop_front(bytes + sizeof(CSerialProtocol::SMessageHeader), bytes_to_retrieve)) { - if ((message_length % sizeof(uint32_t)) != 0U) + 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) { - PX4_ERR("Protocol version %u, message code %u: message length is not a multiple of 32 bit words.\n", _next_message_protocol_version, _next_message_code); + PX4_INFO("Protocol version %u, message code %u: CRC failed.\n", _next_message_protocol_version, _next_message_code); } else { - 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) - { - PX4_INFO("Protocol version %u, message code %u: CRC failed.\n", _next_message_protocol_version, _next_message_code); - } - else - { - decodeMessageAndPublishData(bytes, message_id); - } + decodeMessageAndPublishData(bytes, message_id); } } @@ -303,22 +296,17 @@ bool EulerNavDriver::retrieveProtocolVersionAndMessageType(Ringbuffer& buffer, u void EulerNavDriver::decodeMessageAndPublishData(const uint8_t* data, CSerialProtocol::EMessageIds messsage_id) { - static uint32_t inertial_message_counter{0U}; - static uint32_t navigation_message_counter{0U}; - switch (messsage_id) { case CSerialProtocol::EMessageIds::eInertialData: - ++inertial_message_counter; + ++_statistics._inertial_message_counter; break; case CSerialProtocol::EMessageIds::eNavigationData: - ++navigation_message_counter; + ++_statistics._navigation_message_counter; break; default: break; } - - PX4_INFO("Inertial messages received: %lu. Navigation messages received: %lu.\n", inertial_message_counter, navigation_message_counter); } int32_t EulerNavDriver::getMessageLength(CSerialProtocol::EMessageIds messsage_id) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 850ca9acc505..96b6d48f15e3 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -30,11 +30,19 @@ class EulerNavDriver : public ModuleBase void run() final; private: + struct Statistics + { + uint32_t _total_bytes_read{0U}; + uint32_t _inertial_message_counter{0U}; + uint32_t _navigation_message_counter{0U}; + }; + static constexpr int TASK_STACK_SIZE{2048}; static constexpr int SERIAL_READ_BUFFER_SIZE{128}; static constexpr int MIN_BYTES_TO_READ{16}; - static constexpr int SERIAL_READ_TIMEOUT_MS{5}; + static constexpr int SERIAL_READ_TIMEOUT_US{5000}; static constexpr int DATA_BUFFER_SIZE{512}; + static constexpr hrt_abstime STATISTICS_PRINT_PERIOD{5000000U}; // Min length of a valid message. 5 bytes header + 4 bytes CRC + padding to 12 (multiple of 32 bit words) static constexpr int MIN_MESSAGE_LENGTH{12}; @@ -45,7 +53,7 @@ class EulerNavDriver : public ModuleBase static bool retrieveProtocolVersionAndMessageType(Ringbuffer& buffer, uint16_t& protocol_ver, uint8_t& message_code); - static void decodeMessageAndPublishData(const uint8_t* data, CSerialProtocol::EMessageIds messsage_id); + void decodeMessageAndPublishData(const uint8_t* data, CSerialProtocol::EMessageIds messsage_id); static int32_t getMessageLength(CSerialProtocol::EMessageIds messsage_id); @@ -58,6 +66,7 @@ class EulerNavDriver : public ModuleBase bool _next_message_detected{false}; uint16_t _next_message_protocol_version{0U}; uint8_t _next_message_code{0U}; + Statistics _statistics{}; bool _is_initialized{false}; }; From 4c904005888fdfe94a265b3cefe60d8391c05428 Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Sun, 5 Jan 2025 19:25:18 +0100 Subject: [PATCH 15/21] Implement and use initialize() and deinitialize() methods --- .../ins/eulernav_bahrs/eulernav_driver.cpp | 129 ++++++++---------- .../ins/eulernav_bahrs/eulernav_driver.h | 4 + 2 files changed, 64 insertions(+), 69 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index cf0963197f85..c5f4d52e17fd 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -5,37 +5,12 @@ EulerNavDriver::EulerNavDriver(const char* device_name) : _serial_port{device_name, 115200, ByteSize::EightBits, Parity::None, StopBits::One, FlowControl::Disabled} , _data_buffer{} { - _serial_port.open(); - - if (_serial_port.isOpen()) - { - PX4_INFO("Serial port opened successfully."); - _is_initialized = true; - } - else - { - PX4_ERR("Failed to open serial port"); - _is_initialized = false; - } - - if (_is_initialized) - { - if (false == _data_buffer.allocate(DATA_BUFFER_SIZE)) - { - PX4_ERR("Failed to allocate data buffer"); - _is_initialized = false; - } - } + initialize(); } EulerNavDriver::~EulerNavDriver() { - if (_serial_port.isOpen()) - { - _serial_port.close(); - } - - _data_buffer.deallocate(); + deinitialize(); } int EulerNavDriver::task_spawn(int argc, char *argv[]) @@ -101,66 +76,82 @@ void EulerNavDriver::run() while(false == should_exit()) { -#ifndef EULERNAV_BAHRS_PARSER_DEBUG - const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer, sizeof(_serial_read_buffer), - MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_US)}; + if (_is_initialized) + { + const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer, sizeof(_serial_read_buffer), + MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_US)}; - _statistics._total_bytes_read += bytes_read; + _statistics._total_bytes_read += bytes_read; - if (bytes_read > 0) - { - if (false == _data_buffer.push_back(_serial_read_buffer, bytes_read)) + if (bytes_read > 0) { - PX4_ERR("No space in data buffer"); + if (false == _data_buffer.push_back(_serial_read_buffer, bytes_read)) + { + PX4_ERR("No space in data buffer"); + } } - } -#else - static int counter = 0; - if (counter < 6) + processDataBuffer(); + + if (hrt_elapsed_time(&time_of_previous_statistics_print) >= STATISTICS_PRINT_PERIOD) + { + PX4_INFO("Elapsed time: %llu [us]. Total bytes received: %lu.\n", hrt_elapsed_time(&start_time), _statistics._total_bytes_read); + PX4_INFO("Inertial messages received: %lu. Navigation messages received: %lu.\n", _statistics._inertial_message_counter, _statistics._navigation_message_counter); + time_of_previous_statistics_print = hrt_absolute_time(); + } + } + else { - uint8_t test_data_part_1[] = { 0x05, 0xce, 0xF0, 0x00, 0xab, 0xcd, 0x12, // Rubbish - 0x4e, 0x45, 0x02, 0x00, 0x01, 0x34, 0xce, 0xff, 0xd3, 0xff, 0x70, 0xe6, 0x05, 0x00, 0xfa, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x32, 0x7d, 0x13, 0x65, // Inertial message - 0x4e, 0x45, 0x02, 0x00, 0x02, 0x9a, 0xb0, 0x22, 0xfe, 0xff, 0x3b, 0x00, 0xa3, 0xff, 0x30, 0xc8, 0x1f, 0x00, 0x00, 0x00, 0x82, 0x98, 0xa3, 0x42, // Navigation message - 0x4e, 0x45, 0x02, 0x00, 0x01, 0x35, 0xce, 0xff, 0xe2, 0xff // Inertial message part 1 - }; + deinitialize(); + px4_usleep(1000000); + initialize(); + } + } +} - uint8_t test_data_part_2[] = { 0x65, 0xe6, 0x00, 0x00, 0xf9, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x1d, 0x14, 0xf1, 0xf0, // Inertial message part 2 - 0x4e, 0x45, 0x02, 0x00, 0x01, 0x36, 0xcf, 0xff, 0xda, 0xff, 0x6e, 0xe6, 0x00, 0x00, 0xf8, 0xff, 0x00, 0x00, 0x3f, 0x00, 0x9f, 0x1b, 0x2b, 0xd1, // Inertial message - 0x00, 0x4e, 0xcd, 0x34, 0x77, 0x86, 0xFF // Rubbish - }; +void EulerNavDriver::initialize() +{ + if (false == _is_initialized) + { + _serial_port.open(); - uint8_t* data_to_push{nullptr}; - size_t byte_count_to_push{0U}; + if (_serial_port.isOpen()) + { + PX4_INFO("Serial port opened successfully."); + _is_initialized = true; + } + else + { + PX4_ERR("Failed to open serial port"); + } - if (counter % 2 == 0) - { - data_to_push = test_data_part_1; - byte_count_to_push = sizeof(test_data_part_1); - } - else + if (_is_initialized) + { + if (false == _data_buffer.allocate(DATA_BUFFER_SIZE)) { - data_to_push = test_data_part_2; - byte_count_to_push = sizeof(test_data_part_2); + PX4_ERR("Failed to allocate data buffer"); + _is_initialized = false; } - - _data_buffer.push_back(data_to_push, byte_count_to_push); - printf("Data buffer size: %d\n", _data_buffer.space_used()); - ++counter; } -#endif // EULERNAV_BAHRS_PARSER_DEBUG - - processDataBuffer(); - if (hrt_elapsed_time(&time_of_previous_statistics_print) >= STATISTICS_PRINT_PERIOD) + if (false == _is_initialized) { - PX4_INFO("Elapsed time: %llu [us]. Total bytes received: %lu.\n", hrt_elapsed_time(&start_time), _statistics._total_bytes_read); - PX4_INFO("Inertial messages received: %lu. Navigation messages received: %lu.\n", _statistics._inertial_message_counter, _statistics._navigation_message_counter); - time_of_previous_statistics_print = hrt_absolute_time(); + deinitialize(); } } } +void EulerNavDriver::deinitialize() +{ + if (_serial_port.isOpen()) + { + _serial_port.close(); + } + + _data_buffer.deallocate(); + _is_initialized = false; +} + void EulerNavDriver::processDataBuffer() { static_assert(MIN_MESSAGE_LENGTH >= (sizeof(CSerialProtocol::SMessageHeader) + sizeof(CSerialProtocol::CrcType_t))); diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 96b6d48f15e3..6997c24961c7 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -47,6 +47,10 @@ class EulerNavDriver : public ModuleBase // Min length of a valid message. 5 bytes header + 4 bytes CRC + padding to 12 (multiple of 32 bit words) static constexpr int MIN_MESSAGE_LENGTH{12}; + void initialize(); + + void deinitialize(); + void processDataBuffer(); static bool findNextMessageHeader(Ringbuffer& buffer); From 0fc01fdb75bbc25f1ed751b3db82f0d7b673d9f3 Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Sun, 5 Jan 2025 19:59:26 +0100 Subject: [PATCH 16/21] Implement print_usage() and print_status() --- .../ins/eulernav_bahrs/eulernav_driver.cpp | 59 +++++++++++++++---- .../ins/eulernav_bahrs/eulernav_driver.h | 12 ++-- 2 files changed, 55 insertions(+), 16 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index c5f4d52e17fd..82e0ca21a513 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -66,13 +66,55 @@ int EulerNavDriver::custom_command(int argc, char *argv[]) int EulerNavDriver::print_usage(const char *reason) { - return 0; + 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); + + } + else + { + PX4_INFO("Initialization failed. The driver is not running.\n"); + } + + return PX4_OK; } void EulerNavDriver::run() { - const auto start_time{hrt_absolute_time()}; - auto time_of_previous_statistics_print{start_time}; + _statistics._start_time = hrt_absolute_time(); while(false == should_exit()) { @@ -81,7 +123,7 @@ void EulerNavDriver::run() const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer, sizeof(_serial_read_buffer), MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_US)}; - _statistics._total_bytes_read += bytes_read; + _statistics._total_bytes_received += bytes_read; if (bytes_read > 0) { @@ -92,13 +134,6 @@ void EulerNavDriver::run() } processDataBuffer(); - - if (hrt_elapsed_time(&time_of_previous_statistics_print) >= STATISTICS_PRINT_PERIOD) - { - PX4_INFO("Elapsed time: %llu [us]. Total bytes received: %lu.\n", hrt_elapsed_time(&start_time), _statistics._total_bytes_read); - PX4_INFO("Inertial messages received: %lu. Navigation messages received: %lu.\n", _statistics._inertial_message_counter, _statistics._navigation_message_counter); - time_of_previous_statistics_print = hrt_absolute_time(); - } } else { @@ -214,7 +249,7 @@ void EulerNavDriver::processDataBuffer() if (expected_crc != actual_crc) { - PX4_INFO("Protocol version %u, message code %u: CRC failed.\n", _next_message_protocol_version, _next_message_code); + ++_statistics._crc_failures; } else { diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 6997c24961c7..205564220fee 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -26,15 +26,21 @@ class EulerNavDriver : public ModuleBase /// @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: - struct Statistics + class Statistics { - uint32_t _total_bytes_read{0U}; + public: + uint32_t _total_bytes_received{0U}; uint32_t _inertial_message_counter{0U}; uint32_t _navigation_message_counter{0U}; + uint32_t _crc_failures{0U}; + hrt_abstime _start_time{0U}; }; static constexpr int TASK_STACK_SIZE{2048}; @@ -42,7 +48,6 @@ class EulerNavDriver : public ModuleBase static constexpr int MIN_BYTES_TO_READ{16}; static constexpr int SERIAL_READ_TIMEOUT_US{5000}; static constexpr int DATA_BUFFER_SIZE{512}; - static constexpr hrt_abstime STATISTICS_PRINT_PERIOD{5000000U}; // Min length of a valid message. 5 bytes header + 4 bytes CRC + padding to 12 (multiple of 32 bit words) static constexpr int MIN_MESSAGE_LENGTH{12}; @@ -71,7 +76,6 @@ class EulerNavDriver : public ModuleBase uint16_t _next_message_protocol_version{0U}; uint8_t _next_message_code{0U}; Statistics _statistics{}; - bool _is_initialized{false}; }; From e3a07d79f3b8c0f1323bf75a063874a05a6b467d Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Sun, 5 Jan 2025 20:07:11 +0100 Subject: [PATCH 17/21] Collect all config constants in a class --- .../ins/eulernav_bahrs/eulernav_driver.cpp | 14 ++++----- .../ins/eulernav_bahrs/eulernav_driver.h | 29 ++++++++++++++----- 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index 82e0ca21a513..019ee3a05610 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -16,7 +16,7 @@ EulerNavDriver::~EulerNavDriver() int EulerNavDriver::task_spawn(int argc, char *argv[]) { int task_id = px4_task_spawn_cmd("bahrs", SCHED_DEFAULT, SCHED_PRIORITY_FAST_DRIVER, - TASK_STACK_SIZE, (px4_main_t)&run_trampoline, argv); + Config::TASK_STACK_SIZE, (px4_main_t)&run_trampoline, argv); if (task_id < 0) { @@ -121,7 +121,7 @@ void EulerNavDriver::run() if (_is_initialized) { const auto bytes_read{_serial_port.readAtLeast(_serial_read_buffer, sizeof(_serial_read_buffer), - MIN_BYTES_TO_READ, SERIAL_READ_TIMEOUT_US)}; + Config::MIN_BYTES_TO_READ, Config::SERIAL_READ_TIMEOUT_US)}; _statistics._total_bytes_received += bytes_read; @@ -162,7 +162,7 @@ void EulerNavDriver::initialize() if (_is_initialized) { - if (false == _data_buffer.allocate(DATA_BUFFER_SIZE)) + if (false == _data_buffer.allocate(Config::DATA_BUFFER_SIZE)) { PX4_ERR("Failed to allocate data buffer"); _is_initialized = false; @@ -189,10 +189,10 @@ void EulerNavDriver::deinitialize() void EulerNavDriver::processDataBuffer() { - static_assert(MIN_MESSAGE_LENGTH >= (sizeof(CSerialProtocol::SMessageHeader) + sizeof(CSerialProtocol::CrcType_t))); + static_assert(Config::MIN_MESSAGE_LENGTH >= (sizeof(CSerialProtocol::SMessageHeader) + sizeof(CSerialProtocol::CrcType_t))); using EMessageIds = CSerialProtocol::EMessageIds; - while (_data_buffer.space_used() >= MIN_MESSAGE_LENGTH) + while (_data_buffer.space_used() >= Config::MIN_MESSAGE_LENGTH) { if (false == _next_message_detected) { @@ -209,12 +209,12 @@ void EulerNavDriver::processDataBuffer() if (_next_message_detected) { - static_assert(sizeof(CSerialProtocol::SMessageHeader) < MIN_MESSAGE_LENGTH); + static_assert(sizeof(CSerialProtocol::SMessageHeader) < Config::MIN_MESSAGE_LENGTH); const EMessageIds message_id{static_cast(_next_message_code)}; const int32_t message_length{getMessageLength(message_id)}; - if ((message_length < 0) || (message_length < MIN_MESSAGE_LENGTH) || + 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. diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 205564220fee..3b1eaab7b0a8 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -43,14 +43,27 @@ class EulerNavDriver : public ModuleBase hrt_abstime _start_time{0U}; }; - static constexpr int TASK_STACK_SIZE{2048}; - static constexpr int SERIAL_READ_BUFFER_SIZE{128}; - static constexpr int MIN_BYTES_TO_READ{16}; - static constexpr int SERIAL_READ_TIMEOUT_US{5000}; - static constexpr int DATA_BUFFER_SIZE{512}; + class Config + { + public: + /// Driver task stack size + static constexpr uint32_t TASK_STACK_SIZE{2048}; + + /// Buffer size for serial port read operations + static constexpr uint32_t SERIAL_READ_BUFFER_SIZE{128}; + + /// Minimum number of bytes to wait for when reading from a serial port + static constexpr uint32_t MIN_BYTES_TO_READ{16}; - // Min length of a valid message. 5 bytes header + 4 bytes CRC + padding to 12 (multiple of 32 bit words) - static constexpr int MIN_MESSAGE_LENGTH{12}; + /// A timeout for serial port read operation + static constexpr uint32_t SERIAL_READ_TIMEOUT_US{5000}; + + /// Size of a ring buffer for storing RX data stream + static constexpr uint32_t DATA_BUFFER_SIZE{512}; + + /// Min length of a valid message. 5 bytes header + 4 bytes CRC + padding to 12 (multiple of 32 bit words) + static constexpr int32_t MIN_MESSAGE_LENGTH{12}; + }; void initialize(); @@ -70,7 +83,7 @@ class EulerNavDriver : public ModuleBase device::Serial _serial_port; Ringbuffer _data_buffer; - uint8_t _serial_read_buffer[SERIAL_READ_BUFFER_SIZE]; + uint8_t _serial_read_buffer[Config::SERIAL_READ_BUFFER_SIZE]; uint32_t _message_storage[8]; bool _next_message_detected{false}; uint16_t _next_message_protocol_version{0U}; From 45492f47e2014e77db5562491530f193182db0eb Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Sun, 5 Jan 2025 20:13:18 +0100 Subject: [PATCH 18/21] Put info about next found message into a class --- .../ins/eulernav_bahrs/eulernav_driver.cpp | 26 +++++++++---------- .../ins/eulernav_bahrs/eulernav_driver.h | 12 ++++++--- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index 019ee3a05610..47f951930871 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -194,34 +194,34 @@ void EulerNavDriver::processDataBuffer() while (_data_buffer.space_used() >= Config::MIN_MESSAGE_LENGTH) { - if (false == _next_message_detected) + if (false == _next_message_info._is_detected) { - _next_message_detected = findNextMessageHeader(_data_buffer); + _next_message_info._is_detected = findNextMessageHeader(_data_buffer); - if (_next_message_detected) + if (_next_message_info._is_detected) { - if (false == retrieveProtocolVersionAndMessageType(_data_buffer, _next_message_protocol_version, _next_message_code)) + if (false == retrieveProtocolVersionAndMessageType(_data_buffer, _next_message_info._protocol_version, _next_message_info._message_code)) { - _next_message_detected = false; + _next_message_info._is_detected = false; } } } - if (_next_message_detected) + if (_next_message_info._is_detected) { static_assert(sizeof(CSerialProtocol::SMessageHeader) < Config::MIN_MESSAGE_LENGTH); - const EMessageIds message_id{static_cast(_next_message_code)}; + 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_detected = false; + _next_message_info._is_detected = false; } - if (_next_message_detected) + if (_next_message_info._is_detected) { const int32_t bytes_to_retrieve{message_length - static_cast(sizeof(CSerialProtocol::SMessageHeader))}; @@ -237,9 +237,9 @@ void EulerNavDriver::processDataBuffer() bytes[0] = CSerialProtocol::uMarker1_; bytes[1] = CSerialProtocol::uMarker2_; - bytes[2] = reinterpret_cast(&_next_message_protocol_version)[0]; - bytes[3] = reinterpret_cast(&_next_message_protocol_version)[1]; - bytes[4] = _next_message_code; + 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)) { @@ -257,7 +257,7 @@ void EulerNavDriver::processDataBuffer() } } - _next_message_detected = false; + _next_message_info._is_detected = false; } } diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 3b1eaab7b0a8..52d425a2d4cc 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -65,6 +65,14 @@ class EulerNavDriver : public ModuleBase static constexpr int32_t MIN_MESSAGE_LENGTH{12}; }; + class NextMessageInfo + { + public: + bool _is_detected{false}; + uint16_t _protocol_version{0U}; + uint8_t _message_code{0U}; + }; + void initialize(); void deinitialize(); @@ -85,9 +93,7 @@ class EulerNavDriver : public ModuleBase Ringbuffer _data_buffer; uint8_t _serial_read_buffer[Config::SERIAL_READ_BUFFER_SIZE]; uint32_t _message_storage[8]; - bool _next_message_detected{false}; - uint16_t _next_message_protocol_version{0U}; - uint8_t _next_message_code{0U}; + NextMessageInfo _next_message_info{}; Statistics _statistics{}; bool _is_initialized{false}; }; From 996ba7fd7827a7e5a2e3cc8433ebbb7bd8358cf6 Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Sun, 5 Jan 2025 20:15:31 +0100 Subject: [PATCH 19/21] Print CRC failure count --- src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp index 47f951930871..e0dce0b4d822 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.cpp @@ -102,6 +102,7 @@ int EulerNavDriver::print_status() 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 From 6040ed5dc3199bd5806d0aaada1f597f78d467ca Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Sun, 5 Jan 2025 20:32:22 +0100 Subject: [PATCH 20/21] Remove unneeded print --- src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp index 918a10db2ffd..6d803816317a 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp +++ b/src/drivers/ins/eulernav_bahrs/eulernav_bahrs_main.cpp @@ -43,7 +43,6 @@ extern "C" __EXPORT int eulernav_bahrs_main(int argc, char *argv[]) { - PX4_INFO("Starting EULER-NAV BAHRS driver..."); EulerNavDriver::main(argc, argv); return OK; } From a10e37af0de9c001bb075aa9457d8d96e825ca2e Mon Sep 17 00:00:00 2001 From: fbaklanov Date: Mon, 6 Jan 2025 07:42:47 +0100 Subject: [PATCH 21/21] Add comments --- .../ins/eulernav_bahrs/eulernav_driver.h | 89 ++++++++++++------- 1 file changed, 57 insertions(+), 32 deletions(-) diff --git a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h index 52d425a2d4cc..e46b57958347 100644 --- a/src/drivers/ins/eulernav_bahrs/eulernav_driver.h +++ b/src/drivers/ins/eulernav_bahrs/eulernav_driver.h @@ -10,6 +10,8 @@ class EulerNavDriver : public ModuleBase { public: + /// @brief Class constructor + /// @param device_name Serial port to open EulerNavDriver(const char* device_name); ~EulerNavDriver(); @@ -33,68 +35,91 @@ class EulerNavDriver : public ModuleBase void run() final; private: + /// @brief Driver performance indicators class Statistics { public: - uint32_t _total_bytes_received{0U}; - uint32_t _inertial_message_counter{0U}; - uint32_t _navigation_message_counter{0U}; - uint32_t _crc_failures{0U}; - hrt_abstime _start_time{0U}; + 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: - /// Driver task stack size - static constexpr uint32_t TASK_STACK_SIZE{2048}; - - /// Buffer size for serial port read operations - static constexpr uint32_t SERIAL_READ_BUFFER_SIZE{128}; - - /// Minimum number of bytes to wait for when reading from a serial port - static constexpr uint32_t MIN_BYTES_TO_READ{16}; - - /// A timeout for serial port read operation - static constexpr uint32_t SERIAL_READ_TIMEOUT_US{5000}; - - /// Size of a ring buffer for storing RX data stream - static constexpr uint32_t DATA_BUFFER_SIZE{512}; - - /// Min length of a valid message. 5 bytes header + 4 bytes CRC + padding to 12 (multiple of 32 bit words) - static constexpr int32_t MIN_MESSAGE_LENGTH{12}; + 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_US{5000}; ///< 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}; - uint16_t _protocol_version{0U}; - uint8_t _message_code{0U}; + 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 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; - Ringbuffer _data_buffer; - uint8_t _serial_read_buffer[Config::SERIAL_READ_BUFFER_SIZE]; - uint32_t _message_storage[8]; - NextMessageInfo _next_message_info{}; - Statistics _statistics{}; - bool _is_initialized{false}; + 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 };