From 127c14cf2531385b581698b203350f28407d0a7a Mon Sep 17 00:00:00 2001 From: Pedro de Santi <62271285+PedroDeSanti@users.noreply.github.com> Date: Thu, 22 Feb 2024 19:44:19 -0300 Subject: [PATCH 001/561] Initialize migrate/cpp --- inc/mcu.hpp | 46 ++++++++++++++++++++++++++++++++++++++++++++++ src/mcu.cpp | 34 ++++++++++++++++++++++++++++++++++ 2 files changed, 80 insertions(+) create mode 100644 inc/mcu.hpp create mode 100644 src/mcu.cpp diff --git a/inc/mcu.hpp b/inc/mcu.hpp new file mode 100644 index 0000000..b543948 --- /dev/null +++ b/inc/mcu.hpp @@ -0,0 +1,46 @@ +/** + * @file mcu.hpp + * + * @brief MCU related + */ + +#ifndef __MCU_HPP__ +#define __MCU_HPP__ + +#include + +/***************************************** + * Public Function Prototypes + *****************************************/ + +extern "C" +{ +/** + * @brief Initializes System Clock. + * @note Defined by cube. + */ +void SystemClock_Config(void); +} + +namespace hal { +class mcu { + public: + /** + * @brief Initializes MCU and some peripherals. + */ + static void init(void); + + /** + * @brief Put the MCU to sleep. + * + * @param ms Sleep time in milliseconds + */ + static void sleep(uint32_t ms); + + /** + * @brief Toggles LED. + */ + static void led_toggle(void); +}; +}; +#endif // __MCU_HPP__ diff --git a/src/mcu.cpp b/src/mcu.cpp new file mode 100644 index 0000000..b11fcff --- /dev/null +++ b/src/mcu.cpp @@ -0,0 +1,34 @@ +/** + * @file mcu.cpp + * + * @brief MCU related + */ + +#include + +#include "mcu.hpp" + +#include "gpio.h" +#include "main.h" + +/***************************************** + * Public Function Body Definitions + *****************************************/ + +namespace hal { +void mcu::init(void) { + HAL_Init(); + + SystemClock_Config(); + + MX_GPIO_Init(); +} + +void mcu::sleep(uint32_t ms) { + HAL_Delay(ms); +} + +void mcu::led_toggle(void) { + HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_5); +} +} From fe12af55fec90f21076cbab5131f9afc33678c73 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sun, 10 Mar 2024 12:50:04 -0300 Subject: [PATCH 002/561] =?UTF-8?q?=E2=9C=A8=20Add=20HAL=20GPIO=20class=20?= =?UTF-8?q?implementation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/gpio.hpp | 176 +++++++++++++++++++++++++++++++++++++++++++++++ src/hal/gpio.cpp | 31 +++++++++ 2 files changed, 207 insertions(+) create mode 100644 inc/hal/gpio.hpp create mode 100644 src/hal/gpio.cpp diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp new file mode 100644 index 0000000..bf7c181 --- /dev/null +++ b/inc/hal/gpio.hpp @@ -0,0 +1,176 @@ +/** + * @file gpio.hpp + * + * @author Comp ThundeRatz + * + * @brief HAL GPIO class header. + * + * @date 01/2024 + * + * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * + */ + +#ifndef __GPIO_HPP__ +#define __GPIO_HPP__ + +#include "gpio.h" + +namespace hal { +/** + * @brief Class for controlling GPIO pins on STM32 microcontrollers. + */ +class Gpio { + private: + /** + * @brief Structure representing a GPIO port. + */ + struct GpioPort { + GPIO_TypeDef* port; + }; + + /** + * @brief Structure representing a GPIO pin. + */ + struct GpioPin { + uint16_t pin; + }; + + public: + /** + * @brief Configuration structure for GPIO pin. + */ + struct Config { + GpioPort port; + GpioPin pin; + }; + + /** + * @brief Structure representing GPIO ports. + */ + struct Port { +#ifdef GPIOA + static constexpr GpioPort A = {GPIOA}; +#endif +#ifdef GPIOB + static constexpr GpioPort B = {GPIOB}; +#endif +#ifdef GPIOC + static constexpr GpioPort C = {GPIOC}; +#endif +#ifdef GPIOD + static constexpr GpioPort D = {GPIOD}; +#endif +#ifdef GPIOE + static constexpr GpioPort E = {GPIOE}; +#endif +#ifdef GPIOF + static constexpr GpioPort F = {GPIOF}; +#endif +#ifdef GPIOG + static constexpr GpioPort G = {GPIOG}; +#endif +#ifdef GPIOH + static constexpr GpioPort H = {GPIOH}; +#endif +#ifdef GPIOI + static constexpr GpioPort I = {GPIOI}; +#endif +#ifdef GPIOJ + static constexpr GpioPort J = {GPIOJ}; +#endif +#ifdef GPIOK + static constexpr GpioPort K = {GPIOK}; +#endif +#ifdef GPIOZ + static constexpr GpioPort Z = {GPIOZ}; +#endif + }; + + /** + * @brief Structure representing GPIO pins. + */ + struct Pin { +#ifdef GPIO_PIN_0 + static constexpr GpioPin P0 = {GPIO_PIN_0}; +#endif +#ifdef GPIO_PIN_1 + static constexpr GpioPin P1 = {GPIO_PIN_1}; +#endif +#ifdef GPIO_PIN_2 + static constexpr GpioPin P2 = {GPIO_PIN_2}; +#endif +#ifdef GPIO_PIN_3 + static constexpr GpioPin P3 = {GPIO_PIN_3}; +#endif +#ifdef GPIO_PIN_4 + static constexpr GpioPin P4 = {GPIO_PIN_4}; +#endif +#ifdef GPIO_PIN_5 + static constexpr GpioPin P5 = {GPIO_PIN_5}; +#endif +#ifdef GPIO_PIN_6 + static constexpr GpioPin P6 = {GPIO_PIN_6}; +#endif +#ifdef GPIO_PIN_7 + static constexpr GpioPin P7 = {GPIO_PIN_7}; +#endif +#ifdef GPIO_PIN_8 + static constexpr GpioPin P8 = {GPIO_PIN_8}; +#endif +#ifdef GPIO_PIN_9 + static constexpr GpioPin P9 = {GPIO_PIN_9}; +#endif +#ifdef GPIO_PIN_10 + static constexpr GpioPin P10 = {GPIO_PIN_10}; +#endif +#ifdef GPIO_PIN_11 + static constexpr GpioPin P11 = {GPIO_PIN_11}; +#endif +#ifdef GPIO_PIN_12 + static constexpr GpioPin P12 = {GPIO_PIN_12}; +#endif +#ifdef GPIO_PIN_13 + static constexpr GpioPin P13 = {GPIO_PIN_13}; +#endif +#ifdef GPIO_PIN_14 + static constexpr GpioPin P14 = {GPIO_PIN_14}; +#endif +#ifdef GPIO_PIN_15 + static constexpr GpioPin P15 = {GPIO_PIN_15}; +#endif + }; + + /** + * @brief Constructor for the Gpio class. + * + * @param gpio_config Configuration for the GPIO pin. + */ + Gpio(const Config& gpio_config); + + /** + * @brief Read the current state of the GPIO pin. + * + * @return The current state of the GPIO pin (true for high, false for low). + */ + bool read() const; + + /** + * @brief Write a new state to the GPIO pin. + * + * @param pin_state The state to be written (true for high, false for low). + */ + void write(bool state); + + /** + * @brief Toggle the state of the GPIO pin. + */ + void toggle(); + + private: + uint16_t pin; + GPIO_TypeDef* port; +}; +} // namespace hal + +#endif // __GPIO_HPP__ diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp new file mode 100644 index 0000000..c3b86fc --- /dev/null +++ b/src/hal/gpio.cpp @@ -0,0 +1,31 @@ +/** + * @file gpio.cpp + * + * @author Comp ThundeRatz + * + * @brief HAL GPIO class source. + * + * @date 01/2024 + * + * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * + */ + +#include "hal/gpio.hpp" + +namespace hal { +Gpio::Gpio(const Config& gpio_config) : pin(gpio_config.pin.pin), port(gpio_config.port.port) { +} + +bool Gpio::read(void) const { + return HAL_GPIO_ReadPin(this->port, this->pin); +} + +void Gpio::write(bool state) { + HAL_GPIO_WritePin(this->port, this->pin, (GPIO_PinState) state); +} + +void Gpio::toggle(void) { + HAL_GPIO_TogglePin(this->port, this->pin); +} +} From 185ef9bd94ffd5a8f458c55fddb17039f50c9c96 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sun, 10 Mar 2024 12:50:16 -0300 Subject: [PATCH 003/561] =?UTF-8?q?=E2=9C=A8=20Add=20proxy=20Led=20class?= =?UTF-8?q?=20implementation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/led.hpp | 59 +++++++++++++++++++++++++++++++++++++++++++++++ src/proxy/led.cpp | 31 +++++++++++++++++++++++++ 2 files changed, 90 insertions(+) create mode 100644 inc/proxy/led.hpp create mode 100644 src/proxy/led.cpp diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp new file mode 100644 index 0000000..1f5a061 --- /dev/null +++ b/inc/proxy/led.hpp @@ -0,0 +1,59 @@ +/** + * @file led.hpp + * + * @author Comp ThundeRatz + * + * @brief Proxy Led class header. + * + * @date 01/2024 + * + * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * + */ + +#ifndef __LED_HPP__ +#define __LED_HPP__ + +#include "hal/gpio.hpp" + +namespace proxy { +/** + * @brief Class for controlling Led. + */ +class Led { + public: + /** + * @brief configuration structure for led. + */ + struct Config { + hal::Gpio::Config gpio_config; + }; + + /** + * @brief Constructor for the Led class. + * + * @param led_config Configuration for the led. + */ + Led(Config led_config); + + /** + * @brief Turn the led on. + */ + void turn_on(); + + /** + * @brief Turn the led off. + */ + void turn_off(); + + /** + * @brief Toggle led. + */ + void toggle(); + + private: + hal::Gpio led_gpio; +}; +} // namespace proxy + +#endif // __LED_HPP__ diff --git a/src/proxy/led.cpp b/src/proxy/led.cpp new file mode 100644 index 0000000..180c9ea --- /dev/null +++ b/src/proxy/led.cpp @@ -0,0 +1,31 @@ +/** + * @file led.cpp + * + * @author Comp ThundeRatz + * + * @brief Proxy Led class source. + * + * @date 01/2024 + * + * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * + */ + +#include "proxy/led.hpp" + +namespace proxy { +Led::Led(Config led_config) : led_gpio(led_config.gpio_config) { +} + +void Led::turn_on() { + this->led_gpio.write(true); +} + +void Led::turn_off() { + this->led_gpio.write(false); +} + +void Led::toggle() { + this->led_gpio.toggle(); +} +} // namespace proxy From 0f01246170ad054be7874f5fee8e84639e4b2e95 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sun, 10 Mar 2024 12:50:28 -0300 Subject: [PATCH 004/561] =?UTF-8?q?=E2=9C=A8=20Add=20Button=20class=20to?= =?UTF-8?q?=20proxy=20namespace?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/button.hpp | 51 ++++++++++++++++++++++++++++++++++++++++++++ src/proxy/button.cpp | 23 ++++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 inc/proxy/button.hpp create mode 100644 src/proxy/button.cpp diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp new file mode 100644 index 0000000..619ad25 --- /dev/null +++ b/inc/proxy/button.hpp @@ -0,0 +1,51 @@ +/** + * @file button.hpp + * + * @author Comp ThundeRatz + * + * @brief Proxy Button class header. + * + * @date 01/2024 + * + * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * + */ + +#ifndef __BUTTON_HPP__ +#define __BUTTON_HPP__ + +#include "hal/gpio.hpp" + +namespace proxy { +/** + * @brief Class for controlling Button. + */ +class Button { + public: + /** + * @brief configuration structure for button. + */ + struct Config { + hal::Gpio::Config gpio_config; + }; + + /** + * @brief Constructor for the Button class. + * + * @param button_config Configuration for the button. + */ + Button(Config button_config); + + /** + * @brief Get the button state. + * + * @return true if the button is pressed, false otherwise. + */ + bool get_state(); + + private: + hal::Gpio button_gpio; +}; +} // namespace proxy + +#endif // __BUTTON_HPP__ diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp new file mode 100644 index 0000000..ebd27ed --- /dev/null +++ b/src/proxy/button.cpp @@ -0,0 +1,23 @@ +/** + * @file button.cpp + * + * @author Comp ThundeRatz + * + * @brief Proxy Button class source. + * + * @date 01/2024 + * + * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * + */ + +#include "proxy/button.hpp" + +namespace proxy { +Button::Button(Config button_config) : button_gpio(button_config.gpio_config) { +} + +bool Button::get_state() { + return this->button_gpio.read(); +} +} // namespace proxy From bf5cd04868e9d9e3335006709f418748830d7362 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sun, 10 Mar 2024 12:50:55 -0300 Subject: [PATCH 005/561] =?UTF-8?q?=E2=9C=A8=20Add=20proxy=20objects=20for?= =?UTF-8?q?=20LED=20and=20button?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mcu.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/mcu.cpp b/src/mcu.cpp index b11fcff..35df8e2 100644 --- a/src/mcu.cpp +++ b/src/mcu.cpp @@ -27,8 +27,4 @@ void mcu::init(void) { void mcu::sleep(uint32_t ms) { HAL_Delay(ms); } - -void mcu::led_toggle(void) { - HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_5); -} } From 97902a2c6a9d341dfb6778b7bef6dfc3562520f1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 11 Mar 2024 01:09:51 -0300 Subject: [PATCH 006/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Move=20code=20logi?= =?UTF-8?q?c=20to=20micras=5Fcontroller?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Eduardo Santos Barreto --- inc/hal/gpio.hpp | 40 ++++++++++++++++++++-------------------- inc/{ => hal}/mcu.hpp | 10 +++++----- inc/proxy/button.hpp | 28 ++++++++++++++-------------- inc/proxy/led.hpp | 28 ++++++++++++++-------------- src/hal/gpio.cpp | 9 ++------- src/{ => hal}/mcu.cpp | 0 src/proxy/button.cpp | 10 +++------- src/proxy/led.cpp | 11 +++-------- 8 files changed, 61 insertions(+), 75 deletions(-) rename inc/{ => hal}/mcu.hpp (75%) rename src/{ => hal}/mcu.cpp (100%) diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index bf7c181..0ab7dd7 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -1,14 +1,9 @@ /** * @file gpio.hpp * - * @author Comp ThundeRatz - * - * @brief HAL GPIO class header. - * - * @date 01/2024 - * - * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * @brief HAL GPIO class header * + * @date 03/2024 */ #ifndef __GPIO_HPP__ @@ -18,19 +13,19 @@ namespace hal { /** - * @brief Class for controlling GPIO pins on STM32 microcontrollers. + * @brief Class for controlling GPIO pins on STM32 microcontrollers */ class Gpio { private: /** - * @brief Structure representing a GPIO port. + * @brief Structure representing a GPIO port */ struct GpioPort { GPIO_TypeDef* port; }; /** - * @brief Structure representing a GPIO pin. + * @brief Structure representing a GPIO pin */ struct GpioPin { uint16_t pin; @@ -38,7 +33,7 @@ class Gpio { public: /** - * @brief Configuration structure for GPIO pin. + * @brief Configuration structure for GPIO pin */ struct Config { GpioPort port; @@ -46,7 +41,7 @@ class Gpio { }; /** - * @brief Structure representing GPIO ports. + * @brief Structure representing GPIO ports */ struct Port { #ifdef GPIOA @@ -88,7 +83,7 @@ class Gpio { }; /** - * @brief Structure representing GPIO pins. + * @brief Structure representing GPIO pins */ struct Pin { #ifdef GPIO_PIN_0 @@ -142,28 +137,33 @@ class Gpio { }; /** - * @brief Constructor for the Gpio class. + * @brief Constructor for the Gpio class * - * @param gpio_config Configuration for the GPIO pin. + * @param gpio_config Configuration for the GPIO pin */ Gpio(const Config& gpio_config); /** - * @brief Read the current state of the GPIO pin. + * @brief Destroy the Gpio object + */ + ~Gpio() = default; + + /** + * @brief Read the current state of the GPIO pin * - * @return The current state of the GPIO pin (true for high, false for low). + * @return The current state of the GPIO pin (true for high, false for low) */ bool read() const; /** - * @brief Write a new state to the GPIO pin. + * @brief Write a new state to the GPIO pin * - * @param pin_state The state to be written (true for high, false for low). + * @param pin_state The state to be written (true for high, false for low) */ void write(bool state); /** - * @brief Toggle the state of the GPIO pin. + * @brief Toggle the state of the GPIO pin */ void toggle(); diff --git a/inc/mcu.hpp b/inc/hal/mcu.hpp similarity index 75% rename from inc/mcu.hpp rename to inc/hal/mcu.hpp index b543948..d9cb6da 100644 --- a/inc/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -16,8 +16,8 @@ extern "C" { /** - * @brief Initializes System Clock. - * @note Defined by cube. + * @brief Initializes System Clock + * @note Defined by cube */ void SystemClock_Config(void); } @@ -26,19 +26,19 @@ namespace hal { class mcu { public: /** - * @brief Initializes MCU and some peripherals. + * @brief Initializes MCU and some peripherals */ static void init(void); /** - * @brief Put the MCU to sleep. + * @brief Put the MCU to sleep * * @param ms Sleep time in milliseconds */ static void sleep(uint32_t ms); /** - * @brief Toggles LED. + * @brief Toggles LED */ static void led_toggle(void); }; diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index 619ad25..b4ac9b7 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -1,14 +1,9 @@ /** * @file button.hpp * - * @author Comp ThundeRatz - * - * @brief Proxy Button class header. - * - * @date 01/2024 - * - * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * @brief Proxy Button class header * + * @date 03/2024 */ #ifndef __BUTTON_HPP__ @@ -18,28 +13,33 @@ namespace proxy { /** - * @brief Class for controlling Button. + * @brief Class for controlling Button */ class Button { public: /** - * @brief configuration structure for button. + * @brief configuration structure for button */ struct Config { hal::Gpio::Config gpio_config; }; /** - * @brief Constructor for the Button class. + * @brief Constructor for the Button class * - * @param button_config Configuration for the button. + * @param button_config Configuration for the button + */ + Button(const Config& button_config); + + /** + * @brief Destroy the Button object */ - Button(Config button_config); + ~Button() = default; /** - * @brief Get the button state. + * @brief Get the button state * - * @return true if the button is pressed, false otherwise. + * @return true if the button is pressed, false otherwise */ bool get_state(); diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp index 1f5a061..264934b 100644 --- a/inc/proxy/led.hpp +++ b/inc/proxy/led.hpp @@ -1,14 +1,9 @@ /** * @file led.hpp * - * @author Comp ThundeRatz - * - * @brief Proxy Led class header. - * - * @date 01/2024 - * - * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * @brief Proxy Led class header * + * @date 03/2024 */ #ifndef __LED_HPP__ @@ -23,31 +18,36 @@ namespace proxy { class Led { public: /** - * @brief configuration structure for led. + * @brief configuration structure for led */ struct Config { hal::Gpio::Config gpio_config; }; /** - * @brief Constructor for the Led class. + * @brief Constructor for the Led class * - * @param led_config Configuration for the led. + * @param led_config Configuration for the led + */ + Led(const Config& led_config); + + /** + * @brief Destroy the Led object */ - Led(Config led_config); + ~Led() = default; /** - * @brief Turn the led on. + * @brief Turn the led on */ void turn_on(); /** - * @brief Turn the led off. + * @brief Turn the led off */ void turn_off(); /** - * @brief Toggle led. + * @brief Toggle led */ void toggle(); diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index c3b86fc..2801371 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -1,14 +1,9 @@ /** * @file gpio.cpp * - * @author Comp ThundeRatz - * - * @brief HAL GPIO class source. - * - * @date 01/2024 - * - * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * @brief HAL GPIO class source * + * @date 03/2024 */ #include "hal/gpio.hpp" diff --git a/src/mcu.cpp b/src/hal/mcu.cpp similarity index 100% rename from src/mcu.cpp rename to src/hal/mcu.cpp diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index ebd27ed..10e9374 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -1,20 +1,16 @@ /** * @file button.cpp * - * @author Comp ThundeRatz + * @brief Proxy Button class source * - * @brief Proxy Button class source. - * - * @date 01/2024 - * - * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * @date 03/2024 * */ #include "proxy/button.hpp" namespace proxy { -Button::Button(Config button_config) : button_gpio(button_config.gpio_config) { +Button::Button(const Config& button_config) : button_gpio(button_config.gpio_config) { } bool Button::get_state() { diff --git a/src/proxy/led.cpp b/src/proxy/led.cpp index 180c9ea..bd0fb24 100644 --- a/src/proxy/led.cpp +++ b/src/proxy/led.cpp @@ -1,20 +1,15 @@ /** * @file led.cpp * - * @author Comp ThundeRatz - * - * @brief Proxy Led class source. - * - * @date 01/2024 - * - * @copyright MIT License - Copyright (c) 2024 ThundeRatz + * @brief Proxy Led class source * + * @date 03/2024 */ #include "proxy/led.hpp" namespace proxy { -Led::Led(Config led_config) : led_gpio(led_config.gpio_config) { +Led::Led(const Config& led_config) : led_gpio(led_config.gpio_config) { } void Led::turn_on() { From 0cb59bedea7a84aae4a4c682074aadf342e4f06e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 11 Mar 2024 15:14:22 -0300 Subject: [PATCH 007/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20destructors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/gpio.hpp | 5 ----- inc/proxy/button.hpp | 5 ----- inc/proxy/led.hpp | 5 ----- 3 files changed, 15 deletions(-) diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index 0ab7dd7..8ee2bfb 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -143,11 +143,6 @@ class Gpio { */ Gpio(const Config& gpio_config); - /** - * @brief Destroy the Gpio object - */ - ~Gpio() = default; - /** * @brief Read the current state of the GPIO pin * diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index b4ac9b7..328cf3d 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -31,11 +31,6 @@ class Button { */ Button(const Config& button_config); - /** - * @brief Destroy the Button object - */ - ~Button() = default; - /** * @brief Get the button state * diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp index 264934b..fb5cbd0 100644 --- a/inc/proxy/led.hpp +++ b/inc/proxy/led.hpp @@ -31,11 +31,6 @@ class Led { */ Led(const Config& led_config); - /** - * @brief Destroy the Led object - */ - ~Led() = default; - /** * @brief Turn the led on */ From f84e5ff2fb1c904bd7d32d612b7851be7dec2560 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Fri, 15 Mar 2024 16:07:56 -0300 Subject: [PATCH 008/561] =?UTF-8?q?=F0=9F=A9=B9=20Update=20include=20path?= =?UTF-8?q?=20for=20mcu.hpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/hal/mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index 35df8e2..220c02f 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -6,7 +6,7 @@ #include -#include "mcu.hpp" +#include "hal/mcu.hpp" #include "gpio.h" #include "main.h" From df06f18aff882ec8640c30ee57f41a37a870ab86 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Fri, 15 Mar 2024 16:32:58 -0300 Subject: [PATCH 009/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20GPIO=20?= =?UTF-8?q?class=20structure=20and=20constructor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/gpio.hpp | 115 +---------------------------------------------- src/hal/gpio.cpp | 2 +- 2 files changed, 3 insertions(+), 114 deletions(-) diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index 8ee2bfb..25e1d9d 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -16,124 +16,13 @@ namespace hal { * @brief Class for controlling GPIO pins on STM32 microcontrollers */ class Gpio { - private: - /** - * @brief Structure representing a GPIO port - */ - struct GpioPort { - GPIO_TypeDef* port; - }; - - /** - * @brief Structure representing a GPIO pin - */ - struct GpioPin { - uint16_t pin; - }; - public: /** * @brief Configuration structure for GPIO pin */ struct Config { - GpioPort port; - GpioPin pin; - }; - - /** - * @brief Structure representing GPIO ports - */ - struct Port { -#ifdef GPIOA - static constexpr GpioPort A = {GPIOA}; -#endif -#ifdef GPIOB - static constexpr GpioPort B = {GPIOB}; -#endif -#ifdef GPIOC - static constexpr GpioPort C = {GPIOC}; -#endif -#ifdef GPIOD - static constexpr GpioPort D = {GPIOD}; -#endif -#ifdef GPIOE - static constexpr GpioPort E = {GPIOE}; -#endif -#ifdef GPIOF - static constexpr GpioPort F = {GPIOF}; -#endif -#ifdef GPIOG - static constexpr GpioPort G = {GPIOG}; -#endif -#ifdef GPIOH - static constexpr GpioPort H = {GPIOH}; -#endif -#ifdef GPIOI - static constexpr GpioPort I = {GPIOI}; -#endif -#ifdef GPIOJ - static constexpr GpioPort J = {GPIOJ}; -#endif -#ifdef GPIOK - static constexpr GpioPort K = {GPIOK}; -#endif -#ifdef GPIOZ - static constexpr GpioPort Z = {GPIOZ}; -#endif - }; - - /** - * @brief Structure representing GPIO pins - */ - struct Pin { -#ifdef GPIO_PIN_0 - static constexpr GpioPin P0 = {GPIO_PIN_0}; -#endif -#ifdef GPIO_PIN_1 - static constexpr GpioPin P1 = {GPIO_PIN_1}; -#endif -#ifdef GPIO_PIN_2 - static constexpr GpioPin P2 = {GPIO_PIN_2}; -#endif -#ifdef GPIO_PIN_3 - static constexpr GpioPin P3 = {GPIO_PIN_3}; -#endif -#ifdef GPIO_PIN_4 - static constexpr GpioPin P4 = {GPIO_PIN_4}; -#endif -#ifdef GPIO_PIN_5 - static constexpr GpioPin P5 = {GPIO_PIN_5}; -#endif -#ifdef GPIO_PIN_6 - static constexpr GpioPin P6 = {GPIO_PIN_6}; -#endif -#ifdef GPIO_PIN_7 - static constexpr GpioPin P7 = {GPIO_PIN_7}; -#endif -#ifdef GPIO_PIN_8 - static constexpr GpioPin P8 = {GPIO_PIN_8}; -#endif -#ifdef GPIO_PIN_9 - static constexpr GpioPin P9 = {GPIO_PIN_9}; -#endif -#ifdef GPIO_PIN_10 - static constexpr GpioPin P10 = {GPIO_PIN_10}; -#endif -#ifdef GPIO_PIN_11 - static constexpr GpioPin P11 = {GPIO_PIN_11}; -#endif -#ifdef GPIO_PIN_12 - static constexpr GpioPin P12 = {GPIO_PIN_12}; -#endif -#ifdef GPIO_PIN_13 - static constexpr GpioPin P13 = {GPIO_PIN_13}; -#endif -#ifdef GPIO_PIN_14 - static constexpr GpioPin P14 = {GPIO_PIN_14}; -#endif -#ifdef GPIO_PIN_15 - static constexpr GpioPin P15 = {GPIO_PIN_15}; -#endif + GPIO_TypeDef* port; + uint16_t pin; }; /** diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index 2801371..aafd745 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -9,7 +9,7 @@ #include "hal/gpio.hpp" namespace hal { -Gpio::Gpio(const Config& gpio_config) : pin(gpio_config.pin.pin), port(gpio_config.port.port) { +Gpio::Gpio(const Config& gpio_config) : pin(gpio_config.pin), port(gpio_config.port) { } bool Gpio::read(void) const { From e27ea22e81fb68a42ff2c4fe426c281b46f0cc09 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 05:29:37 -0300 Subject: [PATCH 010/561] =?UTF-8?q?=E2=9C=A8=20Add=20ADC=20DMA=20HAL=20wra?= =?UTF-8?q?pper?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 59 +++++++++++++++++++++++++++++++++++++++++++++ src/hal/adc_dma.cpp | 29 ++++++++++++++++++++++ 2 files changed, 88 insertions(+) create mode 100644 inc/hal/adc_dma.hpp create mode 100644 src/hal/adc_dma.cpp diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp new file mode 100644 index 0000000..8b6c7f5 --- /dev/null +++ b/inc/hal/adc_dma.hpp @@ -0,0 +1,59 @@ +/** + * @file adc_dma.hpp + * + * @brief STM32 ADC DMA HAL wrapper. + * + * @date 03/2024 + */ + +#ifndef __HAL_ADC_DMA_HPP__ +#define __HAL_ADC_DMA_HPP__ + +#include +#include + +#include "adc.h" + +namespace hal { +/** + * @brief Class to handle ADC DMA peripheral on STM32 microcontrollers. + */ +class AdcDma { + public: + /** + * @brief ADC DMA configuration struct. + */ + struct Config { + ADC_HandleTypeDef* handle; + std::function init_function; + }; + + /** + * @brief Construct a new AdcDma object. + * + * @param adc_config ADC DMA configuration struct. + */ + AdcDma(Config& adc_config); + + /** + * @brief Enable ADC, start conversion of regular group and transfer result through DMA. + * + * @param buffer Destination Buffer address. + * @param size Number of data to be transferred from ADC DMA peripheral to memory. + */ + void start_dma(uint32_t* buffer, uint32_t size); + + /** + * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode). + */ + void stop_dma(); + + private: + /** + * @brief ADC DMA handle. + */ + ADC_HandleTypeDef* handle; +}; +} + +#endif // __HAL_ADC_DMA_HPP__ diff --git a/src/hal/adc_dma.cpp b/src/hal/adc_dma.cpp new file mode 100644 index 0000000..6644047 --- /dev/null +++ b/src/hal/adc_dma.cpp @@ -0,0 +1,29 @@ +/** + * @file adc_dma.cpp + * + * @brief STM32 ADC DMA HAL wrapper. + * + * @date 03/2024 + */ + +#include "dma.h" +#include "hal/adc_dma.hpp" + +namespace hal { +/***************************************** + * Public Methods Bodies Definitions. + *****************************************/ + +AdcDma::AdcDma(Config& adc_config) : handle(adc_config.handle) { + MX_DMA_Init(); + adc_config.init_function(); +} + +void AdcDma::start_dma(uint32_t* buffer, uint32_t size) { + HAL_ADC_Start_DMA(this->handle, buffer, size); +} + +void AdcDma::stop_dma() { + HAL_ADC_Stop_DMA(this->handle); +} +} // namespace hal From 20892ed3170afe8149ddd867e1dfc2aa4a09e0be Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 05:29:50 -0300 Subject: [PATCH 011/561] =?UTF-8?q?=E2=9C=A8=20Add=20Tim=20class=20for=20S?= =?UTF-8?q?TM32=20TIM=20HAL=20wrapper?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/tim.hpp | 106 ++++++++++++++++++++++++++++++++++++++++++++++++ src/hal/tim.cpp | 51 +++++++++++++++++++++++ 2 files changed, 157 insertions(+) create mode 100644 inc/hal/tim.hpp create mode 100644 src/hal/tim.cpp diff --git a/inc/hal/tim.hpp b/inc/hal/tim.hpp new file mode 100644 index 0000000..87ba1a8 --- /dev/null +++ b/inc/hal/tim.hpp @@ -0,0 +1,106 @@ +/** + * @file tim.hpp + * + * @brief STM32 TIM HAL wrapper. + * + * @date 03/2024 + */ + +#ifndef __TIM_HPP__ +#define __TIM_HPP__ + +#include +#include + +#include "tim.h" + +namespace hal { +/** + * @brief Class to handle TIM peripheral on STM32 microcontrollers. + */ +class Tim { + public: + /** + * @brief TIM configuration struct. + */ + struct Config { + TIM_HandleTypeDef* handle; + std::function init_function; + }; + + /** + * @brief Constructor for the Tim class. + * + * @param tim_config Configuration for timer handler + */ + Tim(Config& tim_config); + + /** + * @brief Initialize timer base generation. + */ + void base_start(); + + /** + * @brief Stop timer base generation. + */ + void base_stop(); + + /** + * @brief Set timer counter. + * + * @param counter Counter value. + */ + void set_counter(uint32_t counter); + + /** + * @brief Get timer counter. + * + * @return uint32_t Counter value. + */ + uint32_t get_counter(); + + /** + * @brief Start PWM generation. + * + * @param channel Tim channel to be enabled. + */ + void pwm_start(uint32_t channel); + + /** + * @brief Stop PWM generation. + * + * @param channel Tim channel to be disabled. + */ + void pwm_stop(uint32_t channel); + + /** + * @brief Set timer prescaler. + * + * @param prescaler Prescaler new value. + */ + void set_prescaler(uint32_t prescaler); + + /** + * @brief Set the TIM Capture Compare Register value. + * + * @param channel TIM channel to be configured. + * @param compare Compare register new value. + */ + void set_compare(uint32_t channel, uint32_t compare); + + /** + * @brief Get the Autoreload register value. + * + * @return uint32_t The value of the Autoreload register. + */ + uint32_t get_autoreload(); + + private: + /** + * @brief Timer handler. + */ + TIM_HandleTypeDef* handle; +}; +} // namespace hal + +#endif // __TIM_HPP__ diff --git a/src/hal/tim.cpp b/src/hal/tim.cpp new file mode 100644 index 0000000..bd0fa1d --- /dev/null +++ b/src/hal/tim.cpp @@ -0,0 +1,51 @@ +/** + * @file tim.cpp + * + * @brief STM32 TIM HAL wrapper. + * + * @date 03/2024 + */ + +#include "hal/tim.hpp" + +namespace hal { +Tim::Tim(Config& tim_config) : handle(tim_config.handle) { + tim_config.init_function(); +} + +void Tim::base_start() { + HAL_TIM_Base_Start(this->handle); +} + +void Tim::base_stop() { + HAL_TIM_Base_Stop(this->handle); +} + +void Tim::set_counter(uint32_t counter) { + __HAL_TIM_SET_COUNTER(this->handle, counter); +} + +uint32_t Tim::get_counter() { + return __HAL_TIM_GET_COUNTER(this->handle); +} + +void Tim::pwm_start(uint32_t channel) { + HAL_TIM_PWM_Start(this->handle, channel); +} + +void Tim::pwm_stop(uint32_t channel) { + HAL_TIM_PWM_Stop(this->handle, channel); +} + +void Tim::set_prescaler(uint32_t prescaler) { + __HAL_TIM_SET_PRESCALER(this->handle, prescaler); +} + +void Tim::set_compare(uint32_t channel, uint32_t compare) { + __HAL_TIM_SET_COMPARE(this->handle, channel, compare); +} + +uint32_t Tim::get_autoreload() { + return __HAL_TIM_GET_AUTORELOAD(this->handle); +} +} // namespace hal From 7b8509a446b68f5eb2b07826f4dbc7b4ef0cec50 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 05:30:10 -0300 Subject: [PATCH 012/561] =?UTF-8?q?=E2=9C=A8=20Add=20timer=20reset=20and?= =?UTF-8?q?=20get=20timer=20value=20functions=20to=20MCU=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/mcu.hpp | 16 +++++++++++++--- src/hal/mcu.cpp | 8 ++++++++ 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index d9cb6da..1cfe55c 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -33,14 +33,24 @@ class mcu { /** * @brief Put the MCU to sleep * - * @param ms Sleep time in milliseconds + * @param ms Sleep time in milliseconds */ static void sleep(uint32_t ms); /** - * @brief Toggles LED + * @brief Reset timer. + * + * @param tim_counter Timer counter to reset. + */ + static void reset_timer(uint32_t& tim_counter); + + /** + * @brief Get timer value in milliseconds. + * + * @param tim_counter Timer counter to get value from. + * @return Timer value in milliseconds. */ - static void led_toggle(void); + static uint32_t get_timer_ms(uint32_t tim_counter); }; }; #endif // __MCU_HPP__ diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index 220c02f..3db9684 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -27,4 +27,12 @@ void mcu::init(void) { void mcu::sleep(uint32_t ms) { HAL_Delay(ms); } + +void mcu::reset_timer(uint32_t& ticks) { + ticks = HAL_GetTick(); +} + +uint32_t mcu::get_timer_ms(uint32_t ticks) { + return HAL_GetTick() - ticks; +} } From c71b03ac91ce3bfcab394bcab016273ca6112627 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 05:30:29 -0300 Subject: [PATCH 013/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Refactor=20button?= =?UTF-8?q?=20class=20to=20add=20status=20checking=20and=20edge=20detectio?= =?UTF-8?q?n?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/button.hpp | 121 +++++++++++++++++++++++++++++++++++++++---- src/proxy/button.cpp | 66 +++++++++++++++++++++-- 2 files changed, 174 insertions(+), 13 deletions(-) diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index 328cf3d..25370a9 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -10,37 +10,140 @@ #define __BUTTON_HPP__ #include "hal/gpio.hpp" +#include namespace proxy { /** - * @brief Class for controlling Button + * @brief Class for controlling buttons. */ class Button { public: /** - * @brief configuration structure for button + * @brief Enum for button status. + */ + enum Status { + NO_PRESS, + SHORT_PRESS, + LONG_PRESS, + EXTRA_LONG_PRESS + }; + + /** + * @brief Enum for button pull resistor. + */ + enum PullResistor { + PULL_UP, + PULL_DOWN, + }; + + /** + * @brief Configuration structure for button. */ struct Config { hal::Gpio::Config gpio_config; + PullResistor pull_resistor; + uint16_t debounce_delay_ms = 10; + uint16_t long_press_delay_ms = 1000; + uint16_t extra_long_press_delay_ms = 5000; }; /** - * @brief Constructor for the Button class + * @brief Constructor for Button class. * - * @param button_config Configuration for the button + * @param button_config Button configuration. */ Button(const Config& button_config); /** - * @brief Get the button state + * @brief Get button status. * - * @return true if the button is pressed, false otherwise + * @return Button status. */ - bool get_state(); + Status get_status(); + + /** + * @brief Check if button is pressed. + * + * @return True if button is pressed, false otherwise. + */ + bool is_pressed(); + + /** + * @brief Check if button is released. + * + * @return True if button is released, false otherwise. + */ + bool is_released(); + + /** + * @brief Check if button was just pressed. + * + * @return True if button was just pressed, false otherwise. + */ + bool is_rising_edge() const; + + /** + * @brief Check if button was just released. + * + * @return True if button was just released, false otherwise. + */ + bool is_falling_edge() const; private: - hal::Gpio button_gpio; -}; + /** + * @brief Update button state. + */ + void update_state(); + + /** + * @brief Get raw button reading. + * + * @return Button reading. + */ + bool get_raw_reading() const; + + /** + * @brief Button pressing delays. + */ + const uint16_t debounce_delay_ms; + const uint16_t long_press_delay_ms; + const uint16_t extra_long_press_delay_ms; + + /** + * @brief Gpio object for button. + */ + hal::Gpio gpio; + + /** + * @brief pull resistor configuration + */ + PullResistor pull_resistor; + + /** + * @brief timer to check if button is debouncing + */ + uint32_t debounce_timer = 0; + + /** + * @brief timer to determine type of button press + */ + uint32_t status_timer = 0; + + /** + * @brief flag to know when button is debouncing + */ + bool is_debouncing = false; + + /** + * @brief flag to know if button was being pressed + */ + bool previous_state = false; + + /** + * @brief flag to know if button is being pressed + */ + bool current_state = false; +}; // class Button } // namespace proxy #endif // __BUTTON_HPP__ diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index 10e9374..65eda81 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -4,16 +4,74 @@ * @brief Proxy Button class source * * @date 03/2024 - * */ +#include "hal/mcu.hpp" #include "proxy/button.hpp" namespace proxy { -Button::Button(const Config& button_config) : button_gpio(button_config.gpio_config) { +Button::Button(const Config& button_config) : + debounce_delay_ms(button_config.debounce_delay_ms), + long_press_delay_ms(button_config.long_press_delay_ms), + extra_long_press_delay_ms(button_config.extra_long_press_delay_ms), + gpio(button_config.gpio_config), + pull_resistor(button_config.pull_resistor) { +} + +void Button::update_state() { + bool raw_reading = this->get_raw_reading(); + + if ((raw_reading != this->current_state) && !this->is_debouncing) { + this->is_debouncing = true; + hal::mcu::reset_timer(this->debounce_timer); + } else if ((hal::mcu::get_timer_ms(this->debounce_timer) < debounce_delay_ms) && this->is_debouncing) { + if (this->current_state == raw_reading) { + this->is_debouncing = false; + } + } else { + this->is_debouncing = false; + this->previous_state = this->current_state; + this->current_state = raw_reading; + } +} + +Button::Status Button::get_status() { + this->update_state(); + + if (this->is_rising_edge()) { + hal::mcu::reset_timer(this->status_timer); + } else if (this->is_falling_edge()) { + if (hal::mcu::get_timer_ms(this->status_timer) > extra_long_press_delay_ms) { + return EXTRA_LONG_PRESS; + } else if (hal::mcu::get_timer_ms(this->status_timer) > long_press_delay_ms) { + return LONG_PRESS; + } else { + return SHORT_PRESS; + } + } + + return NO_PRESS; +} + +bool Button::is_pressed() { + this->update_state(); + return (this->current_state == true); +} + +bool Button::is_released() { + this->update_state(); + return (this->current_state == false); +} + +bool Button::is_rising_edge() const { + return (this->current_state == true && this->previous_state == false); +} + +bool Button::is_falling_edge() const { + return (this->current_state == false && this->previous_state == true); } -bool Button::get_state() { - return this->button_gpio.read(); +bool Button::get_raw_reading() const { + return (this->gpio.read() == (bool) this->pull_resistor); } } // namespace proxy From 90d259e520e654b8fc2db707602dc6b3d77c6b1c Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 05:30:55 -0300 Subject: [PATCH 014/561] =?UTF-8?q?=E2=9C=A8=20Add=20CurrentSensorArray=20?= =?UTF-8?q?class=20implementation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/current_sensor_array.hpp | 58 ++++++++++++++++++++++++++++++ src/proxy/current_sensor_array.cpp | 22 ++++++++++++ 2 files changed, 80 insertions(+) create mode 100644 inc/proxy/current_sensor_array.hpp create mode 100644 src/proxy/current_sensor_array.cpp diff --git a/inc/proxy/current_sensor_array.hpp b/inc/proxy/current_sensor_array.hpp new file mode 100644 index 0000000..2d45631 --- /dev/null +++ b/inc/proxy/current_sensor_array.hpp @@ -0,0 +1,58 @@ +/** + * @file current_sensor_array.hpp + * + * @brief Proxy CurrentSensorArray class header + * + * @date 03/2024 + */ + +#ifndef __CURRENT_SENSOR_ARRAY_HPP__ +#define __CURRENT_SENSOR_ARRAY_HPP__ + +#include + +#include "hal/adc_dma.hpp" + +namespace proxy { +/** + * @brief Class for controlling CurrentSensorArray. + */ +template +class CurrentSensorArray { + public: + /** + * @brief configuration structure for current sensor + */ + struct Config { + hal::AdcDma::Config adc_config; + }; + + /** + * @brief Constructor for the CurrentSensorArray class + * + * @param current_sensor_config Configuration for the current sensor + */ + CurrentSensorArray(const Config& current_sensor_config); + + /** + * @brief Get the current from the sensor + * + * @param sensor_index Index of the sensor + * @return uint16_t Current reading from the sensor + */ + uint16_t get_current(uint8_t sensor_index); + + private: + /** + * @brief ADC DMA handle. + */ + hal::AdcDma current_sensor_adc; + + /** + * @brief Buffer to store the ADC values + */ + volatile uint32_t dma_buffer[num_of_sensors]; +}; +} // namespace proxy + +#endif // __CURRENT_SENSOR_ARRAY_HPP__ diff --git a/src/proxy/current_sensor_array.cpp b/src/proxy/current_sensor_array.cpp new file mode 100644 index 0000000..ff22f81 --- /dev/null +++ b/src/proxy/current_sensor_array.cpp @@ -0,0 +1,22 @@ +/** + * @file current_sensor_array.cpp + * + * @brief Proxy CurrentSensorArray class implementation + * + * @date 03/2024 + */ + +#include "proxy/current_sensor_array.hpp" + +namespace proxy { +template +CurrentSensorArray::CurrentSensorArray(const Config& current_sensor_config) : current_sensor_adc( + current_sensor_config.adc_config) { + this->current_sensor_adc.start_dma(this->adc_buffer, num_of_sensors); +} + +template +uint16_t CurrentSensorArray::get_current(uint8_t sensor_index) { + return this->adc_buffer[sensor_index]; +} +} // namespace proxy From 61d1e1a6c11c92e8ba36d3852f57806aa0b88efd Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 05:31:06 -0300 Subject: [PATCH 015/561] =?UTF-8?q?=E2=9C=A8=20Add=20DistanceSensorArray?= =?UTF-8?q?=20class=20and=20its=20implementation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/distance_sensor_array.hpp | 78 +++++++++++++++++++++++++++++ src/proxy/distance_sensor_array.cpp | 32 ++++++++++++ 2 files changed, 110 insertions(+) create mode 100644 inc/proxy/distance_sensor_array.hpp create mode 100644 src/proxy/distance_sensor_array.cpp diff --git a/inc/proxy/distance_sensor_array.hpp b/inc/proxy/distance_sensor_array.hpp new file mode 100644 index 0000000..208f37e --- /dev/null +++ b/inc/proxy/distance_sensor_array.hpp @@ -0,0 +1,78 @@ +/** + * @file distance_sensor_array.hpp + * + * @brief Proxy DistanceSensorArray class header + * + * @date 03/2024 + */ + +#ifndef __DISTANCE_SENSOR_ARRAY_HPP__ +#define __DISTANCE_SENSOR_ARRAY_HPP__ + +#include + +#include "hal/adc_dma.hpp" +#include "hal/tim.hpp" + +namespace proxy { +/** + * @brief Class for controlling DistanceSensorArray. + */ +template +class DistanceSensorArray { + public: + /** + * @brief configuration structure for distance sensor + */ + struct Config { + hal::AdcDma::Config sensors_adc_config; + hal::Tim::Config infrared_tim_config; + uint32_t infrared_led_channel; + }; + + /** + * @brief Constructor for the DistanceSensorArray class + * + * @param distance_sensor_config Configuration for the distance sensor + */ + DistanceSensorArray(const Config& distance_sensor_config); + + /** + * @brief Set the infrared DISTANCE_SENSOR_ARRAY intensity + * + * @param intensity Intensity of the infrared DISTANCE_SENSOR_ARRAY + */ + void set_infrared_led_intensity(uint8_t intensity); + + /** + * @brief Get the distance from the sensor + * + * @param sensor_index Index of the sensor + * @return uint16_t Distance reading from the sensor + */ + uint16_t get_distance(uint8_t sensor_index); + + private: + /** + * @brief ADC DMA handle. + */ + hal::AdcDma distance_sensor_adc; + + /** + * @brief Timer handle for infrared LED + */ + hal::Tim infrared_led_tim; + + /** + * @brief Channel for infrared LED + */ + uint32_t infrared_led_channel; + + /** + * @brief Buffer to store the ADC values + */ + volatile uint32_t dma_buffer[num_of_sensors]; +}; +} // namespace proxy + +#endif // __DISTANCE_SENSOR_ARRAY_HPP__ diff --git a/src/proxy/distance_sensor_array.cpp b/src/proxy/distance_sensor_array.cpp new file mode 100644 index 0000000..84f87ee --- /dev/null +++ b/src/proxy/distance_sensor_array.cpp @@ -0,0 +1,32 @@ +/** + * @file distance_sensor_array.cpp + * + * @brief Proxy DistanceSensorArray class implementation + * + * @date 03/2024 + */ + +#include "proxy/distance_sensor_array.hpp" + +namespace proxy { +template +DistanceSensorArray::DistanceSensorArray(const Config& distance_sensor_config) : + distance_sensor_adc(distance_sensor_config.sensor_adc_config), + infrared_led_tim(distance_sensor_config.infrared_tim_config), + infrared_led_channel(distance_sensor_config.infrared_led_channel) { + this->distance_sensor_adc.start_dma(this->adc_buffer, num_of_sensors); + this->infrared_led_tim.pwm_start(this->infrared_led_channel); + this->infrared_led_tim.set_compare(this->infrared_led_tim.get_autoreload()); +} + +template +void DistanceSensorArray::set_infrared_led_intensity(uint8_t intensity) { + uint32_t compare = (intensity * this->infrared_led_tim.get_autoreload()) / 255; + this->infrared_led_tim.set_compare(this->infrared_led_channel, compare); +} + +template +uint16_t DistanceSensorArray::get_distance(uint8_t sensor_index) { + return this->adc_buffer[sensor_index]; +} +} // namespace proxy From b2f649e447d0d73c3c88b177b51ea5e108aba107 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 20 Mar 2024 18:42:44 -0300 Subject: [PATCH 016/561] =?UTF-8?q?=F0=9F=92=A1=20Add=20comments=20to=20gp?= =?UTF-8?q?io=20hal?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/gpio.hpp | 11 ++++++++++- src/hal/gpio.cpp | 4 ++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index 25e1d9d..21299f3 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -9,6 +9,8 @@ #ifndef __GPIO_HPP__ #define __GPIO_HPP__ +#include + #include "gpio.h" namespace hal { @@ -52,8 +54,15 @@ class Gpio { void toggle(); private: - uint16_t pin; + /** + * @brief The port of the GPIO + */ GPIO_TypeDef* port; + + /** + * @brief The pin number of the GPIO + */ + uint16_t pin; }; } // namespace hal diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index aafd745..179fdc8 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -9,7 +9,7 @@ #include "hal/gpio.hpp" namespace hal { -Gpio::Gpio(const Config& gpio_config) : pin(gpio_config.pin), port(gpio_config.port) { +Gpio::Gpio(const Config& gpio_config) : port{gpio_config.port}, pin{gpio_config.pin} { } bool Gpio::read(void) const { @@ -17,7 +17,7 @@ bool Gpio::read(void) const { } void Gpio::write(bool state) { - HAL_GPIO_WritePin(this->port, this->pin, (GPIO_PinState) state); + HAL_GPIO_WritePin(this->port, this->pin, std::static_cast(state)); } void Gpio::toggle(void) { From ea2bbb3140cc20d7ee73a4f2b3dc5a4d27136b81 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 20 Mar 2024 18:45:48 -0300 Subject: [PATCH 017/561] =?UTF-8?q?=F0=9F=92=A1=20Update=20comments=20on?= =?UTF-8?q?=20led=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/led.hpp | 19 +++++++++++-------- src/proxy/led.cpp | 2 +- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp index fb5cbd0..2a3a0c3 100644 --- a/inc/proxy/led.hpp +++ b/inc/proxy/led.hpp @@ -13,41 +13,44 @@ namespace proxy { /** - * @brief Class for controlling Led. + * @brief Class for controlling an LED */ class Led { public: /** - * @brief configuration structure for led + * @brief Configuration structure for LED */ struct Config { - hal::Gpio::Config gpio_config; + hal::Gpio::Config gpio; }; /** * @brief Constructor for the Led class * - * @param led_config Configuration for the led + * @param led_config Configuration for the LED */ Led(const Config& led_config); /** - * @brief Turn the led on + * @brief Turn the LED on */ void turn_on(); /** - * @brief Turn the led off + * @brief Turn the LED off */ void turn_off(); /** - * @brief Toggle led + * @brief Toggle the LED */ void toggle(); private: - hal::Gpio led_gpio; + /** + * @brief Gpio object for the LED + */ + hal::Gpio gpio; }; } // namespace proxy diff --git a/src/proxy/led.cpp b/src/proxy/led.cpp index bd0fb24..23cb8ba 100644 --- a/src/proxy/led.cpp +++ b/src/proxy/led.cpp @@ -9,7 +9,7 @@ #include "proxy/led.hpp" namespace proxy { -Led::Led(const Config& led_config) : led_gpio(led_config.gpio_config) { +Led::Led(const Config& led_config) : gpio{led_config.gpio} { } void Led::turn_on() { From b1767b349dd242a815c6ec23d96bd8de8fbf276d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 20 Mar 2024 18:46:37 -0300 Subject: [PATCH 018/561] =?UTF-8?q?=F0=9F=92=A1=20Update=20comments=20on?= =?UTF-8?q?=20button=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/button.hpp | 92 +++++++++++++++++++++----------------------- src/proxy/button.cpp | 63 ++++++++++++++---------------- 2 files changed, 72 insertions(+), 83 deletions(-) diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index 25370a9..bb38244 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -9,17 +9,18 @@ #ifndef __BUTTON_HPP__ #define __BUTTON_HPP__ +#include + #include "hal/gpio.hpp" -#include namespace proxy { /** - * @brief Class for controlling buttons. + * @brief Class for controlling a button */ class Button { public: /** - * @brief Enum for button status. + * @brief Enum for button status */ enum Status { NO_PRESS, @@ -29,7 +30,7 @@ class Button { }; /** - * @brief Enum for button pull resistor. + * @brief Enum for button pull resistor */ enum PullResistor { PULL_UP, @@ -37,113 +38,106 @@ class Button { }; /** - * @brief Configuration structure for button. + * @brief Configuration structure for button */ struct Config { - hal::Gpio::Config gpio_config; + hal::Gpio::Config gpio; PullResistor pull_resistor; - uint16_t debounce_delay_ms = 10; - uint16_t long_press_delay_ms = 1000; - uint16_t extra_long_press_delay_ms = 5000; + uint16_t debounce_delay = 10; + uint16_t long_press_delay = 1000; + uint16_t extra_long_press_delay = 5000; }; /** - * @brief Constructor for Button class. + * @brief Constructor for Button class * - * @param button_config Button configuration. + * @param button_config Button configuration */ Button(const Config& button_config); /** - * @brief Get button status. - * - * @return Button status. - */ - Status get_status(); - - /** - * @brief Check if button is pressed. + * @brief Check if button is pressed * - * @return True if button is pressed, false otherwise. + * @return True if button is pressed, false otherwise */ bool is_pressed(); /** - * @brief Check if button is released. + * @brief Get button status * - * @return True if button is released, false otherwise. + * @return Button status */ - bool is_released(); + Status get_status(); + private: /** - * @brief Check if button was just pressed. + * @brief Get raw button reading * - * @return True if button was just pressed, false otherwise. + * @return Button reading */ - bool is_rising_edge() const; + bool get_raw_reading() const; /** - * @brief Check if button was just released. - * - * @return True if button was just released, false otherwise. + * @brief Update button state */ - bool is_falling_edge() const; + void update_state(); - private: /** - * @brief Update button state. + * @brief Check if button was just pressed + * + * @return True if button was just pressed, false otherwise */ - void update_state(); + bool is_rising_edge() const; /** - * @brief Get raw button reading. + * @brief Check if button was just released * - * @return Button reading. + * @return True if button was just released, false otherwise */ - bool get_raw_reading() const; + bool is_falling_edge() const; /** - * @brief Button pressing delays. + * @brief Button pressing delays in ms */ - const uint16_t debounce_delay_ms; - const uint16_t long_press_delay_ms; - const uint16_t extra_long_press_delay_ms; + const uint16_t debounce_delay; + const uint16_t long_press_delay; + const uint16_t extra_long_press_delay; /** - * @brief Gpio object for button. + * @brief Gpio object for button */ - hal::Gpio gpio; + const hal::Gpio gpio; /** - * @brief pull resistor configuration + * @brief Pull resistor configuration */ PullResistor pull_resistor; /** - * @brief timer to check if button is debouncing + * @brief Timer to check if button is debouncing */ uint32_t debounce_timer = 0; /** - * @brief timer to determine type of button press + * @brief Timer to determine type of button press */ uint32_t status_timer = 0; /** - * @brief flag to know when button is debouncing + * @brief Flag to know when button is debouncing */ bool is_debouncing = false; /** - * @brief flag to know if button was being pressed + * @brief Flag to know if button was being pressed */ bool previous_state = false; /** - * @brief flag to know if button is being pressed + * @brief Flag to know if button is being pressed */ bool current_state = false; -}; // class Button +}; } // namespace proxy #endif // __BUTTON_HPP__ diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index 65eda81..95f12dd 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -11,28 +11,16 @@ namespace proxy { Button::Button(const Config& button_config) : - debounce_delay_ms(button_config.debounce_delay_ms), - long_press_delay_ms(button_config.long_press_delay_ms), - extra_long_press_delay_ms(button_config.extra_long_press_delay_ms), - gpio(button_config.gpio_config), - pull_resistor(button_config.pull_resistor) { + debounce_delay{button_config.debounce_delay}, + long_press_delay{button_config.long_press_delay}, + extra_long_press_delay{button_config.extra_long_press_delay}, + gpio{button_config.gpio}, + pull_resistor{button_config.pull_resistor} { } -void Button::update_state() { - bool raw_reading = this->get_raw_reading(); - - if ((raw_reading != this->current_state) && !this->is_debouncing) { - this->is_debouncing = true; - hal::mcu::reset_timer(this->debounce_timer); - } else if ((hal::mcu::get_timer_ms(this->debounce_timer) < debounce_delay_ms) && this->is_debouncing) { - if (this->current_state == raw_reading) { - this->is_debouncing = false; - } - } else { - this->is_debouncing = false; - this->previous_state = this->current_state; - this->current_state = raw_reading; - } +bool Button::is_pressed() { + this->update_state(); + return this->current_state; } Button::Status Button::get_status() { @@ -41,9 +29,9 @@ Button::Status Button::get_status() { if (this->is_rising_edge()) { hal::mcu::reset_timer(this->status_timer); } else if (this->is_falling_edge()) { - if (hal::mcu::get_timer_ms(this->status_timer) > extra_long_press_delay_ms) { + if (hal::mcu::get_timer(this->status_timer) > extra_long_press_delay) { return EXTRA_LONG_PRESS; - } else if (hal::mcu::get_timer_ms(this->status_timer) > long_press_delay_ms) { + } else if (hal::mcu::get_timer(this->status_timer) > long_press_delay) { return LONG_PRESS; } else { return SHORT_PRESS; @@ -53,25 +41,32 @@ Button::Status Button::get_status() { return NO_PRESS; } -bool Button::is_pressed() { - this->update_state(); - return (this->current_state == true); +bool Button::get_raw_reading() const { + return this->gpio.read() == this->pull_resistor; } -bool Button::is_released() { - this->update_state(); - return (this->current_state == false); +void Button::update_state() { + bool raw_reading = this->get_raw_reading(); + + if ((raw_reading != this->current_state) and not this->is_debouncing) { + this->is_debouncing = true; + hal::mcu::reset_timer(this->debounce_timer); + } else if ((hal::mcu::get_timer(this->debounce_timer) < debounce_delay) and this->is_debouncing) { + if (this->current_state == raw_reading) { + this->is_debouncing = false; + } + } else { + this->is_debouncing = false; + this->previous_state = this->current_state; + this->current_state = raw_reading; + } } bool Button::is_rising_edge() const { - return (this->current_state == true && this->previous_state == false); + return this->current_state and not this->previous_state; } bool Button::is_falling_edge() const { - return (this->current_state == false && this->previous_state == true); -} - -bool Button::get_raw_reading() const { - return (this->gpio.read() == (bool) this->pull_resistor); + return not this->current_state and this->previous_state; } } // namespace proxy From 2c96d9b2045af36d5cc1cf097a635818636ca896 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 21:47:51 -0300 Subject: [PATCH 019/561] =?UTF-8?q?=F0=9F=8E=A8=20Fix=20formatting=20in=20?= =?UTF-8?q?ADC=20DMA=20wrapper?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa Co-authored-by: Matheus Rezende --- inc/hal/adc_dma.hpp | 22 +++++++++++----------- src/hal/adc_dma.cpp | 4 ++-- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index 8b6c7f5..b2c1c19 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -1,7 +1,7 @@ /** * @file adc_dma.hpp * - * @brief STM32 ADC DMA HAL wrapper. + * @brief STM32 ADC DMA HAL wrapper * * @date 03/2024 */ @@ -16,12 +16,12 @@ namespace hal { /** - * @brief Class to handle ADC DMA peripheral on STM32 microcontrollers. + * @brief Class to handle ADC DMA peripheral on STM32 microcontrollers */ class AdcDma { public: /** - * @brief ADC DMA configuration struct. + * @brief ADC DMA configuration struct */ struct Config { ADC_HandleTypeDef* handle; @@ -29,28 +29,28 @@ class AdcDma { }; /** - * @brief Construct a new AdcDma object. + * @brief Construct a new AdcDma object * - * @param adc_config ADC DMA configuration struct. + * @param adc_config ADC DMA configuration struct */ AdcDma(Config& adc_config); /** - * @brief Enable ADC, start conversion of regular group and transfer result through DMA. + * @brief Enable ADC, start conversion of regular group and transfer result through DMA * - * @param buffer Destination Buffer address. - * @param size Number of data to be transferred from ADC DMA peripheral to memory. + * @param buffer Destination Buffer address + * @param size Number of data to be transferred from ADC DMA peripheral to memory */ - void start_dma(uint32_t* buffer, uint32_t size); + void start_dma(uint32_t buffer[], uint32_t size); /** - * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode). + * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode) */ void stop_dma(); private: /** - * @brief ADC DMA handle. + * @brief ADC DMA handle */ ADC_HandleTypeDef* handle; }; diff --git a/src/hal/adc_dma.cpp b/src/hal/adc_dma.cpp index 6644047..9659a77 100644 --- a/src/hal/adc_dma.cpp +++ b/src/hal/adc_dma.cpp @@ -1,7 +1,7 @@ /** * @file adc_dma.cpp * - * @brief STM32 ADC DMA HAL wrapper. + * @brief STM32 ADC DMA HAL wrapper * * @date 03/2024 */ @@ -19,7 +19,7 @@ AdcDma::AdcDma(Config& adc_config) : handle(adc_config.handle) { adc_config.init_function(); } -void AdcDma::start_dma(uint32_t* buffer, uint32_t size) { +void AdcDma::start_dma(uint32_t buffer[], uint32_t size) { HAL_ADC_Start_DMA(this->handle, buffer, size); } From aa0dafcf8499c371f56d6e890cab6a9dbf9c29f7 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 21:48:17 -0300 Subject: [PATCH 020/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20TIM=20c?= =?UTF-8?q?lass=20to=20Timer=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa Co-authored-by: Matheus Rezende --- inc/hal/tim.hpp | 106 -------------------------------------- inc/hal/timer.hpp | 127 ++++++++++++++++++++++++++++++++++++++++++++++ src/hal/tim.cpp | 51 ------------------- src/hal/timer.cpp | 63 +++++++++++++++++++++++ 4 files changed, 190 insertions(+), 157 deletions(-) delete mode 100644 inc/hal/tim.hpp create mode 100644 inc/hal/timer.hpp delete mode 100644 src/hal/tim.cpp create mode 100644 src/hal/timer.cpp diff --git a/inc/hal/tim.hpp b/inc/hal/tim.hpp deleted file mode 100644 index 87ba1a8..0000000 --- a/inc/hal/tim.hpp +++ /dev/null @@ -1,106 +0,0 @@ -/** - * @file tim.hpp - * - * @brief STM32 TIM HAL wrapper. - * - * @date 03/2024 - */ - -#ifndef __TIM_HPP__ -#define __TIM_HPP__ - -#include -#include - -#include "tim.h" - -namespace hal { -/** - * @brief Class to handle TIM peripheral on STM32 microcontrollers. - */ -class Tim { - public: - /** - * @brief TIM configuration struct. - */ - struct Config { - TIM_HandleTypeDef* handle; - std::function init_function; - }; - - /** - * @brief Constructor for the Tim class. - * - * @param tim_config Configuration for timer handler - */ - Tim(Config& tim_config); - - /** - * @brief Initialize timer base generation. - */ - void base_start(); - - /** - * @brief Stop timer base generation. - */ - void base_stop(); - - /** - * @brief Set timer counter. - * - * @param counter Counter value. - */ - void set_counter(uint32_t counter); - - /** - * @brief Get timer counter. - * - * @return uint32_t Counter value. - */ - uint32_t get_counter(); - - /** - * @brief Start PWM generation. - * - * @param channel Tim channel to be enabled. - */ - void pwm_start(uint32_t channel); - - /** - * @brief Stop PWM generation. - * - * @param channel Tim channel to be disabled. - */ - void pwm_stop(uint32_t channel); - - /** - * @brief Set timer prescaler. - * - * @param prescaler Prescaler new value. - */ - void set_prescaler(uint32_t prescaler); - - /** - * @brief Set the TIM Capture Compare Register value. - * - * @param channel TIM channel to be configured. - * @param compare Compare register new value. - */ - void set_compare(uint32_t channel, uint32_t compare); - - /** - * @brief Get the Autoreload register value. - * - * @return uint32_t The value of the Autoreload register. - */ - uint32_t get_autoreload(); - - private: - /** - * @brief Timer handler. - */ - TIM_HandleTypeDef* handle; -}; -} // namespace hal - -#endif // __TIM_HPP__ diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp new file mode 100644 index 0000000..d6ca32c --- /dev/null +++ b/inc/hal/timer.hpp @@ -0,0 +1,127 @@ +/** + * @file timer.hpp + * + * @brief STM32 Timer HAL wrapper + * + * @date 03/2024 + */ + +#ifndef __TIMER_HPP__ +#define __TIMER_HPP__ + +#include +#include + +#include "tim.h" + +namespace hal { +/** + * @brief Class to handle timer peripheral on STM32 microcontrollers + */ +class Timer { + public: + /** + * @brief Timer configuration struct + */ + struct Config { + TIM_HandleTypeDef* handle; + std::function init_function; + }; + + /** + * @brief Constructor for the Timer class + * + * @param timer_config Configuration for timer handler + */ + Timer(Config& timer_config); + + /** + * @brief Initialize timer base generation + */ + void base_start(); + + /** + * @brief Stop timer base generation + */ + void base_stop(); + + /** + * @brief Set timer counter + * + * @param counter Counter value + */ + void set_counter(uint32_t counter); + + /** + * @brief Get timer counter + * + * @return uint32_t Counter value + */ + uint32_t get_counter(); + + /** + * @brief Start PWM generation + * + * @param channel Timer channel to be enabled + */ + void pwm_start(uint32_t channel); + + /** + * @brief Stop PWM generation + * + * @param channel Timer channel to be disabled + */ + void pwm_stop(uint32_t channel); + + /** + * @brief Set timer prescaler + * + * @param prescaler Prescaler new value + */ + void set_prescaler(uint32_t prescaler); + + /** + * @brief Get the timer prescaler + * + * @return uint32_t The value of the prescaler + */ + uint32_t get_prescaler(); + + /** + * @brief Set the timer Capture Compare Register value + * + * @param channel Timer channel to be configured + * @param compare Compare register new value + */ + void set_compare(uint32_t channel, uint32_t compare); + + /** + * @brief Get the Autoreload register value + * + * @return uint32_t The value of the Autoreload register + */ + uint32_t get_autoreload(); + + /** + * @brief Set the Autoreload register value + * + * @param autoreload New value for the Autoreload register + */ + void set_autoreload(uint32_t autoreload); + + /** + * @brief Get the timer clock frequency + * + * @return uint32_t Timer clock frequency + */ + uint32_t get_timer_clock_freq(); + + private: + /** + * @brief Timer handler + */ + TIM_HandleTypeDef* handle; +}; +} // namespace hal + +#endif // __TIMER_HPP__ diff --git a/src/hal/tim.cpp b/src/hal/tim.cpp deleted file mode 100644 index bd0fa1d..0000000 --- a/src/hal/tim.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/** - * @file tim.cpp - * - * @brief STM32 TIM HAL wrapper. - * - * @date 03/2024 - */ - -#include "hal/tim.hpp" - -namespace hal { -Tim::Tim(Config& tim_config) : handle(tim_config.handle) { - tim_config.init_function(); -} - -void Tim::base_start() { - HAL_TIM_Base_Start(this->handle); -} - -void Tim::base_stop() { - HAL_TIM_Base_Stop(this->handle); -} - -void Tim::set_counter(uint32_t counter) { - __HAL_TIM_SET_COUNTER(this->handle, counter); -} - -uint32_t Tim::get_counter() { - return __HAL_TIM_GET_COUNTER(this->handle); -} - -void Tim::pwm_start(uint32_t channel) { - HAL_TIM_PWM_Start(this->handle, channel); -} - -void Tim::pwm_stop(uint32_t channel) { - HAL_TIM_PWM_Stop(this->handle, channel); -} - -void Tim::set_prescaler(uint32_t prescaler) { - __HAL_TIM_SET_PRESCALER(this->handle, prescaler); -} - -void Tim::set_compare(uint32_t channel, uint32_t compare) { - __HAL_TIM_SET_COMPARE(this->handle, channel, compare); -} - -uint32_t Tim::get_autoreload() { - return __HAL_TIM_GET_AUTORELOAD(this->handle); -} -} // namespace hal diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp new file mode 100644 index 0000000..7e1a228 --- /dev/null +++ b/src/hal/timer.cpp @@ -0,0 +1,63 @@ +/** + * @file timer.cpp + * + * @brief STM32 TIM HAL wrapper + * + * @date 03/2024 + */ + +#include "hal/timer.hpp" + +namespace hal { +Timer::Timer(Config& tim_config) : handle(tim_config.handle) { + tim_config.init_function(); +} + +void Timer::base_start() { + HAL_TIM_Base_Start(this->handle); +} + +void Timer::base_stop() { + HAL_TIM_Base_Stop(this->handle); +} + +void Timer::set_counter(uint32_t counter) { + __HAL_TIM_SET_COUNTER(this->handle, counter); +} + +uint32_t Timer::get_counter() { + return __HAL_TIM_GET_COUNTER(this->handle); +} + +void Timer::pwm_start(uint32_t channel) { + HAL_TIM_PWM_Start(this->handle, channel); +} + +void Timer::pwm_stop(uint32_t channel) { + HAL_TIM_PWM_Stop(this->handle, channel); +} + +void Timer::set_prescaler(uint32_t prescaler) { + __HAL_TIM_SET_PRESCALER(this->handle, prescaler); +} + +uint32_t Timer::get_prescaler() { + return this->handle->Instance->PSC; +} + +void Timer::set_compare(uint32_t channel, uint32_t compare) { + __HAL_TIM_SET_COMPARE(this->handle, channel, compare); +} + +uint32_t Timer::get_autoreload() { + return __HAL_TIM_GET_AUTORELOAD(this->handle); +} + +void Timer::set_autoreload(uint32_t autoreload) { + __HAL_TIM_SET_AUTORELOAD(this->handle, autoreload); +} + +uint32_t Timer::get_timer_clock_freq() { + return HAL_RCC_GetPCLK1Freq(); +} +} // namespace hal From 587d9abd9f74f697ae229a024d863d4f00b2c802 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 21:48:32 -0300 Subject: [PATCH 021/561] =?UTF-8?q?=E2=9C=A8=20Add=20PWM=20HAL=20wrapper?= =?UTF-8?q?=20for=20STM32=20microcontrollers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa Co-authored-by: Matheus Rezende --- inc/hal/pwm.hpp | 65 +++++++++++++++++++++++++++++++++++++++++++++++++ src/hal/pwm.cpp | 28 +++++++++++++++++++++ 2 files changed, 93 insertions(+) create mode 100644 inc/hal/pwm.hpp create mode 100644 src/hal/pwm.cpp diff --git a/inc/hal/pwm.hpp b/inc/hal/pwm.hpp new file mode 100644 index 0000000..93c5882 --- /dev/null +++ b/inc/hal/pwm.hpp @@ -0,0 +1,65 @@ +/** + * @file pwm.hpp + * + * @brief STM32 PWM HAL wrapper + * + * @date 03/2024 + */ + +#ifndef __PWM_HPP__ +#define __PWM_HPP__ + +#include +#include + +#include "hal/timer.hpp" + +namespace hal { +/** + * @brief Class to handle PWM peripheral on STM32 microcontrollers + */ +class Pwm { + public: + /** + * @brief PWM configuration struct + */ + struct Config { + Timer::Config timer; + uint32_t timer_channel; + }; + + /** + * @brief Construct a new Pwm object + * + * @param pwm_config Configuration for the PWM + */ + Pwm(Config& pwm_config); + + /** + * @brief Set the PWM duty cycle + * + * @param duty_cycle Duty cycle value + */ + void set_duty_cycle(float duty_cycle); + + /** + * @brief Set the PWM frequency + * + * @param frequency Frequency value + */ + void set_frequency(uint32_t frequency); + + private: + /** + * @brief Timer object + */ + Timer timer; + + /** + * @brief Channel number of the timer + */ + uint32_t channel; +}; +} // namespace hal + +#endif // __PWM_HPP__ diff --git a/src/hal/pwm.cpp b/src/hal/pwm.cpp new file mode 100644 index 0000000..bb25432 --- /dev/null +++ b/src/hal/pwm.cpp @@ -0,0 +1,28 @@ +/** + * @file pwm.cpp + * + * @brief STM32 PWM HAL wrapper + * + * @date 03/2024 + */ + +#include "hal/pwm.hpp" + +namespace hal { +Pwm::Pwm(Config& pwm_config) : timer{pwm_config.timer}, channel{pwm_config.timer_channel} { + this->timer.pwm_start(this->channel); + this->timer.set_compare(this->channel, 0); +} + +void Pwm::set_duty_cycle(float duty_cycle) { + uint32_t compare = (duty_cycle * this->timer.get_autoreload()) / 100; + this->timer.set_compare(this->channel, compare); +} + +void Pwm::set_frequency(uint32_t frequency) { + uint32_t clock_freq = this->timer.get_timer_clock_freq(); + uint32_t prescaler = this->timer.get_prescaler(); + uint32_t autoreload = clock_freq / ((prescaler + 1) * frequency); + this->timer.set_autoreload(autoreload); +} +} // namespace hal From 184151cdd6c219d74a27e93b76eb2c028b9d5231 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 21:48:55 -0300 Subject: [PATCH 022/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20distanc?= =?UTF-8?q?e=20sensor=20classes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa Co-authored-by: Matheus Rezende --- inc/proxy/distance_sensor_array.hpp | 78 ---------------------------- inc/proxy/distance_sensors.hpp | 80 +++++++++++++++++++++++++++++ src/proxy/distance_sensor_array.cpp | 32 ------------ src/proxy/distance_sensors.cpp | 38 ++++++++++++++ 4 files changed, 118 insertions(+), 110 deletions(-) delete mode 100644 inc/proxy/distance_sensor_array.hpp create mode 100644 inc/proxy/distance_sensors.hpp delete mode 100644 src/proxy/distance_sensor_array.cpp create mode 100644 src/proxy/distance_sensors.cpp diff --git a/inc/proxy/distance_sensor_array.hpp b/inc/proxy/distance_sensor_array.hpp deleted file mode 100644 index 208f37e..0000000 --- a/inc/proxy/distance_sensor_array.hpp +++ /dev/null @@ -1,78 +0,0 @@ -/** - * @file distance_sensor_array.hpp - * - * @brief Proxy DistanceSensorArray class header - * - * @date 03/2024 - */ - -#ifndef __DISTANCE_SENSOR_ARRAY_HPP__ -#define __DISTANCE_SENSOR_ARRAY_HPP__ - -#include - -#include "hal/adc_dma.hpp" -#include "hal/tim.hpp" - -namespace proxy { -/** - * @brief Class for controlling DistanceSensorArray. - */ -template -class DistanceSensorArray { - public: - /** - * @brief configuration structure for distance sensor - */ - struct Config { - hal::AdcDma::Config sensors_adc_config; - hal::Tim::Config infrared_tim_config; - uint32_t infrared_led_channel; - }; - - /** - * @brief Constructor for the DistanceSensorArray class - * - * @param distance_sensor_config Configuration for the distance sensor - */ - DistanceSensorArray(const Config& distance_sensor_config); - - /** - * @brief Set the infrared DISTANCE_SENSOR_ARRAY intensity - * - * @param intensity Intensity of the infrared DISTANCE_SENSOR_ARRAY - */ - void set_infrared_led_intensity(uint8_t intensity); - - /** - * @brief Get the distance from the sensor - * - * @param sensor_index Index of the sensor - * @return uint16_t Distance reading from the sensor - */ - uint16_t get_distance(uint8_t sensor_index); - - private: - /** - * @brief ADC DMA handle. - */ - hal::AdcDma distance_sensor_adc; - - /** - * @brief Timer handle for infrared LED - */ - hal::Tim infrared_led_tim; - - /** - * @brief Channel for infrared LED - */ - uint32_t infrared_led_channel; - - /** - * @brief Buffer to store the ADC values - */ - volatile uint32_t dma_buffer[num_of_sensors]; -}; -} // namespace proxy - -#endif // __DISTANCE_SENSOR_ARRAY_HPP__ diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp new file mode 100644 index 0000000..6aa591a --- /dev/null +++ b/inc/proxy/distance_sensors.hpp @@ -0,0 +1,80 @@ +/** + * @file distance_sensors.hpp + * + * @brief Proxy DistanceSensors class header + * + * @date 03/2024 + */ + +#ifndef __DISTANCE_SENSORS_HPP__ +#define __DISTANCE_SENSORS_HPP__ + +#include + +#include "hal/adc_dma.hpp" +#include "hal/pwm.hpp" + +namespace proxy { +/** + * @brief Class for controlling DistanceSensors + */ +template +class DistanceSensors { + public: + /** + * @brief configuration structure for distance sensors + */ + struct Config { + hal::AdcDma::Config adc; + hal::Pwm::Config infrared_pwm; + }; + + /** + * @brief Constructor for the DistanceSensors class + * + * @param distance_sensor_config Configuration for the distance sensors + */ + DistanceSensors(const Config& distance_sensor_config); + + /** + * @brief Set the infrared DistanceSensors intensity + * + * @param intensity Intensity percentage of the infrared LED + */ + void set_infrared_led_intensity(float intensity); + + /** + * @brief Get the distance from a sensor + * + * @param sensor_index Index of the sensor + * @return float Distance reading from the sensors + */ + float get_distance(uint8_t sensor_index); + + /** + * @brief Get the distance from a sensor + * + * @param sensor_index Index of the sensor + * @return uint32_t Raw reading from the distance sensor + */ + uint32_t get_distance_raw(uint8_t sensor_index); + + private: + /** + * @brief ADC DMA handle + */ + hal::AdcDma distance_sensor_adc; + + /** + * @brief PWM handle for infrared LED + */ + hal::Pwm infrared_pwm; + + /** + * @brief Buffer to store the ADC values + */ + volatile std::array dma_buffer; +}; +} // namespace proxy + +#endif // __DISTANCE_SENSORS_HPP__ diff --git a/src/proxy/distance_sensor_array.cpp b/src/proxy/distance_sensor_array.cpp deleted file mode 100644 index 84f87ee..0000000 --- a/src/proxy/distance_sensor_array.cpp +++ /dev/null @@ -1,32 +0,0 @@ -/** - * @file distance_sensor_array.cpp - * - * @brief Proxy DistanceSensorArray class implementation - * - * @date 03/2024 - */ - -#include "proxy/distance_sensor_array.hpp" - -namespace proxy { -template -DistanceSensorArray::DistanceSensorArray(const Config& distance_sensor_config) : - distance_sensor_adc(distance_sensor_config.sensor_adc_config), - infrared_led_tim(distance_sensor_config.infrared_tim_config), - infrared_led_channel(distance_sensor_config.infrared_led_channel) { - this->distance_sensor_adc.start_dma(this->adc_buffer, num_of_sensors); - this->infrared_led_tim.pwm_start(this->infrared_led_channel); - this->infrared_led_tim.set_compare(this->infrared_led_tim.get_autoreload()); -} - -template -void DistanceSensorArray::set_infrared_led_intensity(uint8_t intensity) { - uint32_t compare = (intensity * this->infrared_led_tim.get_autoreload()) / 255; - this->infrared_led_tim.set_compare(this->infrared_led_channel, compare); -} - -template -uint16_t DistanceSensorArray::get_distance(uint8_t sensor_index) { - return this->adc_buffer[sensor_index]; -} -} // namespace proxy diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp new file mode 100644 index 0000000..b84aa6f --- /dev/null +++ b/src/proxy/distance_sensors.cpp @@ -0,0 +1,38 @@ +/** + * @file distance_sensor.cpp + * + * @brief Proxy DistanceSensors class implementation + * + * @date 03/2024 + */ + +#include "proxy/distance_sensors.hpp" + +namespace proxy { +static constexpr float max_distance{0.3f}; + +static constexpr uint32_t max_adc_reading{4095}; + +template +DistanceSensors::DistanceSensors(const Config& distance_sensor_config) : + distance_sensor_adc{distance_sensor_config.sensor_adc_config}, + infrared_pwm{distance_sensor_config.infrared_pwm} { + this->distance_sensor_adc.start_dma(this->adc_buffer.data(), num_of_sensors); + this->infrared_pwm.set_duty_cycle(100.0f); +} + +template +void DistanceSensors::set_infrared_led_intensity(float intensity) { + this->infrared_pwm.set_duty_cycle(intensity); +} + +template +float DistanceSensors::get_distance(uint8_t sensor_index) { + return max_distance * this->adc_buffer.at(sensor_index) / max_adc_reading; +} + +template +uint32_t DistanceSensors::get_distance_raw(uint8_t sensor_index) { + return this->adc_buffer.at(sensor_index); +} +} // namespace proxy From 68bbca9910b2e5d119dd3d721506840f99823b65 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 21:49:16 -0300 Subject: [PATCH 023/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20current?= =?UTF-8?q?=20sensor=20classes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa Co-authored-by: Matheus Rezende --- inc/proxy/current_sensor_array.hpp | 58 ------------------------ inc/proxy/current_sensors.hpp | 72 ++++++++++++++++++++++++++++++ src/proxy/current_sensor_array.cpp | 22 --------- src/proxy/current_sensors.cpp | 32 +++++++++++++ 4 files changed, 104 insertions(+), 80 deletions(-) delete mode 100644 inc/proxy/current_sensor_array.hpp create mode 100644 inc/proxy/current_sensors.hpp delete mode 100644 src/proxy/current_sensor_array.cpp create mode 100644 src/proxy/current_sensors.cpp diff --git a/inc/proxy/current_sensor_array.hpp b/inc/proxy/current_sensor_array.hpp deleted file mode 100644 index 2d45631..0000000 --- a/inc/proxy/current_sensor_array.hpp +++ /dev/null @@ -1,58 +0,0 @@ -/** - * @file current_sensor_array.hpp - * - * @brief Proxy CurrentSensorArray class header - * - * @date 03/2024 - */ - -#ifndef __CURRENT_SENSOR_ARRAY_HPP__ -#define __CURRENT_SENSOR_ARRAY_HPP__ - -#include - -#include "hal/adc_dma.hpp" - -namespace proxy { -/** - * @brief Class for controlling CurrentSensorArray. - */ -template -class CurrentSensorArray { - public: - /** - * @brief configuration structure for current sensor - */ - struct Config { - hal::AdcDma::Config adc_config; - }; - - /** - * @brief Constructor for the CurrentSensorArray class - * - * @param current_sensor_config Configuration for the current sensor - */ - CurrentSensorArray(const Config& current_sensor_config); - - /** - * @brief Get the current from the sensor - * - * @param sensor_index Index of the sensor - * @return uint16_t Current reading from the sensor - */ - uint16_t get_current(uint8_t sensor_index); - - private: - /** - * @brief ADC DMA handle. - */ - hal::AdcDma current_sensor_adc; - - /** - * @brief Buffer to store the ADC values - */ - volatile uint32_t dma_buffer[num_of_sensors]; -}; -} // namespace proxy - -#endif // __CURRENT_SENSOR_ARRAY_HPP__ diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp new file mode 100644 index 0000000..cfdaf38 --- /dev/null +++ b/inc/proxy/current_sensors.hpp @@ -0,0 +1,72 @@ +/** + * @file current_sensors.hpp + * + * @brief Proxy CurrentSensors class header + * + * @date 03/2024 + */ + +#ifndef __CURRENT_SENSORS_HPP__ +#define __CURRENT_SENSORS_HPP__ + +#include + +#include "hal/adc_dma.hpp" + +namespace proxy { +/** + * @brief Class for controlling CurrentSensors + */ +template +class CurrentSensors { + public: + /** + * @brief configuration structure for current sensors + */ + struct Config { + hal::AdcDma::Config adc; + float shunt_resistor; + }; + + /** + * @brief Constructor for the CurrentSensors class + * + * @param current_sensors_config Configuration for the current sensors + */ + CurrentSensors(const Config& current_sensors_config); + + /** + * @brief Get the current from the sensor + * + * @param sensor_index Index of the sensor + * @return float Current reading from the sensor + */ + float get_current(uint8_t sensor_index); + + /** + * @brief Get the raw reading from the current sensor + * + * @param sensor_index Index of the sensor + * @return uint16_t Current reading from the sensor + */ + uint32_t get_current_raw(uint8_t sensor_index); + + private: + /** + * @brief ADC DMA handle + */ + hal::AdcDma adc; + + /** + * @brief Buffer to store the ADC values + */ + volatile std::array dma_buffer; + + /** + * @brief Value of the shunt resistor in ohms + */ + float shunt_resistor; +}; +} // namespace proxy + +#endif // __CURRENT_SENSORS_HPP__ diff --git a/src/proxy/current_sensor_array.cpp b/src/proxy/current_sensor_array.cpp deleted file mode 100644 index ff22f81..0000000 --- a/src/proxy/current_sensor_array.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/** - * @file current_sensor_array.cpp - * - * @brief Proxy CurrentSensorArray class implementation - * - * @date 03/2024 - */ - -#include "proxy/current_sensor_array.hpp" - -namespace proxy { -template -CurrentSensorArray::CurrentSensorArray(const Config& current_sensor_config) : current_sensor_adc( - current_sensor_config.adc_config) { - this->current_sensor_adc.start_dma(this->adc_buffer, num_of_sensors); -} - -template -uint16_t CurrentSensorArray::get_current(uint8_t sensor_index) { - return this->adc_buffer[sensor_index]; -} -} // namespace proxy diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp new file mode 100644 index 0000000..6a43cec --- /dev/null +++ b/src/proxy/current_sensors.cpp @@ -0,0 +1,32 @@ +/** + * @file current_sensors.cpp + * + * @brief Proxy CurrentSensors class implementation + * + * @date 03/2024 + */ + +#include "proxy/current_sensors.hpp" + +namespace proxy { +static constexpr float reference_voltage{3.3f}; + +static constexpr uint32_t max_adc_reading{4095}; + +template +CurrentSensors::CurrentSensors(const Config& current_sensors_config) : + adc(current_sensors_config.adc_config), shunt_resistor(current_sensors_config.shunt_resistor) { + this->adc.start_dma(this->adc_buffer.data(), num_of_sensors); + this->adc.start_dma(this->adc_buffer.data(), num_of_sensors); +} + +template +float CurrentSensors::get_current(uint8_t sensor_index) { + return reference_voltage * this->adc_buffer.at(sensor_index) / (max_adc_reading * this->shunt_resistor); +} + +template +uint32_t CurrentSensors::get_current_raw(uint8_t sensor_index) { + return this->adc_buffer.at(sensor_index); +} +} // namespace proxy From e5a7e95c86f47bc6edbf2b5f76646ecf15f3be93 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 21:49:56 -0300 Subject: [PATCH 024/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20GPIO=20?= =?UTF-8?q?write=20method=20to=20use=20C-style=20cast?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa Co-authored-by: Matheus Rezende --- src/hal/gpio.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index 179fdc8..c6a0d3b 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -17,7 +17,7 @@ bool Gpio::read(void) const { } void Gpio::write(bool state) { - HAL_GPIO_WritePin(this->port, this->pin, std::static_cast(state)); + HAL_GPIO_WritePin(this->port, this->pin, (GPIO_PinState) state); } void Gpio::toggle(void) { From 713aea22be226101087b87271f044b9eabe7ef8c Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 21:50:52 -0300 Subject: [PATCH 025/561] =?UTF-8?q?=F0=9F=9A=A8=20Update=20button.cpp=20wr?= =?UTF-8?q?ong=20function=20name?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa Co-authored-by: Matheus Rezende --- src/proxy/button.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index 95f12dd..96d2253 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -29,9 +29,9 @@ Button::Status Button::get_status() { if (this->is_rising_edge()) { hal::mcu::reset_timer(this->status_timer); } else if (this->is_falling_edge()) { - if (hal::mcu::get_timer(this->status_timer) > extra_long_press_delay) { + if (hal::mcu::get_timer_ms(this->status_timer) > extra_long_press_delay) { return EXTRA_LONG_PRESS; - } else if (hal::mcu::get_timer(this->status_timer) > long_press_delay) { + } else if (hal::mcu::get_timer_ms(this->status_timer) > long_press_delay) { return LONG_PRESS; } else { return SHORT_PRESS; @@ -51,7 +51,7 @@ void Button::update_state() { if ((raw_reading != this->current_state) and not this->is_debouncing) { this->is_debouncing = true; hal::mcu::reset_timer(this->debounce_timer); - } else if ((hal::mcu::get_timer(this->debounce_timer) < debounce_delay) and this->is_debouncing) { + } else if ((hal::mcu::get_timer_ms(this->debounce_timer) < debounce_delay) and this->is_debouncing) { if (this->current_state == raw_reading) { this->is_debouncing = false; } From 60fc4855b5f05d777ec791783c94b058d7b580e7 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 21:51:07 -0300 Subject: [PATCH 026/561] =?UTF-8?q?=E2=9C=A8=20Add=20DipSwitch=20class=20f?= =?UTF-8?q?or=20controlling=20a=20dip=20switch?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa Co-authored-by: Matheus Rezende --- inc/proxy/dip_switch.hpp | 39 +++++++++++++++++++++++++++++++++++++++ src/proxy/dip_switch.cpp | 35 +++++++++++++++++++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 inc/proxy/dip_switch.hpp create mode 100644 src/proxy/dip_switch.cpp diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp new file mode 100644 index 0000000..f00b890 --- /dev/null +++ b/inc/proxy/dip_switch.hpp @@ -0,0 +1,39 @@ +/** + * @file dip_switch.hpp + * + * @brief Proxy Dip Switch class header + * + * @date 03/2024 + */ + +#ifndef __DIP_SWITCH_HPP__ +#define __DIP_SWITCH_HPP__ + +#include +#include + +#include "hal/gpio.hpp" + +namespace proxy { +/** + * @brief Class for controlling a dip switch + */ +template +class DipSwitch { + public: + struct Config { + std::array gpio_array; + }; + + DipSwitch(const Config& dip_switch_config); + + bool get_switch_state(uint8_t switch_index); + + uint8_t get_switches_value(); + + private: + std::array gpio_array; +}; +} // namespace proxy + +#endif // __DIP_SWITCH_HPP__ diff --git a/src/proxy/dip_switch.cpp b/src/proxy/dip_switch.cpp new file mode 100644 index 0000000..57e8ffd --- /dev/null +++ b/src/proxy/dip_switch.cpp @@ -0,0 +1,35 @@ +/** + * @file dip_switch.cpp + * + * @brief Proxy DIP Switch class source + * + * @date 03/2024 + */ + +#include "hal/mcu.hpp" +#include "proxy/dip_switch.hpp" + +namespace proxy { +template +DipSwitch::DipSwitch(const Config& dip_switch_config) { + for (uint8_t i = 0; i < num_of_sensors; i++) { + gpio_array[i] = dip_switch_config.gpio_array[i]; + } +} + +template +bool DipSwitch::get_switch_state(uint8_t switch_index) { + return gpio_array.at(switch_index).read(); +} + +template +uint8_t DipSwitch::get_switches_value() { + uint8_t switches_value = 0; + + for (uint8_t i = 0; i < num_of_sensors; i++) { + switches_value |= (gpio_array[i].read() << i); + } + + return switches_value; +} +} // namespace proxy From 15ca18f056268e82f36bf884530bb7a8e26d94dc Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 21:51:27 -0300 Subject: [PATCH 027/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20LED=20c?= =?UTF-8?q?lass=20to=20use=20'gpio'=20member=20variable=20instead=20of=20'?= =?UTF-8?q?led=5Fgpio'?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa Co-authored-by: Matheus Rezende --- src/proxy/led.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/proxy/led.cpp b/src/proxy/led.cpp index 23cb8ba..9820101 100644 --- a/src/proxy/led.cpp +++ b/src/proxy/led.cpp @@ -13,14 +13,14 @@ Led::Led(const Config& led_config) : gpio{led_config.gpio} { } void Led::turn_on() { - this->led_gpio.write(true); + this->gpio.write(true); } void Led::turn_off() { - this->led_gpio.write(false); + this->gpio.write(false); } void Led::toggle() { - this->led_gpio.toggle(); + this->gpio.toggle(); } } // namespace proxy From 8a30d8727d47129752cfb5b8a706150c3359a7b6 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 20 Mar 2024 21:52:09 -0300 Subject: [PATCH 028/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Update=20MCU=20hea?= =?UTF-8?q?der=20file=20to=20use=20cstdint=20instead=20of=20stdint.h?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa Co-authored-by: Matheus Rezende --- inc/hal/mcu.hpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index 1cfe55c..8066c8c 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -2,12 +2,14 @@ * @file mcu.hpp * * @brief MCU related + * + * @date 03/2024 */ #ifndef __MCU_HPP__ #define __MCU_HPP__ -#include +#include /***************************************** * Public Function Prototypes @@ -38,19 +40,19 @@ class mcu { static void sleep(uint32_t ms); /** - * @brief Reset timer. + * @brief Reset timer * - * @param tim_counter Timer counter to reset. + * @param tim_counter Timer counter to reset */ static void reset_timer(uint32_t& tim_counter); /** - * @brief Get timer value in milliseconds. + * @brief Get timer value in milliseconds * - * @param tim_counter Timer counter to get value from. - * @return Timer value in milliseconds. + * @param tim_counter Timer counter to get value from + * @return Timer value in milliseconds */ static uint32_t get_timer_ms(uint32_t tim_counter); }; -}; +}; // namespace hal #endif // __MCU_HPP__ From 067423682a3fdfbed6ab938ca268055b1ddaf836 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:43:06 -0300 Subject: [PATCH 029/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Improve=20ADC=20DM?= =?UTF-8?q?A=20initialization=20and=20include=20header=20file?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/hal/adc_dma.hpp | 5 ++--- src/hal/adc_dma.cpp | 6 +----- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index b2c1c19..817a028 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -9,11 +9,10 @@ #ifndef __HAL_ADC_DMA_HPP__ #define __HAL_ADC_DMA_HPP__ +#include #include #include -#include "adc.h" - namespace hal { /** * @brief Class to handle ADC DMA peripheral on STM32 microcontrollers @@ -33,7 +32,7 @@ class AdcDma { * * @param adc_config ADC DMA configuration struct */ - AdcDma(Config& adc_config); + AdcDma(const Config& adc_config); /** * @brief Enable ADC, start conversion of regular group and transfer result through DMA diff --git a/src/hal/adc_dma.cpp b/src/hal/adc_dma.cpp index 9659a77..c82886b 100644 --- a/src/hal/adc_dma.cpp +++ b/src/hal/adc_dma.cpp @@ -10,11 +10,7 @@ #include "hal/adc_dma.hpp" namespace hal { -/***************************************** - * Public Methods Bodies Definitions. - *****************************************/ - -AdcDma::AdcDma(Config& adc_config) : handle(adc_config.handle) { +AdcDma::AdcDma(const Config& adc_config) : handle{adc_config.handle} { MX_DMA_Init(); adc_config.init_function(); } From 7e5613d04b2709dd0c5ef0263d97ea6b9b2976ff Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:43:43 -0300 Subject: [PATCH 030/561] =?UTF-8?q?=F0=9F=8E=A8=20Update=20include=20state?= =?UTF-8?q?ment=20in=20gpio.hpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/hal/gpio.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index 21299f3..2eaaa7a 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -10,8 +10,7 @@ #define __GPIO_HPP__ #include - -#include "gpio.h" +#include namespace hal { /** From 64cd36a76ab2209a1a4aeba9617b27d87acc28df Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:44:08 -0300 Subject: [PATCH 031/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Refactor=20Pwm=20c?= =?UTF-8?q?onstructor=20to=20use=20const=20Config&=20parameter?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/hal/pwm.hpp | 5 ++--- src/hal/pwm.cpp | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/inc/hal/pwm.hpp b/inc/hal/pwm.hpp index 93c5882..8721f98 100644 --- a/inc/hal/pwm.hpp +++ b/inc/hal/pwm.hpp @@ -10,7 +10,6 @@ #define __PWM_HPP__ #include -#include #include "hal/timer.hpp" @@ -33,7 +32,7 @@ class Pwm { * * @param pwm_config Configuration for the PWM */ - Pwm(Config& pwm_config); + Pwm(const Config& pwm_config); /** * @brief Set the PWM duty cycle @@ -45,7 +44,7 @@ class Pwm { /** * @brief Set the PWM frequency * - * @param frequency Frequency value + * @param frequency Frequency value in Hz */ void set_frequency(uint32_t frequency); diff --git a/src/hal/pwm.cpp b/src/hal/pwm.cpp index bb25432..7b16435 100644 --- a/src/hal/pwm.cpp +++ b/src/hal/pwm.cpp @@ -9,7 +9,7 @@ #include "hal/pwm.hpp" namespace hal { -Pwm::Pwm(Config& pwm_config) : timer{pwm_config.timer}, channel{pwm_config.timer_channel} { +Pwm::Pwm(const Config& pwm_config) : timer{pwm_config.timer}, channel{pwm_config.timer_channel} { this->timer.pwm_start(this->channel); this->timer.set_compare(this->channel, 0); } From 71964ef4c7c3972129f48b8a171f7170c9b2f9ac Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:44:35 -0300 Subject: [PATCH 032/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20timer?= =?UTF-8?q?=20initialization=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/hal/timer.hpp | 5 ++--- src/hal/timer.cpp | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index d6ca32c..0047979 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -11,8 +11,7 @@ #include #include - -#include "tim.h" +#include namespace hal { /** @@ -33,7 +32,7 @@ class Timer { * * @param timer_config Configuration for timer handler */ - Timer(Config& timer_config); + Timer(const Config& timer_config); /** * @brief Initialize timer base generation diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index 7e1a228..77b6a11 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -9,7 +9,7 @@ #include "hal/timer.hpp" namespace hal { -Timer::Timer(Config& tim_config) : handle(tim_config.handle) { +Timer::Timer(const Config& tim_config) : handle{tim_config.handle} { tim_config.init_function(); } From a8bb4a82121b15853a442922d987f3bf8af4e970 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:45:01 -0300 Subject: [PATCH 033/561] =?UTF-8?q?=E2=9C=A8=20Add=20MotorDriver=20class?= =?UTF-8?q?=20and=20implementation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/proxy/motor_driver.hpp | 94 ++++++++++++++++++++++++++++++++++++++ src/proxy/motor_driver.cpp | 47 +++++++++++++++++++ 2 files changed, 141 insertions(+) create mode 100644 inc/proxy/motor_driver.hpp create mode 100644 src/proxy/motor_driver.cpp diff --git a/inc/proxy/motor_driver.hpp b/inc/proxy/motor_driver.hpp new file mode 100644 index 0000000..227f345 --- /dev/null +++ b/inc/proxy/motor_driver.hpp @@ -0,0 +1,94 @@ +/** + * @file motor_driver.hpp + * + * @brief Proxy MotorDriver class declaration + * + * @date 03/2024 + */ + +#ifndef __MOTOR_DRIVER_HPP__ +#define __MOTOR_DRIVER_HPP__ + +#include + +#include "hal/pwm.hpp" +#include "hal/gpio.hpp" + +namespace proxy { +/** + * @brief Class for controlling a motor driver + */ +class MotorDriver { + public: + /** + * @brief Configuration structure for the motor driver + */ + struct Config { + hal::Pwm::Config pwm; + hal::Gpio::Config direction_gpio; + hal::Gpio::Config enable_gpio; + }; + + /** + * @brief Enum for rotation direction + */ + enum RotationDirection { + FORWARD, + BACKWARD + }; + + /** + * @brief Construct a new Motor Driver object + * + * @param motor_driver_config + */ + MotorDriver(const Config& motor_driver_config); + + /** + * @brief Enable the motor driver + */ + void enable(); + + /** + * @brief Disable the motor driver + */ + void disable(); + + /** + * @brief Set the speed of the motors + * + * @param speed Speed percentage of the motor + */ + void set_speed(float speed); + + /** + * @brief Stop the motors + */ + void stop(); + + private: + /** + * @brief Set the rotation direction of the motor + * + * @param direction Rotation direction + */ + void set_direction(RotationDirection direction); + + /** + * @brief PWM object for controlling the motor speed + */ + hal::Pwm pwm; + + /** + * @brief GPIO object for controlling the motor rotation direction + */ + hal::Gpio direction_gpio; + + /** + * @brief GPIO handle for the motor driver enable pin + */ + hal::Gpio enable_gpio; +}; +} // namespace proxy + +#endif // __MOTOR_DRIVER_HPP__ diff --git a/src/proxy/motor_driver.cpp b/src/proxy/motor_driver.cpp new file mode 100644 index 0000000..f6eb285 --- /dev/null +++ b/src/proxy/motor_driver.cpp @@ -0,0 +1,47 @@ +/** + * @file motor_driver.cpp + * + * @brief Proxy MotorDriver class source + * + * @date 03/2024 + */ + +#include "proxy/motor_driver.hpp" + +namespace proxy { +MotorDriver::MotorDriver(const Config& motor_driver_config) : + pwm{motor_driver_config.pwm}, + direction_gpio{motor_driver_config.direction_gpio}, + enable_gpio{motor_driver_config.enable_gpio} { + this->enable(); + this->stop(); +} + +void MotorDriver::enable() { + this->enable_gpio.write(true); +} + +void MotorDriver::disable() { + this->enable_gpio.write(false); +} + +void MotorDriver::set_speed(float speed) { + if (speed > 0.0f) { + this->pwm.set_duty_cycle(speed); + this->set_direction(RotationDirection::FORWARD); + } else if (speed < 0.0f) { + this->pwm.set_duty_cycle(-speed); + this->set_direction(RotationDirection::BACKWARD); + } else { + this->stop(); + } +} + +void MotorDriver::stop() { + this->set_speed(0.0f); +} + +void MotorDriver::set_direction(RotationDirection direction) { + this->direction_gpio.write((bool) direction); +} +} // namespace proxy From 74955296d56ee8c7ba93b4a7bd0b98a370d382b3 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:45:11 -0300 Subject: [PATCH 034/561] =?UTF-8?q?=E2=9C=A8=20Add=20Buzzer=20class=20for?= =?UTF-8?q?=20controlling=20a=20buzzer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/proxy/buzzer.hpp | 77 ++++++++++++++++++++++++++++++++++++++++++++ src/proxy/buzzer.cpp | 38 ++++++++++++++++++++++ 2 files changed, 115 insertions(+) create mode 100644 inc/proxy/buzzer.hpp create mode 100644 src/proxy/buzzer.cpp diff --git a/inc/proxy/buzzer.hpp b/inc/proxy/buzzer.hpp new file mode 100644 index 0000000..d191b77 --- /dev/null +++ b/inc/proxy/buzzer.hpp @@ -0,0 +1,77 @@ +/** + * @file buzzer.hpp + * + * @brief Proxy Buzzer class declaration + * + * @date 03/2024 + */ + +#ifndef __BUZZER_HPP__ +#define __BUZZER_HPP__ + +#include + +#include "hal/pwm.hpp" + +namespace proxy { +/** + * @brief Class for controlling a buzzer + */ +class Buzzer { + public: + /** + * @brief Configuration structure for the buzzer + */ + struct Config { + hal::Pwm::Config pwm; + }; + + /** + * @brief Constructor for the Buzzer class + * + * @param buzzer_config Configuration for the buzzer + */ + Buzzer(const Config& buzzer_config); + + /** + * @brief Play a tone for a duration + * + * @param frequency Buzzer sound frequency in Hz + * @param duration Duration of the sound in ms + */ + void play(uint32_t frequency, uint32_t duration = 0); + + /** + * @brief Update the buzzer state + */ + void update(); + + /** + * @brief Stop the buzzer sound + */ + void stop(); + + private: + /** + * @brief PWM object + */ + hal::Pwm pwm; + + /** + * @brief Timer to play the sound + */ + uint32_t timer = 0; + + /** + * @brief Flag to check if the buzzer is playing + */ + bool is_playing = false; + + /** + * @brief Duration of the sound + */ + uint32_t duration = 0; +}; +} // namespace proxy + +#endif // __BUZZER_HPP__ diff --git a/src/proxy/buzzer.cpp b/src/proxy/buzzer.cpp new file mode 100644 index 0000000..ec03eb3 --- /dev/null +++ b/src/proxy/buzzer.cpp @@ -0,0 +1,38 @@ +/** + * @file buzzer.cpp + * + * @brief Proxy Buzzer class implementation + * + * @date 03/2024 + */ + +#include "hal/mcu.hpp" +#include "proxy/buzzer.hpp" + +namespace proxy { +Buzzer::Buzzer(const Config& buzzer_config) : pwm{buzzer_config.pwm} { + this->stop(); +} + +void Buzzer::play(uint32_t frequency, uint32_t duration) { + this->pwm.set_duty_cycle(50.0f); + this->pwm.set_frequency(frequency); + this->is_playing = true; + + if (duration > 0) { + this->duration = duration; + hal::mcu::reset_timer(this->timer); + } +} + +void Buzzer::update() { + if (this->is_playing and hal::mcu::get_timer_ms(this->timer) > this->duration) { + this->stop(); + } +} + +void Buzzer::stop() { + this->is_playing = false; + this->pwm.set_duty_cycle(0.0f); +} +} // namespace proxy From a983692d24796018fcd9567f69f4b8626b6d00e1 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:45:22 -0300 Subject: [PATCH 035/561] =?UTF-8?q?=E2=9C=A8=20Add=20DualMotorDriver=20cla?= =?UTF-8?q?ss=20to=20control=20a=20dual=20motor=20driver?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/proxy/dual_motor_driver.hpp | 102 ++++++++++++++++++++++++++++++++ src/proxy/dual_motor_driver.cpp | 66 +++++++++++++++++++++ 2 files changed, 168 insertions(+) create mode 100644 inc/proxy/dual_motor_driver.hpp create mode 100644 src/proxy/dual_motor_driver.cpp diff --git a/inc/proxy/dual_motor_driver.hpp b/inc/proxy/dual_motor_driver.hpp new file mode 100644 index 0000000..239e97b --- /dev/null +++ b/inc/proxy/dual_motor_driver.hpp @@ -0,0 +1,102 @@ +/** + * @file dual_motor_driver.hpp + * + * @brief Proxy DualMotorDriver class declaration + * + * @date 03/2024 + */ + +#ifndef __DUAL_MOTOR_DRIVER_HPP__ +#define __DUAL_MOTOR_DRIVER_HPP__ + +#include + +#include "hal/pwm.hpp" +#include "hal/gpio.hpp" + +namespace proxy { +/** + * @brief Class for controlling a dual motor driver + */ +class DualMotorDriver { + public: + /** + * @brief Configuration structure for the dual motor driver + */ + struct Config { + hal::Pwm::Config pwm_left_fwd; + hal::Pwm::Config pwm_left_bwd; + hal::Pwm::Config pwm_right_fwd; + hal::Pwm::Config pwm_right_bwd; + hal::Gpio::Config enable_gpio; + }; + + /** + * @brief Construct a new Dual Motor Driver object + * + * @param dual_motor_driver_config + */ + DualMotorDriver(const Config& dual_motor_driver_config); + + /** + * @brief Enable the motor driver + */ + void enable(); + + /** + * @brief Disable the motor driver + */ + void disable(); + + /** + * @brief Set the speed of the motors + * + * @param left_speed Speed of the left motor + * @param right_speed Speed of the right motor + */ + void set_speed(float left_speed, float right_speed); + + /** + * @brief Stop the motors + */ + void stop(); + + private: + /** + * @brief Stop the left motor + */ + void stop_left_motor(); + + /** + * @brief Stop the right motor + */ + void stop_right_motor(); + + /** + * @brief PWM handle for the left motor forward + */ + hal::Pwm pwm_left_fwd; + + /** + * @brief PWM handle for the left motor backward + */ + hal::Pwm pwm_left_bwd; + + /** + * @brief PWM handle for the right motor forward + */ + hal::Pwm pwm_right_fwd; + + /** + * @brief PWM handle for the right motor backward + */ + hal::Pwm pwm_right_bwd; + + /** + * @brief GPIO handle for the motor driver enable pin + */ + hal::Gpio enable_gpio; +}; +} // namespace proxy + +#endif // __DUAL_MOTOR_DRIVER_HPP__ diff --git a/src/proxy/dual_motor_driver.cpp b/src/proxy/dual_motor_driver.cpp new file mode 100644 index 0000000..bb802f9 --- /dev/null +++ b/src/proxy/dual_motor_driver.cpp @@ -0,0 +1,66 @@ +/** + * @file dual_motor_driver.cpp + * + * @brief Proxy DualMotorDriver class source + * + * @date 03/2024 + */ + +#include "proxy/dual_motor_driver.hpp" + +namespace proxy { +DualMotorDriver::DualMotorDriver(const Config& motor_driver_config) : + pwm_left_fwd{motor_driver_config.pwm_left_fwd}, + pwm_left_bwd{motor_driver_config.pwm_left_bwd}, + pwm_right_fwd{motor_driver_config.pwm_right_fwd}, + pwm_right_bwd{motor_driver_config.pwm_right_bwd}, + enable_gpio{motor_driver_config.enable_gpio} { + this->enable(); + this->stop(); +} + +void DualMotorDriver::enable() { + this->enable_gpio.write(true); +} + +void DualMotorDriver::disable() { + this->enable_gpio.write(false); +} + +void DualMotorDriver::set_speed(float left_speed, float right_speed) { + if (left_speed > 0.0f) { + this->pwm_left_fwd.set_duty_cycle(left_speed); + this->pwm_left_bwd.set_duty_cycle(0.0f); + } else if (left_speed < 0.0f) { + this->pwm_left_fwd.set_duty_cycle(0.0f); + this->pwm_left_bwd.set_duty_cycle(-left_speed); + } else { + this->stop_left_motor(); + } + + if (right_speed > 0.0f) { + this->pwm_right_fwd.set_duty_cycle(right_speed); + this->pwm_right_bwd.set_duty_cycle(0.0f); + } else if (right_speed < 0.0f) { + this->pwm_right_fwd.set_duty_cycle(0.0f); + this->pwm_right_bwd.set_duty_cycle(-right_speed); + } else { + this->stop_right_motor(); + } +} + +void DualMotorDriver::stop() { + this->stop_left_motor(); + this->stop_right_motor(); +} + +void DualMotorDriver::stop_left_motor() { + this->pwm_left_fwd.set_duty_cycle(0.0f); + this->pwm_left_bwd.set_duty_cycle(0.0f); +} + +void DualMotorDriver::stop_right_motor() { + this->pwm_right_fwd.set_duty_cycle(0.0f); + this->pwm_right_bwd.set_duty_cycle(0.0f); +} +} // namespace proxy From 6be8139bfbc8dd3c87ef340bb266717bbb814e37 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:45:37 -0300 Subject: [PATCH 036/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Update=20DistanceS?= =?UTF-8?q?ensors=20class=20to=20add=20const=20qualifiers=20to=20get=5Fdis?= =?UTF-8?q?tance=20and=20get=5Fdistance=5Fraw=20methods?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/proxy/distance_sensors.hpp | 4 ++-- src/proxy/distance_sensors.cpp | 7 +++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index 6aa591a..a25ec3a 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -49,7 +49,7 @@ class DistanceSensors { * @param sensor_index Index of the sensor * @return float Distance reading from the sensors */ - float get_distance(uint8_t sensor_index); + float get_distance(uint8_t sensor_index) const; /** * @brief Get the distance from a sensor @@ -57,7 +57,7 @@ class DistanceSensors { * @param sensor_index Index of the sensor * @return uint32_t Raw reading from the distance sensor */ - uint32_t get_distance_raw(uint8_t sensor_index); + uint32_t get_distance_raw(uint8_t sensor_index) const; private: /** diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index b84aa6f..7544449 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -15,8 +15,7 @@ static constexpr uint32_t max_adc_reading{4095}; template DistanceSensors::DistanceSensors(const Config& distance_sensor_config) : - distance_sensor_adc{distance_sensor_config.sensor_adc_config}, - infrared_pwm{distance_sensor_config.infrared_pwm} { + distance_sensor_adc{distance_sensor_config.sensor_adc_config}, infrared_pwm{distance_sensor_config.infrared_pwm} { this->distance_sensor_adc.start_dma(this->adc_buffer.data(), num_of_sensors); this->infrared_pwm.set_duty_cycle(100.0f); } @@ -27,12 +26,12 @@ void DistanceSensors::set_infrared_led_intensity(float intensity } template -float DistanceSensors::get_distance(uint8_t sensor_index) { +float DistanceSensors::get_distance(uint8_t sensor_index) const { return max_distance * this->adc_buffer.at(sensor_index) / max_adc_reading; } template -uint32_t DistanceSensors::get_distance_raw(uint8_t sensor_index) { +uint32_t DistanceSensors::get_distance_raw(uint8_t sensor_index) const { return this->adc_buffer.at(sensor_index); } } // namespace proxy From 629155e2c2c67994c1f0bd41dde6ad014266f36f Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:45:48 -0300 Subject: [PATCH 037/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Add=20const=20qual?= =?UTF-8?q?ifier=20to=20get=5Fcurrent=20and=20get=5Fcurrent=5Fraw=20method?= =?UTF-8?q?s?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/proxy/current_sensors.hpp | 5 +++-- src/proxy/current_sensors.cpp | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index cfdaf38..96643b0 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -9,6 +9,7 @@ #ifndef __CURRENT_SENSORS_HPP__ #define __CURRENT_SENSORS_HPP__ +#include #include #include "hal/adc_dma.hpp" @@ -41,7 +42,7 @@ class CurrentSensors { * @param sensor_index Index of the sensor * @return float Current reading from the sensor */ - float get_current(uint8_t sensor_index); + float get_current(uint8_t sensor_index) const; /** * @brief Get the raw reading from the current sensor @@ -49,7 +50,7 @@ class CurrentSensors { * @param sensor_index Index of the sensor * @return uint16_t Current reading from the sensor */ - uint32_t get_current_raw(uint8_t sensor_index); + uint32_t get_current_raw(uint8_t sensor_index) const; private: /** diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp index 6a43cec..1b50c33 100644 --- a/src/proxy/current_sensors.cpp +++ b/src/proxy/current_sensors.cpp @@ -15,18 +15,18 @@ static constexpr uint32_t max_adc_reading{4095}; template CurrentSensors::CurrentSensors(const Config& current_sensors_config) : - adc(current_sensors_config.adc_config), shunt_resistor(current_sensors_config.shunt_resistor) { + adc{current_sensors_config.adc_config}, shunt_resistor{current_sensors_config.shunt_resistor} { this->adc.start_dma(this->adc_buffer.data(), num_of_sensors); this->adc.start_dma(this->adc_buffer.data(), num_of_sensors); } template -float CurrentSensors::get_current(uint8_t sensor_index) { +float CurrentSensors::get_current(uint8_t sensor_index) const { return reference_voltage * this->adc_buffer.at(sensor_index) / (max_adc_reading * this->shunt_resistor); } template -uint32_t CurrentSensors::get_current_raw(uint8_t sensor_index) { +uint32_t CurrentSensors::get_current_raw(uint8_t sensor_index) const { return this->adc_buffer.at(sensor_index); } } // namespace proxy From 47f857585a2109bab99dded911fd3b2e08261897 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:46:00 -0300 Subject: [PATCH 038/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20DipSwit?= =?UTF-8?q?ch=20class=20to=20use=20const=20member=20functions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/proxy/dip_switch.hpp | 29 ++++++++++++++++++++++++++--- src/proxy/dip_switch.cpp | 6 +++--- 2 files changed, 29 insertions(+), 6 deletions(-) diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp index f00b890..2b4c2bb 100644 --- a/inc/proxy/dip_switch.hpp +++ b/inc/proxy/dip_switch.hpp @@ -21,17 +21,40 @@ namespace proxy { template class DipSwitch { public: + /** + * @brief Configuration struct for DipSwitch + */ struct Config { std::array gpio_array; }; + /** + * @brief Construct a new Dip Switch object + * + * @param dip_switch_config Configuration struct for DipSwitch + */ DipSwitch(const Config& dip_switch_config); - bool get_switch_state(uint8_t switch_index); - - uint8_t get_switches_value(); + /** + * @brief Get the state of a switch + * + * @param switch_index Index of the switch + * @return true if the switch is on + * @return false if the switch is off + */ + bool get_switch_state(uint8_t switch_index) const; + + /** + * @brief Get the value of all switches + * + * @return uint8_t Value of all switches + */ + uint8_t get_switches_value() const; private: + /** + * @brief Array of GPIOs for the switches + */ std::array gpio_array; }; } // namespace proxy diff --git a/src/proxy/dip_switch.cpp b/src/proxy/dip_switch.cpp index 57e8ffd..09f3066 100644 --- a/src/proxy/dip_switch.cpp +++ b/src/proxy/dip_switch.cpp @@ -13,17 +13,17 @@ namespace proxy { template DipSwitch::DipSwitch(const Config& dip_switch_config) { for (uint8_t i = 0; i < num_of_sensors; i++) { - gpio_array[i] = dip_switch_config.gpio_array[i]; + this->gpio_array[i] = dip_switch_config.gpio_array[i]; } } template -bool DipSwitch::get_switch_state(uint8_t switch_index) { +bool DipSwitch::get_switch_state(uint8_t switch_index) const { return gpio_array.at(switch_index).read(); } template -uint8_t DipSwitch::get_switches_value() { +uint8_t DipSwitch::get_switches_value() const { uint8_t switches_value = 0; for (uint8_t i = 0; i < num_of_sensors; i++) { From 5d946bd04f8c575bef954fd30c664f648dfecfd7 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:47:12 -0300 Subject: [PATCH 039/561] =?UTF-8?q?=F0=9F=94=A8=20Update=20MCU=20initializ?= =?UTF-8?q?ation=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- src/hal/mcu.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index 3db9684..b9a2cf5 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -4,17 +4,11 @@ * @brief MCU related */ -#include +#include +#include #include "hal/mcu.hpp" -#include "gpio.h" -#include "main.h" - -/***************************************** - * Public Function Body Definitions - *****************************************/ - namespace hal { void mcu::init(void) { HAL_Init(); From ff39c7e9d2c57910cbd5f582b4f3a3a2c198a08d Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:47:39 -0300 Subject: [PATCH 040/561] =?UTF-8?q?=E2=9C=A8=20=F0=9F=9A=A7=20Add=20SPI=20?= =?UTF-8?q?HAL=20wrapper=20for=20STM32=20microcontrollers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/hal/spi.hpp | 80 +++++++++++++++++++++++++++++++++++++++++++++++++ src/hal/spi.cpp | 39 ++++++++++++++++++++++++ 2 files changed, 119 insertions(+) create mode 100644 inc/hal/spi.hpp create mode 100644 src/hal/spi.cpp diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp new file mode 100644 index 0000000..f22c05b --- /dev/null +++ b/inc/hal/spi.hpp @@ -0,0 +1,80 @@ +/** + * @file spi.hpp + * + * @brief STM32 SPI HAL wrapper + * + * @date 03/2024 + */ + +#ifndef __SPI_HPP__ +#define __SPI_HPP__ + +#include +#include +#include + +#include "hal/gpio.hpp" + +namespace hal { +/** + * @brief Class to handle SPI peripheral on STM32 microcontrollers + */ +template +class Spi { + public: + /** + * @brief SPI configuration struct + */ + struct Config { + SPI_HandleTypeDef* handle; + std::function init_function; + std::array gpio_array; + }; + + /** + * @brief Construct a new Spi object + * + * @param spi_config Configuration for the SPI + */ + Spi(const Config& spi_config); + + /** + * @brief Activate the chip select for a device + */ + void select_device(uint8_t device); + + /** + * @brief Deactivate the chip select for a device + */ + void unselect_device(uint8_t device); + + /** + * @brief Transmit data over SPI + * + * @param data Data to transmit + * @param size Size of the buffer + */ + void transmit(uint8_t data[], uint32_t size); + + /** + * @brief Receive data over SPI + * + * @param data Data to receive data + * @param size Size of the data + */ + void receive(uint8_t data[], uint32_t size); + + private: + /** + * @brief Handle for the SPI + */ + SPI_HandleTypeDef* handle; + + /** + * @brief Array of GPIOs for the switches + */ + std::array gpio_array; +}; +} // namespace hal + +#endif // __SPI_HPP__ diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp new file mode 100644 index 0000000..5667b2b --- /dev/null +++ b/src/hal/spi.cpp @@ -0,0 +1,39 @@ +/** + * @file spi.cpp + * + * @brief Proxy SPI Switch class source + * + * @date 03/2024 + */ + +#include "hal/mcu.hpp" +#include "hal/spi.hpp" + +namespace hal { +template +Spi::Spi(const Config& spi_config) : handle{spi_config.handle} { + for (uint8_t i = 0; i < num_of_devices; i++) { + this->gpio_array[i] = spi_config.gpio_array[i]; + } +} + +template +void Spi::select_device(uint8_t device) { + this->gpio_array[device].write(false); +} + +template +void Spi::unselect_device(uint8_t device) { + this->gpio_array[device].write(true); +} + +template +void Spi::transmit(uint8_t data[], uint32_t size) { + HAL_SPI_Transmit_DMA(this->handle, data, size); +} + +template +void Spi::receive(uint8_t data[], uint32_t size) { + HAL_SPI_Receive_DMA(this->handle, data, size); +} +} // namespace proxy From 321ee334cdf09e2ae6cddee6caec10ec76b7d87b Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Thu, 21 Mar 2024 01:47:58 -0300 Subject: [PATCH 041/561] =?UTF-8?q?=F0=9F=9A=A7=20=E2=9C=A8=20Add=20IMU=20?= =?UTF-8?q?HAL=20wrapper=20for=20STM32=20microcontrollers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Gabriel Cosme Barbosa --- inc/proxy/imu.hpp | 44 ++++++++++++++++++++++++++++++++++++++++++++ src/proxy/imu.cpp | 16 ++++++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 inc/proxy/imu.hpp create mode 100644 src/proxy/imu.cpp diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp new file mode 100644 index 0000000..cc88f42 --- /dev/null +++ b/inc/proxy/imu.hpp @@ -0,0 +1,44 @@ +/** + * @file imu.hpp + * + * @brief STM32 IMU HAL wrapper + * + * @date 03/2024 + */ + +#ifndef __IMU_HPP__ +#define __IMU_HPP__ + +#include + +#include "hal/spi.hpp" + +namespace proxy { +/** + * @brief Class to handle IMU peripheral on STM32 microcontrollers + */ +class Imu { + public: + /** + * @brief IMU configuration struct + */ + struct Config { + hal::Spi<3>::Config spi; // nossa nossa nossa nossa, que tristeza T.T + }; + + /** + * @brief Construct a new Imu object + * + * @param imu_config Configuration for the IMU + */ + Imu(const Config& imu_config); + + private: + /** + * @brief SPI for the IMU communication + */ + hal::Spi<3> spi; // nossa nossa nossa nossa, que tristeza T.T +}; +} // namespace proxy + +#endif // __IMU_HPP__ diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp new file mode 100644 index 0000000..be1fc7c --- /dev/null +++ b/src/proxy/imu.cpp @@ -0,0 +1,16 @@ +/** + * @file imu.cpp + * + * @brief Proxy Imu class source + * + * @date 03/2024 + */ + +#include + +#include "proxy/imu.hpp" + +namespace proxy { +Imu::Imu(const Config& imu_config) : spi{imu_config.spi} { +} +} // namespace proxy From 7138680bafeb3dd241d3b4ae738ffe466532aeab Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 03:17:34 -0300 Subject: [PATCH 042/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20imu=20and=20spi=20?= =?UTF-8?q?implementation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/spi.hpp | 19 +++++++++---------- inc/proxy/imu.hpp | 12 +++++++++--- src/hal/spi.cpp | 23 +++++++---------------- src/proxy/imu.cpp | 29 ++++++++++++++++++++++++++--- 4 files changed, 51 insertions(+), 32 deletions(-) diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index f22c05b..49d5632 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -19,16 +19,15 @@ namespace hal { /** * @brief Class to handle SPI peripheral on STM32 microcontrollers */ -template class Spi { public: /** * @brief SPI configuration struct */ struct Config { - SPI_HandleTypeDef* handle; - std::function init_function; - std::array gpio_array; + SPI_HandleTypeDef* handle; + void (* init_function)(void); + hal::Gpio::Config gpio; }; /** @@ -36,17 +35,17 @@ class Spi { * * @param spi_config Configuration for the SPI */ - Spi(const Config& spi_config); + Spi(Config& spi_config); /** - * @brief Activate the chip select for a device + * @brief Activate the chip select */ - void select_device(uint8_t device); + void select_device(); /** - * @brief Deactivate the chip select for a device + * @brief Deactivate the chip select */ - void unselect_device(uint8_t device); + void unselect_device(); /** * @brief Transmit data over SPI @@ -73,7 +72,7 @@ class Spi { /** * @brief Array of GPIOs for the switches */ - std::array gpio_array; + hal::Gpio gpio; }; } // namespace hal diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index cc88f42..041e0dc 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -10,6 +10,7 @@ #define __IMU_HPP__ #include +#include #include "hal/spi.hpp" @@ -23,7 +24,7 @@ class Imu { * @brief IMU configuration struct */ struct Config { - hal::Spi<3>::Config spi; // nossa nossa nossa nossa, que tristeza T.T + hal::Spi::Config spi; }; /** @@ -31,13 +32,18 @@ class Imu { * * @param imu_config Configuration for the IMU */ - Imu(const Config& imu_config); + Imu(Config& imu_config); private: /** * @brief SPI for the IMU communication */ - hal::Spi<3> spi; // nossa nossa nossa nossa, que tristeza T.T + hal::Spi spi; + + /** + * @brief Device context for the IMU library + */ + stmdev_ctx_t dev_ctx; }; } // namespace proxy diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index 5667b2b..83948e8 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -6,34 +6,25 @@ * @date 03/2024 */ -#include "hal/mcu.hpp" #include "hal/spi.hpp" namespace hal { -template -Spi::Spi(const Config& spi_config) : handle{spi_config.handle} { - for (uint8_t i = 0; i < num_of_devices; i++) { - this->gpio_array[i] = spi_config.gpio_array[i]; - } +Spi::Spi(Config& spi_config) : handle{spi_config.handle}, gpio{spi_config.gpio} { } -template -void Spi::select_device(uint8_t device) { - this->gpio_array[device].write(false); +void Spi::select_device() { + this->gpio.write(false); } -template -void Spi::unselect_device(uint8_t device) { - this->gpio_array[device].write(true); +void Spi::unselect_device() { + this->gpio.write(true); } -template -void Spi::transmit(uint8_t data[], uint32_t size) { +void Spi::transmit(uint8_t data[], uint32_t size) { HAL_SPI_Transmit_DMA(this->handle, data, size); } -template -void Spi::receive(uint8_t data[], uint32_t size) { +void Spi::receive(uint8_t data[], uint32_t size) { HAL_SPI_Receive_DMA(this->handle, data, size); } } // namespace proxy diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index be1fc7c..27d5986 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -6,11 +6,34 @@ * @date 03/2024 */ -#include - #include "proxy/imu.hpp" namespace proxy { -Imu::Imu(const Config& imu_config) : spi{imu_config.spi} { +Imu::Imu(Config& imu_config) : spi{imu_config.spi} { + this->dev_ctx.write_reg = + [](void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len) -> int32_t { + auto spi = static_cast(handle); + + spi->select_device(); + spi->transmit(®, 1); + spi->transmit((uint8_t*) bufp, len); + spi->unselect_device(); + + return 0; + }; + + this->dev_ctx.read_reg = + [](void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) -> int32_t { + auto spi = static_cast(handle); + + spi->select_device(); + spi->transmit(®, 1); + spi->receive(bufp, len); + spi->unselect_device(); + + return 0; + }; + + this->dev_ctx.handle = &imu_config.spi; } } // namespace proxy From 63bd3354bd1bec80dd0c6b055d15379f351f2679 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 03:18:25 -0300 Subject: [PATCH 043/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20const=20from=20?= =?UTF-8?q?config=20in=20constructors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 10 +++++----- inc/hal/gpio.hpp | 2 +- inc/hal/pwm.hpp | 2 +- inc/hal/timer.hpp | 6 +++--- inc/proxy/button.hpp | 2 +- inc/proxy/buzzer.hpp | 2 +- inc/proxy/current_sensors.hpp | 2 +- inc/proxy/dip_switch.hpp | 2 +- inc/proxy/distance_sensors.hpp | 2 +- inc/proxy/dual_motor_driver.hpp | 2 +- inc/proxy/led.hpp | 2 +- inc/proxy/motor_driver.hpp | 2 +- src/hal/adc_dma.cpp | 2 +- src/hal/gpio.cpp | 2 +- src/hal/pwm.cpp | 2 +- src/hal/timer.cpp | 2 +- src/proxy/button.cpp | 2 +- src/proxy/buzzer.cpp | 2 +- src/proxy/current_sensors.cpp | 2 +- src/proxy/dip_switch.cpp | 2 +- src/proxy/distance_sensors.cpp | 2 +- src/proxy/dual_motor_driver.cpp | 2 +- src/proxy/led.cpp | 2 +- src/proxy/motor_driver.cpp | 2 +- 24 files changed, 30 insertions(+), 30 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index 817a028..d062f67 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -1,7 +1,7 @@ /** * @file adc_dma.hpp * - * @brief STM32 ADC DMA HAL wrapper + * @brief ADC DMA HAL header * * @date 03/2024 */ @@ -20,11 +20,11 @@ namespace hal { class AdcDma { public: /** - * @brief ADC DMA configuration struct + * @brief Configuration structure for ADC DMA */ struct Config { - ADC_HandleTypeDef* handle; - std::function init_function; + ADC_HandleTypeDef* handle; + void (* init_function)(void); }; /** @@ -32,7 +32,7 @@ class AdcDma { * * @param adc_config ADC DMA configuration struct */ - AdcDma(const Config& adc_config); + AdcDma(Config& adc_config); /** * @brief Enable ADC, start conversion of regular group and transfer result through DMA diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index 2eaaa7a..6cd34ad 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -31,7 +31,7 @@ class Gpio { * * @param gpio_config Configuration for the GPIO pin */ - Gpio(const Config& gpio_config); + Gpio(Config& gpio_config); /** * @brief Read the current state of the GPIO pin diff --git a/inc/hal/pwm.hpp b/inc/hal/pwm.hpp index 8721f98..1785eca 100644 --- a/inc/hal/pwm.hpp +++ b/inc/hal/pwm.hpp @@ -32,7 +32,7 @@ class Pwm { * * @param pwm_config Configuration for the PWM */ - Pwm(const Config& pwm_config); + Pwm(Config& pwm_config); /** * @brief Set the PWM duty cycle diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index 0047979..7b6a1bb 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -23,8 +23,8 @@ class Timer { * @brief Timer configuration struct */ struct Config { - TIM_HandleTypeDef* handle; - std::function init_function; + TIM_HandleTypeDef* handle; + void (* init_function)(void); }; /** @@ -32,7 +32,7 @@ class Timer { * * @param timer_config Configuration for timer handler */ - Timer(const Config& timer_config); + Timer(Config& timer_config); /** * @brief Initialize timer base generation diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index bb38244..dab7f64 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -53,7 +53,7 @@ class Button { * * @param button_config Button configuration */ - Button(const Config& button_config); + Button(Config& button_config); /** * @brief Check if button is pressed diff --git a/inc/proxy/buzzer.hpp b/inc/proxy/buzzer.hpp index d191b77..10fca10 100644 --- a/inc/proxy/buzzer.hpp +++ b/inc/proxy/buzzer.hpp @@ -31,7 +31,7 @@ class Buzzer { * * @param buzzer_config Configuration for the buzzer */ - Buzzer(const Config& buzzer_config); + Buzzer(Config& buzzer_config); /** * @brief Play a tone for a duration diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index 96643b0..13222de 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -34,7 +34,7 @@ class CurrentSensors { * * @param current_sensors_config Configuration for the current sensors */ - CurrentSensors(const Config& current_sensors_config); + CurrentSensors(Config& current_sensors_config); /** * @brief Get the current from the sensor diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp index 2b4c2bb..9d555e4 100644 --- a/inc/proxy/dip_switch.hpp +++ b/inc/proxy/dip_switch.hpp @@ -33,7 +33,7 @@ class DipSwitch { * * @param dip_switch_config Configuration struct for DipSwitch */ - DipSwitch(const Config& dip_switch_config); + DipSwitch(Config& dip_switch_config); /** * @brief Get the state of a switch diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index a25ec3a..f6bb1dc 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -34,7 +34,7 @@ class DistanceSensors { * * @param distance_sensor_config Configuration for the distance sensors */ - DistanceSensors(const Config& distance_sensor_config); + DistanceSensors(Config& distance_sensor_config); /** * @brief Set the infrared DistanceSensors intensity diff --git a/inc/proxy/dual_motor_driver.hpp b/inc/proxy/dual_motor_driver.hpp index 239e97b..3bf9475 100644 --- a/inc/proxy/dual_motor_driver.hpp +++ b/inc/proxy/dual_motor_driver.hpp @@ -36,7 +36,7 @@ class DualMotorDriver { * * @param dual_motor_driver_config */ - DualMotorDriver(const Config& dual_motor_driver_config); + DualMotorDriver(Config& dual_motor_driver_config); /** * @brief Enable the motor driver diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp index 2a3a0c3..543a62d 100644 --- a/inc/proxy/led.hpp +++ b/inc/proxy/led.hpp @@ -29,7 +29,7 @@ class Led { * * @param led_config Configuration for the LED */ - Led(const Config& led_config); + Led(Config& led_config); /** * @brief Turn the LED on diff --git a/inc/proxy/motor_driver.hpp b/inc/proxy/motor_driver.hpp index 227f345..84b529e 100644 --- a/inc/proxy/motor_driver.hpp +++ b/inc/proxy/motor_driver.hpp @@ -42,7 +42,7 @@ class MotorDriver { * * @param motor_driver_config */ - MotorDriver(const Config& motor_driver_config); + MotorDriver(Config& motor_driver_config); /** * @brief Enable the motor driver diff --git a/src/hal/adc_dma.cpp b/src/hal/adc_dma.cpp index c82886b..4b10241 100644 --- a/src/hal/adc_dma.cpp +++ b/src/hal/adc_dma.cpp @@ -10,7 +10,7 @@ #include "hal/adc_dma.hpp" namespace hal { -AdcDma::AdcDma(const Config& adc_config) : handle{adc_config.handle} { +AdcDma::AdcDma(Config& adc_config) : handle{adc_config.handle} { MX_DMA_Init(); adc_config.init_function(); } diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index c6a0d3b..1963ba9 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -9,7 +9,7 @@ #include "hal/gpio.hpp" namespace hal { -Gpio::Gpio(const Config& gpio_config) : port{gpio_config.port}, pin{gpio_config.pin} { +Gpio::Gpio(Config& gpio_config) : port{gpio_config.port}, pin{gpio_config.pin} { } bool Gpio::read(void) const { diff --git a/src/hal/pwm.cpp b/src/hal/pwm.cpp index 7b16435..bb25432 100644 --- a/src/hal/pwm.cpp +++ b/src/hal/pwm.cpp @@ -9,7 +9,7 @@ #include "hal/pwm.hpp" namespace hal { -Pwm::Pwm(const Config& pwm_config) : timer{pwm_config.timer}, channel{pwm_config.timer_channel} { +Pwm::Pwm(Config& pwm_config) : timer{pwm_config.timer}, channel{pwm_config.timer_channel} { this->timer.pwm_start(this->channel); this->timer.set_compare(this->channel, 0); } diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index 77b6a11..088276a 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -9,7 +9,7 @@ #include "hal/timer.hpp" namespace hal { -Timer::Timer(const Config& tim_config) : handle{tim_config.handle} { +Timer::Timer(Config& tim_config) : handle{tim_config.handle} { tim_config.init_function(); } diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index 96d2253..a358381 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -10,7 +10,7 @@ #include "proxy/button.hpp" namespace proxy { -Button::Button(const Config& button_config) : +Button::Button(Config& button_config) : debounce_delay{button_config.debounce_delay}, long_press_delay{button_config.long_press_delay}, extra_long_press_delay{button_config.extra_long_press_delay}, diff --git a/src/proxy/buzzer.cpp b/src/proxy/buzzer.cpp index ec03eb3..286acdb 100644 --- a/src/proxy/buzzer.cpp +++ b/src/proxy/buzzer.cpp @@ -10,7 +10,7 @@ #include "proxy/buzzer.hpp" namespace proxy { -Buzzer::Buzzer(const Config& buzzer_config) : pwm{buzzer_config.pwm} { +Buzzer::Buzzer(Config& buzzer_config) : pwm{buzzer_config.pwm} { this->stop(); } diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp index 1b50c33..6e7c229 100644 --- a/src/proxy/current_sensors.cpp +++ b/src/proxy/current_sensors.cpp @@ -14,7 +14,7 @@ static constexpr float reference_voltage{3.3f}; static constexpr uint32_t max_adc_reading{4095}; template -CurrentSensors::CurrentSensors(const Config& current_sensors_config) : +CurrentSensors::CurrentSensors(Config& current_sensors_config) : adc{current_sensors_config.adc_config}, shunt_resistor{current_sensors_config.shunt_resistor} { this->adc.start_dma(this->adc_buffer.data(), num_of_sensors); this->adc.start_dma(this->adc_buffer.data(), num_of_sensors); diff --git a/src/proxy/dip_switch.cpp b/src/proxy/dip_switch.cpp index 09f3066..dd623d2 100644 --- a/src/proxy/dip_switch.cpp +++ b/src/proxy/dip_switch.cpp @@ -11,7 +11,7 @@ namespace proxy { template -DipSwitch::DipSwitch(const Config& dip_switch_config) { +DipSwitch::DipSwitch(Config& dip_switch_config) { for (uint8_t i = 0; i < num_of_sensors; i++) { this->gpio_array[i] = dip_switch_config.gpio_array[i]; } diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index 7544449..6eb2e21 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -14,7 +14,7 @@ static constexpr float max_distance{0.3f}; static constexpr uint32_t max_adc_reading{4095}; template -DistanceSensors::DistanceSensors(const Config& distance_sensor_config) : +DistanceSensors::DistanceSensors(Config& distance_sensor_config) : distance_sensor_adc{distance_sensor_config.sensor_adc_config}, infrared_pwm{distance_sensor_config.infrared_pwm} { this->distance_sensor_adc.start_dma(this->adc_buffer.data(), num_of_sensors); this->infrared_pwm.set_duty_cycle(100.0f); diff --git a/src/proxy/dual_motor_driver.cpp b/src/proxy/dual_motor_driver.cpp index bb802f9..41f82fe 100644 --- a/src/proxy/dual_motor_driver.cpp +++ b/src/proxy/dual_motor_driver.cpp @@ -9,7 +9,7 @@ #include "proxy/dual_motor_driver.hpp" namespace proxy { -DualMotorDriver::DualMotorDriver(const Config& motor_driver_config) : +DualMotorDriver::DualMotorDriver(Config& motor_driver_config) : pwm_left_fwd{motor_driver_config.pwm_left_fwd}, pwm_left_bwd{motor_driver_config.pwm_left_bwd}, pwm_right_fwd{motor_driver_config.pwm_right_fwd}, diff --git a/src/proxy/led.cpp b/src/proxy/led.cpp index 9820101..f1f6955 100644 --- a/src/proxy/led.cpp +++ b/src/proxy/led.cpp @@ -9,7 +9,7 @@ #include "proxy/led.hpp" namespace proxy { -Led::Led(const Config& led_config) : gpio{led_config.gpio} { +Led::Led(Config& led_config) : gpio{led_config.gpio} { } void Led::turn_on() { diff --git a/src/proxy/motor_driver.cpp b/src/proxy/motor_driver.cpp index f6eb285..c54eea7 100644 --- a/src/proxy/motor_driver.cpp +++ b/src/proxy/motor_driver.cpp @@ -9,7 +9,7 @@ #include "proxy/motor_driver.hpp" namespace proxy { -MotorDriver::MotorDriver(const Config& motor_driver_config) : +MotorDriver::MotorDriver(Config& motor_driver_config) : pwm{motor_driver_config.pwm}, direction_gpio{motor_driver_config.direction_gpio}, enable_gpio{motor_driver_config.enable_gpio} { From 4e513676bd52f3db378642fcbcd968dbba87eba7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 03:26:21 -0300 Subject: [PATCH 044/561] =?UTF-8?q?=F0=9F=9A=B8=20Change=20c-style=20cast?= =?UTF-8?q?=20to=20static=5Fcast?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/hal/gpio.cpp | 2 +- src/proxy/motor_driver.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index 1963ba9..747bbda 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -17,7 +17,7 @@ bool Gpio::read(void) const { } void Gpio::write(bool state) { - HAL_GPIO_WritePin(this->port, this->pin, (GPIO_PinState) state); + HAL_GPIO_WritePin(this->port, this->pin, static_cast(state)); } void Gpio::toggle(void) { diff --git a/src/proxy/motor_driver.cpp b/src/proxy/motor_driver.cpp index c54eea7..01a3d34 100644 --- a/src/proxy/motor_driver.cpp +++ b/src/proxy/motor_driver.cpp @@ -42,6 +42,6 @@ void MotorDriver::stop() { } void MotorDriver::set_direction(RotationDirection direction) { - this->direction_gpio.write((bool) direction); + this->direction_gpio.write(static_cast(direction)); } } // namespace proxy From 9868ad8e9591a6a14e252553fc8e96bab6cbe7b8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 04:36:04 -0300 Subject: [PATCH 045/561] =?UTF-8?q?=F0=9F=9A=A7=20Add=20imu=20functions=20?= =?UTF-8?q?for=20testing?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/imu.hpp | 38 +++++++++++++++++++++++++++ src/proxy/imu.cpp | 65 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 103 insertions(+) diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 041e0dc..1b0c540 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -34,6 +34,40 @@ class Imu { */ Imu(Config& imu_config); + enum Axis { + W, + X, + Y, + Z + }; + + /** + * @brief Get the IMU orientation over an axis + * + * @param axis Axis to get the orientation from + * + * @return Orientation over the desired axis using quaternions + */ + float get_orientation(Axis axis); + + /** + * @brief Get the IMU angular velocity over an axis + * + * @param axis Axis to get the angular velocity from + * + * @return Angular velocity over the desired axis in rad/s + */ + float get_angular_velocity(Axis axis); + + /** + * @brief Get the IMU linear acceleration over an axis + * + * @param axis Axis to get the linear acceleration from + * + * @return Linear acceleration over the desired axis in m/sยฒ + */ + float get_linear_acceleration(Axis axis); + private: /** * @brief SPI for the IMU communication @@ -44,6 +78,10 @@ class Imu { * @brief Device context for the IMU library */ stmdev_ctx_t dev_ctx; + + std::array angular_velocity; + + std::array linear_acceleration; }; } // namespace proxy diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 27d5986..01aa6af 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -6,6 +6,7 @@ * @date 03/2024 */ +#include "hal/mcu.hpp" #include "proxy/imu.hpp" namespace proxy { @@ -26,6 +27,7 @@ Imu::Imu(Config& imu_config) : spi{imu_config.spi} { [](void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) -> int32_t { auto spi = static_cast(handle); + reg |= 0x80; spi->select_device(); spi->transmit(®, 1); spi->receive(bufp, len); @@ -35,5 +37,68 @@ Imu::Imu(Config& imu_config) : spi{imu_config.spi} { }; this->dev_ctx.handle = &imu_config.spi; + + hal::mcu::sleep(10); + + lsm6dsv_reset_set(&(this->dev_ctx), LSM6DSV_RESTORE_CTRL_REGS); + + lsm6dsv_reset_t rst; + + do { + lsm6dsv_reset_get(&(this->dev_ctx), &rst); + } while (rst != LSM6DSV_READY); + + lsm6dsv_block_data_update_set(&(this->dev_ctx), PROPERTY_ENABLE); + + lsm6dsv_xl_data_rate_set(&(this->dev_ctx), LSM6DSV_ODR_HA02_AT_6400Hz); + lsm6dsv_gy_data_rate_set(&(this->dev_ctx), LSM6DSV_ODR_HA02_AT_6400Hz); + + lsm6dsv_xl_full_scale_set(&(this->dev_ctx), LSM6DSV_8g); + lsm6dsv_gy_full_scale_set(&(this->dev_ctx), LSM6DSV_4000dps); + + lsm6dsv_filt_gy_lp1_set(&(this->dev_ctx), PROPERTY_ENABLE); + lsm6dsv_filt_gy_lp1_bandwidth_set(&(this->dev_ctx), LSM6DSV_GY_ULTRA_LIGHT); + lsm6dsv_filt_xl_lp2_set(&(this->dev_ctx), PROPERTY_ENABLE); + lsm6dsv_filt_xl_lp2_bandwidth_set(&(this->dev_ctx), LSM6DSV_XL_STRONG); +} + +float Imu::get_orientation(Axis axis) { + return 0.0f; +} + +float Imu::get_angular_velocity(Axis axis) { + lsm6dsv_angular_rate_raw_get(&(this->dev_ctx), this->angular_velocity.data()); + + switch (axis) { + case Axis::X: + return lsm6dsv_from_fs4000_to_mdps(this->angular_velocity[0]); + + case Axis::Y: + return lsm6dsv_from_fs4000_to_mdps(this->angular_velocity[1]); + + case Axis::Z: + return lsm6dsv_from_fs4000_to_mdps(this->angular_velocity[2]); + + default: + return 0.0; + } +} + +float Imu::get_linear_acceleration(Axis axis) { + lsm6dsv_acceleration_raw_get(&(this->dev_ctx), this->linear_acceleration.data()); + + switch (axis) { + case Axis::X: + return lsm6dsv_from_fs8_to_mg(this->linear_acceleration[0]); + + case Axis::Y: + return lsm6dsv_from_fs8_to_mg(this->linear_acceleration[1]); + + case Axis::Z: + return lsm6dsv_from_fs8_to_mg(this->linear_acceleration[2]); + + default: + return 0.0; + } } } // namespace proxy From d9dda69edbedd049cbc567e801696d2243258f50 Mon Sep 17 00:00:00 2001 From: Gabriel Cosme Barbosa <62270066+GabrielCosme@users.noreply.github.com> Date: Thu, 21 Mar 2024 15:21:30 -0300 Subject: [PATCH 046/561] =?UTF-8?q?=F0=9F=92=A1Fix=20comment=20on=20spi=20?= =?UTF-8?q?gpio?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Eduardo Santos Barreto --- inc/hal/spi.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index 49d5632..5934e01 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -70,7 +70,7 @@ class Spi { SPI_HandleTypeDef* handle; /** - * @brief Array of GPIOs for the switches + * @brief GPIO for the chip select pin */ hal::Gpio gpio; }; From f17e91b997671ce30faaa2d953d93be929241d2e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 15:23:27 -0300 Subject: [PATCH 047/561] =?UTF-8?q?=E2=9E=96=20Remove=20functional=20inclu?= =?UTF-8?q?des?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 1 - inc/hal/spi.hpp | 1 - inc/hal/timer.hpp | 1 - 3 files changed, 3 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index d062f67..991dfd5 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -11,7 +11,6 @@ #include #include -#include namespace hal { /** diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index 5934e01..d88aec6 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -10,7 +10,6 @@ #define __SPI_HPP__ #include -#include #include #include "hal/gpio.hpp" diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index 7b6a1bb..62924d2 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -10,7 +10,6 @@ #define __TIMER_HPP__ #include -#include #include namespace hal { From c137fbf33f53d3ee8d25d377095858ff943e5b0d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 15:25:05 -0300 Subject: [PATCH 048/561] =?UTF-8?q?=F0=9F=90=9B=20Remove=20extra=20dma=20s?= =?UTF-8?q?tart=20from=20current=20sensors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/current_sensors.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp index 6e7c229..85b110a 100644 --- a/src/proxy/current_sensors.cpp +++ b/src/proxy/current_sensors.cpp @@ -17,7 +17,6 @@ template CurrentSensors::CurrentSensors(Config& current_sensors_config) : adc{current_sensors_config.adc_config}, shunt_resistor{current_sensors_config.shunt_resistor} { this->adc.start_dma(this->adc_buffer.data(), num_of_sensors); - this->adc.start_dma(this->adc_buffer.data(), num_of_sensors); } template From f99d2a852d00916db65451600730ac0125b8a531 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 15:49:22 -0300 Subject: [PATCH 049/561] =?UTF-8?q?=F0=9F=9A=9A=20Rename=20motor=5Fdriver?= =?UTF-8?q?=20to=20fan?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/{motor_driver.hpp => fan.hpp} | 40 ++++++++++++------------- src/proxy/{motor_driver.cpp => fan.cpp} | 24 +++++++-------- 2 files changed, 32 insertions(+), 32 deletions(-) rename inc/proxy/{motor_driver.hpp => fan.hpp} (54%) rename src/proxy/{motor_driver.cpp => fan.cpp} (54%) diff --git a/inc/proxy/motor_driver.hpp b/inc/proxy/fan.hpp similarity index 54% rename from inc/proxy/motor_driver.hpp rename to inc/proxy/fan.hpp index 84b529e..430e27c 100644 --- a/inc/proxy/motor_driver.hpp +++ b/inc/proxy/fan.hpp @@ -1,13 +1,13 @@ /** - * @file motor_driver.hpp + * @file fan.hpp * - * @brief Proxy MotorDriver class declaration + * @brief Proxy Fan class declaration * * @date 03/2024 */ -#ifndef __MOTOR_DRIVER_HPP__ -#define __MOTOR_DRIVER_HPP__ +#ifndef __FAN_HPP__ +#define __FAN_HPP__ #include @@ -16,12 +16,12 @@ namespace proxy { /** - * @brief Class for controlling a motor driver + * @brief Class for controlling the fan */ -class MotorDriver { +class Fan { public: /** - * @brief Configuration structure for the motor driver + * @brief Configuration structure for the fan */ struct Config { hal::Pwm::Config pwm; @@ -38,57 +38,57 @@ class MotorDriver { }; /** - * @brief Construct a new Motor Driver object + * @brief Construct a new fan object * - * @param motor_driver_config + * @param fan_config */ - MotorDriver(Config& motor_driver_config); + Fan(Config& fan_config); /** - * @brief Enable the motor driver + * @brief Enable the fan */ void enable(); /** - * @brief Disable the motor driver + * @brief Disable the fan */ void disable(); /** - * @brief Set the speed of the motors + * @brief Set the speed of the fans * - * @param speed Speed percentage of the motor + * @param speed Speed percentage of the fan */ void set_speed(float speed); /** - * @brief Stop the motors + * @brief Stop the fan */ void stop(); private: /** - * @brief Set the rotation direction of the motor + * @brief Set the rotation direction of the fan * * @param direction Rotation direction */ void set_direction(RotationDirection direction); /** - * @brief PWM object for controlling the motor speed + * @brief PWM object for controlling the fan speed */ hal::Pwm pwm; /** - * @brief GPIO object for controlling the motor rotation direction + * @brief GPIO object for controlling the fan rotation direction */ hal::Gpio direction_gpio; /** - * @brief GPIO handle for the motor driver enable pin + * @brief GPIO handle for the fan enable pin */ hal::Gpio enable_gpio; }; } // namespace proxy -#endif // __MOTOR_DRIVER_HPP__ +#endif // __FAN_HPP__ diff --git a/src/proxy/motor_driver.cpp b/src/proxy/fan.cpp similarity index 54% rename from src/proxy/motor_driver.cpp rename to src/proxy/fan.cpp index 01a3d34..f2fe5d4 100644 --- a/src/proxy/motor_driver.cpp +++ b/src/proxy/fan.cpp @@ -1,31 +1,31 @@ /** - * @file motor_driver.cpp + * @file fan.cpp * - * @brief Proxy MotorDriver class source + * @brief Proxy Fan class source * * @date 03/2024 */ -#include "proxy/motor_driver.hpp" +#include "proxy/fan.hpp" namespace proxy { -MotorDriver::MotorDriver(Config& motor_driver_config) : - pwm{motor_driver_config.pwm}, - direction_gpio{motor_driver_config.direction_gpio}, - enable_gpio{motor_driver_config.enable_gpio} { +Fan::Fan(Config& fan_config) : + pwm{fan_config.pwm}, + direction_gpio{fan_config.direction_gpio}, + enable_gpio{fan_config.enable_gpio} { this->enable(); this->stop(); } -void MotorDriver::enable() { +void Fan::enable() { this->enable_gpio.write(true); } -void MotorDriver::disable() { +void Fan::disable() { this->enable_gpio.write(false); } -void MotorDriver::set_speed(float speed) { +void Fan::set_speed(float speed) { if (speed > 0.0f) { this->pwm.set_duty_cycle(speed); this->set_direction(RotationDirection::FORWARD); @@ -37,11 +37,11 @@ void MotorDriver::set_speed(float speed) { } } -void MotorDriver::stop() { +void Fan::stop() { this->set_speed(0.0f); } -void MotorDriver::set_direction(RotationDirection direction) { +void Fan::set_direction(RotationDirection direction) { this->direction_gpio.write(static_cast(direction)); } } // namespace proxy From cc95cda58ac4994222351d68ed59ef3e2bf34fb8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 15:50:39 -0300 Subject: [PATCH 050/561] =?UTF-8?q?=F0=9F=9A=9A=20Rename=20dual=20motor=20?= =?UTF-8?q?driver=20to=20locomotion?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../{dual_motor_driver.hpp => locomotion.hpp} | 22 +++++++------- .../{dual_motor_driver.cpp => locomotion.cpp} | 30 +++++++++---------- 2 files changed, 26 insertions(+), 26 deletions(-) rename inc/proxy/{dual_motor_driver.hpp => locomotion.hpp} (78%) rename src/proxy/{dual_motor_driver.cpp => locomotion.cpp} (61%) diff --git a/inc/proxy/dual_motor_driver.hpp b/inc/proxy/locomotion.hpp similarity index 78% rename from inc/proxy/dual_motor_driver.hpp rename to inc/proxy/locomotion.hpp index 3bf9475..24da7bb 100644 --- a/inc/proxy/dual_motor_driver.hpp +++ b/inc/proxy/locomotion.hpp @@ -1,13 +1,13 @@ /** - * @file dual_motor_driver.hpp + * @file locomotion.hpp * - * @brief Proxy DualMotorDriver class declaration + * @brief Proxy Locomotion class declaration * * @date 03/2024 */ -#ifndef __DUAL_MOTOR_DRIVER_HPP__ -#define __DUAL_MOTOR_DRIVER_HPP__ +#ifndef __DUAL_LOCOMOTION_HPP__ +#define __DUAL_LOCOMOTION_HPP__ #include @@ -16,12 +16,12 @@ namespace proxy { /** - * @brief Class for controlling a dual motor driver + * @brief Class for controlling the locomotion */ -class DualMotorDriver { +class Locomotion { public: /** - * @brief Configuration structure for the dual motor driver + * @brief Configuration structure for the locomotion */ struct Config { hal::Pwm::Config pwm_left_fwd; @@ -32,11 +32,11 @@ class DualMotorDriver { }; /** - * @brief Construct a new Dual Motor Driver object + * @brief Construct a new locomotion object * - * @param dual_motor_driver_config + * @param locomotion_config */ - DualMotorDriver(Config& dual_motor_driver_config); + Locomotion(Config& locomotion_config); /** * @brief Enable the motor driver @@ -99,4 +99,4 @@ class DualMotorDriver { }; } // namespace proxy -#endif // __DUAL_MOTOR_DRIVER_HPP__ +#endif // __DUAL_LOCOMOTION_HPP__ diff --git a/src/proxy/dual_motor_driver.cpp b/src/proxy/locomotion.cpp similarity index 61% rename from src/proxy/dual_motor_driver.cpp rename to src/proxy/locomotion.cpp index 41f82fe..86205a7 100644 --- a/src/proxy/dual_motor_driver.cpp +++ b/src/proxy/locomotion.cpp @@ -1,33 +1,33 @@ /** - * @file dual_motor_driver.cpp + * @file locomotion.cpp * - * @brief Proxy DualMotorDriver class source + * @brief Proxy Locomotion class source * * @date 03/2024 */ -#include "proxy/dual_motor_driver.hpp" +#include "proxy/locomotion.hpp" namespace proxy { -DualMotorDriver::DualMotorDriver(Config& motor_driver_config) : - pwm_left_fwd{motor_driver_config.pwm_left_fwd}, - pwm_left_bwd{motor_driver_config.pwm_left_bwd}, - pwm_right_fwd{motor_driver_config.pwm_right_fwd}, - pwm_right_bwd{motor_driver_config.pwm_right_bwd}, - enable_gpio{motor_driver_config.enable_gpio} { +Locomotion::Locomotion(Config& locomotion_config) : + pwm_left_fwd{locomotion_config.pwm_left_fwd}, + pwm_left_bwd{locomotion_config.pwm_left_bwd}, + pwm_right_fwd{locomotion_config.pwm_right_fwd}, + pwm_right_bwd{locomotion_config.pwm_right_bwd}, + enable_gpio{locomotion_config.enable_gpio} { this->enable(); this->stop(); } -void DualMotorDriver::enable() { +void Locomotion::enable() { this->enable_gpio.write(true); } -void DualMotorDriver::disable() { +void Locomotion::disable() { this->enable_gpio.write(false); } -void DualMotorDriver::set_speed(float left_speed, float right_speed) { +void Locomotion::set_speed(float left_speed, float right_speed) { if (left_speed > 0.0f) { this->pwm_left_fwd.set_duty_cycle(left_speed); this->pwm_left_bwd.set_duty_cycle(0.0f); @@ -49,17 +49,17 @@ void DualMotorDriver::set_speed(float left_speed, float right_speed) { } } -void DualMotorDriver::stop() { +void Locomotion::stop() { this->stop_left_motor(); this->stop_right_motor(); } -void DualMotorDriver::stop_left_motor() { +void Locomotion::stop_left_motor() { this->pwm_left_fwd.set_duty_cycle(0.0f); this->pwm_left_bwd.set_duty_cycle(0.0f); } -void DualMotorDriver::stop_right_motor() { +void Locomotion::stop_right_motor() { this->pwm_right_fwd.set_duty_cycle(0.0f); this->pwm_right_bwd.set_duty_cycle(0.0f); } From b7d4d2941ff47c94c2ccde99d36f1cd45cf0837b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 15:52:06 -0300 Subject: [PATCH 051/561] =?UTF-8?q?=E2=9E=95=20Add=20missing=20array=20inc?= =?UTF-8?q?lude=20to=20distance=20sensors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/distance_sensors.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index f6bb1dc..3522fcb 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -9,6 +9,7 @@ #ifndef __DISTANCE_SENSORS_HPP__ #define __DISTANCE_SENSORS_HPP__ +#include #include #include "hal/adc_dma.hpp" From 26fab6ed64b3d442523d285bcc5a951a12ebccf5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 17:30:11 -0300 Subject: [PATCH 052/561] =?UTF-8?q?=F0=9F=9A=9A=20Rename=20distance=20sens?= =?UTF-8?q?or=20variables?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/distance_sensors.hpp | 18 ++++++++++++------ src/proxy/distance_sensors.cpp | 23 ++++++++++++----------- 2 files changed, 24 insertions(+), 17 deletions(-) diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index 3522fcb..d133eb3 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -27,7 +27,7 @@ class DistanceSensors { */ struct Config { hal::AdcDma::Config adc; - hal::Pwm::Config infrared_pwm; + hal::Pwm::Config led_pwm; }; /** @@ -38,11 +38,11 @@ class DistanceSensors { DistanceSensors(Config& distance_sensor_config); /** - * @brief Set the infrared DistanceSensors intensity + * @brief Set the distance sensors led intensity * * @param intensity Intensity percentage of the infrared LED */ - void set_infrared_led_intensity(float intensity); + void set_led_intensity(float intensity); /** * @brief Get the distance from a sensor @@ -64,18 +64,24 @@ class DistanceSensors { /** * @brief ADC DMA handle */ - hal::AdcDma distance_sensor_adc; + hal::AdcDma adc; /** * @brief PWM handle for infrared LED */ - hal::Pwm infrared_pwm; + hal::Pwm led_pwm; /** * @brief Buffer to store the ADC values */ - volatile std::array dma_buffer; + std::array buffer; + + static constexpr float max_distance{0.3f}; + + static constexpr uint32_t max_adc_reading{4095}; }; } // namespace proxy +#include "../src/proxy/distance_sensors.cpp" + #endif // __DISTANCE_SENSORS_HPP__ diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index 6eb2e21..ea00b0f 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -6,32 +6,33 @@ * @date 03/2024 */ +#ifndef __DISTANCE_SENSORS_CPP__ +#define __DISTANCE_SENSORS_CPP__ + #include "proxy/distance_sensors.hpp" namespace proxy { -static constexpr float max_distance{0.3f}; - -static constexpr uint32_t max_adc_reading{4095}; - template DistanceSensors::DistanceSensors(Config& distance_sensor_config) : - distance_sensor_adc{distance_sensor_config.sensor_adc_config}, infrared_pwm{distance_sensor_config.infrared_pwm} { - this->distance_sensor_adc.start_dma(this->adc_buffer.data(), num_of_sensors); - this->infrared_pwm.set_duty_cycle(100.0f); + adc{distance_sensor_config.adc}, led_pwm{distance_sensor_config.led_pwm} { + this->adc.start_dma(this->buffer.data(), num_of_sensors); + this->led_pwm.set_duty_cycle(100.0f); } template -void DistanceSensors::set_infrared_led_intensity(float intensity) { - this->infrared_pwm.set_duty_cycle(intensity); +void DistanceSensors::set_led_intensity(float intensity) { + this->led_pwm.set_duty_cycle(intensity); } template float DistanceSensors::get_distance(uint8_t sensor_index) const { - return max_distance * this->adc_buffer.at(sensor_index) / max_adc_reading; + return this->max_distance * this->buffer.at(sensor_index) / this->max_adc_reading; } template uint32_t DistanceSensors::get_distance_raw(uint8_t sensor_index) const { - return this->adc_buffer.at(sensor_index); + return this->buffer.at(sensor_index); } } // namespace proxy + +#endif // __DISTANCE_SENSORS_HPP__ From 1055ad3acc6778a6789eef5502d841bd419572df Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 17:31:21 -0300 Subject: [PATCH 053/561] =?UTF-8?q?=F0=9F=9A=9A=20Rename=20current=20senso?= =?UTF-8?q?r=20variables?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/current_sensors.hpp | 8 +++++++- src/proxy/current_sensors.cpp | 17 +++++++++-------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index 13222de..bdedfff 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -61,13 +61,19 @@ class CurrentSensors { /** * @brief Buffer to store the ADC values */ - volatile std::array dma_buffer; + std::array buffer; /** * @brief Value of the shunt resistor in ohms */ float shunt_resistor; + + static constexpr float reference_voltage{3.3f}; + + static constexpr uint32_t max_adc_reading{4095}; }; } // namespace proxy +#include "../src/proxy/current_sensors.cpp" + #endif // __CURRENT_SENSORS_HPP__ diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp index 85b110a..45017a3 100644 --- a/src/proxy/current_sensors.cpp +++ b/src/proxy/current_sensors.cpp @@ -6,26 +6,27 @@ * @date 03/2024 */ +#ifndef __CURRENT_SENSORS_CPP__ +#define __CURRENT_SENSORS_CPP__ + #include "proxy/current_sensors.hpp" namespace proxy { -static constexpr float reference_voltage{3.3f}; - -static constexpr uint32_t max_adc_reading{4095}; - template CurrentSensors::CurrentSensors(Config& current_sensors_config) : - adc{current_sensors_config.adc_config}, shunt_resistor{current_sensors_config.shunt_resistor} { - this->adc.start_dma(this->adc_buffer.data(), num_of_sensors); + adc{current_sensors_config.adc}, shunt_resistor{current_sensors_config.shunt_resistor} { + this->adc.start_dma(this->buffer.data(), num_of_sensors); } template float CurrentSensors::get_current(uint8_t sensor_index) const { - return reference_voltage * this->adc_buffer.at(sensor_index) / (max_adc_reading * this->shunt_resistor); + return this->reference_voltage * this->buffer.at(sensor_index) / (this->max_adc_reading * this->shunt_resistor); } template uint32_t CurrentSensors::get_current_raw(uint8_t sensor_index) const { - return this->adc_buffer.at(sensor_index); + return this->buffer.at(sensor_index); } } // namespace proxy + +#endif // __CURRENT_SENSORS_CPP__ From faeee23efad8cc1cb6ac159dfce708ad43035722 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 17:31:40 -0300 Subject: [PATCH 054/561] =?UTF-8?q?=E2=9E=95=20Add=20missing=20array=20inc?= =?UTF-8?q?lude=20on=20imu?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/imu.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 1b0c540..0153bd0 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -9,6 +9,7 @@ #ifndef __IMU_HPP__ #define __IMU_HPP__ +#include #include #include From a4dbc495fa02003c482d94a27b0b1cbe80486b0e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 17:55:36 -0300 Subject: [PATCH 055/561] =?UTF-8?q?=F0=9F=8D=BB=20Fix=20gpio=20initializat?= =?UTF-8?q?ion=20on=20dip=20switch?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/dip_switch.hpp | 2 ++ src/proxy/dip_switch.cpp | 13 +++++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp index 9d555e4..5667f8e 100644 --- a/inc/proxy/dip_switch.hpp +++ b/inc/proxy/dip_switch.hpp @@ -59,4 +59,6 @@ class DipSwitch { }; } // namespace proxy +#include "../src/proxy/dip_switch.cpp" + #endif // __DIP_SWITCH_HPP__ diff --git a/src/proxy/dip_switch.cpp b/src/proxy/dip_switch.cpp index dd623d2..9f1b37c 100644 --- a/src/proxy/dip_switch.cpp +++ b/src/proxy/dip_switch.cpp @@ -6,15 +6,18 @@ * @date 03/2024 */ +#ifndef __DIP_SWITCH_CPP__ +#define __DIP_SWITCH_CPP__ + #include "hal/mcu.hpp" #include "proxy/dip_switch.hpp" namespace proxy { template -DipSwitch::DipSwitch(Config& dip_switch_config) { - for (uint8_t i = 0; i < num_of_sensors; i++) { - this->gpio_array[i] = dip_switch_config.gpio_array[i]; - } +DipSwitch::DipSwitch(Config& dip_switch_config) : + gpio_array([&] (std::index_sequence) -> std::array { + return {hal::Gpio{dip_switch_config.gpio_array[I]}...}; + }(std::make_index_sequence())) { } template @@ -33,3 +36,5 @@ uint8_t DipSwitch::get_switches_value() const { return switches_value; } } // namespace proxy + +#endif // __DIP_SWITCH_CPP__ From 10e270e0592fbfaea1aaa73e898cec9f60441ad3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 18:12:47 -0300 Subject: [PATCH 056/561] =?UTF-8?q?=F0=9F=9A=B8=20Change=20stop=20motor=20?= =?UTF-8?q?visibility=20on=20locomotion?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/locomotion.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/inc/proxy/locomotion.hpp b/inc/proxy/locomotion.hpp index 24da7bb..ffd03ad 100644 --- a/inc/proxy/locomotion.hpp +++ b/inc/proxy/locomotion.hpp @@ -61,7 +61,6 @@ class Locomotion { */ void stop(); - private: /** * @brief Stop the left motor */ @@ -72,6 +71,7 @@ class Locomotion { */ void stop_right_motor(); + private: /** * @brief PWM handle for the left motor forward */ From ae591ccc551a1912f5925b782a97850d6b369f1f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 18:13:32 -0300 Subject: [PATCH 057/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Change=20order=20o?= =?UTF-8?q?f=20functions=20on=20drivers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/fan.cpp | 6 +++--- src/proxy/locomotion.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/proxy/fan.cpp b/src/proxy/fan.cpp index f2fe5d4..995a7b0 100644 --- a/src/proxy/fan.cpp +++ b/src/proxy/fan.cpp @@ -13,8 +13,8 @@ Fan::Fan(Config& fan_config) : pwm{fan_config.pwm}, direction_gpio{fan_config.direction_gpio}, enable_gpio{fan_config.enable_gpio} { - this->enable(); this->stop(); + this->enable(); } void Fan::enable() { @@ -27,11 +27,11 @@ void Fan::disable() { void Fan::set_speed(float speed) { if (speed > 0.0f) { - this->pwm.set_duty_cycle(speed); this->set_direction(RotationDirection::FORWARD); + this->pwm.set_duty_cycle(speed); } else if (speed < 0.0f) { - this->pwm.set_duty_cycle(-speed); this->set_direction(RotationDirection::BACKWARD); + this->pwm.set_duty_cycle(-speed); } else { this->stop(); } diff --git a/src/proxy/locomotion.cpp b/src/proxy/locomotion.cpp index 86205a7..8dc6e62 100644 --- a/src/proxy/locomotion.cpp +++ b/src/proxy/locomotion.cpp @@ -15,8 +15,8 @@ Locomotion::Locomotion(Config& locomotion_config) : pwm_right_fwd{locomotion_config.pwm_right_fwd}, pwm_right_bwd{locomotion_config.pwm_right_bwd}, enable_gpio{locomotion_config.enable_gpio} { - this->enable(); this->stop(); + this->enable(); } void Locomotion::enable() { From c70bed68454ed64a44fe820e1d0b4b16123c098a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 23:38:07 -0300 Subject: [PATCH 058/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20timer=20from=20?= =?UTF-8?q?mcu?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/mcu.hpp | 31 ++++--------------------------- src/hal/mcu.cpp | 14 +------------- 2 files changed, 5 insertions(+), 40 deletions(-) diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index 8066c8c..af45afc 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -11,10 +11,6 @@ #include -/***************************************** - * Public Function Prototypes - *****************************************/ - extern "C" { /** @@ -25,34 +21,15 @@ void SystemClock_Config(void); } namespace hal { -class mcu { +/** + * @brief Microcontroller unit class + */ +class Mcu { public: /** * @brief Initializes MCU and some peripherals */ static void init(void); - - /** - * @brief Put the MCU to sleep - * - * @param ms Sleep time in milliseconds - */ - static void sleep(uint32_t ms); - - /** - * @brief Reset timer - * - * @param tim_counter Timer counter to reset - */ - static void reset_timer(uint32_t& tim_counter); - - /** - * @brief Get timer value in milliseconds - * - * @param tim_counter Timer counter to get value from - * @return Timer value in milliseconds - */ - static uint32_t get_timer_ms(uint32_t tim_counter); }; }; // namespace hal #endif // __MCU_HPP__ diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index b9a2cf5..a1b5de3 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -10,23 +10,11 @@ #include "hal/mcu.hpp" namespace hal { -void mcu::init(void) { +void Mcu::init(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); } - -void mcu::sleep(uint32_t ms) { - HAL_Delay(ms); -} - -void mcu::reset_timer(uint32_t& ticks) { - ticks = HAL_GetTick(); -} - -uint32_t mcu::get_timer_ms(uint32_t ticks) { - return HAL_GetTick() - ticks; -} } From fe372dcb223b64cfac2ecdaccee6528e4232512a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 23:39:29 -0300 Subject: [PATCH 059/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Move=20pwm=20funct?= =?UTF-8?q?ions=20from=20timer=20to=20pwm?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/pwm.hpp | 12 ++++++------ src/hal/pwm.cpp | 20 +++++++++++--------- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/inc/hal/pwm.hpp b/inc/hal/pwm.hpp index 1785eca..e6d5359 100644 --- a/inc/hal/pwm.hpp +++ b/inc/hal/pwm.hpp @@ -10,8 +10,7 @@ #define __PWM_HPP__ #include - -#include "hal/timer.hpp" +#include namespace hal { /** @@ -23,8 +22,9 @@ class Pwm { * @brief PWM configuration struct */ struct Config { - Timer::Config timer; - uint32_t timer_channel; + TIM_HandleTypeDef* handle; + void (* init_function)(void); + uint32_t timer_channel; }; /** @@ -50,9 +50,9 @@ class Pwm { private: /** - * @brief Timer object + * @brief Timer handle */ - Timer timer; + TIM_HandleTypeDef* handle; /** * @brief Channel number of the timer diff --git a/src/hal/pwm.cpp b/src/hal/pwm.cpp index bb25432..299249d 100644 --- a/src/hal/pwm.cpp +++ b/src/hal/pwm.cpp @@ -9,20 +9,22 @@ #include "hal/pwm.hpp" namespace hal { -Pwm::Pwm(Config& pwm_config) : timer{pwm_config.timer}, channel{pwm_config.timer_channel} { - this->timer.pwm_start(this->channel); - this->timer.set_compare(this->channel, 0); +Pwm::Pwm(Config& pwm_config) : handle{pwm_config.handle}, channel{pwm_config.timer_channel} { + pwm_config.init_function(); + HAL_TIM_PWM_Start(this->handle, this->channel); + __HAL_TIM_SET_COMPARE(this->handle, this->channel, 0); } void Pwm::set_duty_cycle(float duty_cycle) { - uint32_t compare = (duty_cycle * this->timer.get_autoreload()) / 100; - this->timer.set_compare(this->channel, compare); + uint32_t compare = (duty_cycle * __HAL_TIM_GET_AUTORELOAD(this->handle)) / 100; + __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); } void Pwm::set_frequency(uint32_t frequency) { - uint32_t clock_freq = this->timer.get_timer_clock_freq(); - uint32_t prescaler = this->timer.get_prescaler(); - uint32_t autoreload = clock_freq / ((prescaler + 1) * frequency); - this->timer.set_autoreload(autoreload); + uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); + uint32_t prescaler = this->handle->Instance->PSC; + + uint32_t autoreload = base_freq / ((prescaler + 1) * frequency) - 1; + __HAL_TIM_SET_AUTORELOAD(this->handle, autoreload); } } // namespace hal From ea047a809ec484adda68d0fd54e2e728b82066a9 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 23:40:34 -0300 Subject: [PATCH 060/561] =?UTF-8?q?=E2=9C=A8=20Add=20encoder=20hal=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/encoder.hpp | 51 +++++++++++++++++++++++++++++++++++++++++++++ src/hal/encoder.cpp | 20 ++++++++++++++++++ 2 files changed, 71 insertions(+) create mode 100644 inc/hal/encoder.hpp create mode 100644 src/hal/encoder.cpp diff --git a/inc/hal/encoder.hpp b/inc/hal/encoder.hpp new file mode 100644 index 0000000..b14fd50 --- /dev/null +++ b/inc/hal/encoder.hpp @@ -0,0 +1,51 @@ +/** + * @file encoder.hpp + * + * @brief STM32 encoder HAL wrapper + * + * @date 03/2024 + */ + +#ifndef __ENCODER_HPP__ +#define __ENCODER_HPP__ + +#include + +namespace hal { +/** + * @brief Class to handle encoder peripheral on STM32 microcontrollers + */ +class Encoder { + public: + /** + * @brief Encoder configuration struct + */ + struct Config { + TIM_HandleTypeDef* handle; + void (* init_function)(void); + uint32_t timer_channel; + }; + + /** + * @brief Construct a new Encoder object + * + * @param encoder_config Configuration for the encoder + */ + Encoder(Config& encoder_config); + + /** + * @brief Get the counter value + * + * @return int32_t Current value of the counter + */ + int32_t get_counter(); + + private: + /** + * @brief Timer handle + */ + TIM_HandleTypeDef* handle; +}; +} // namespace hal + +#endif // __ENCODER_HPP__ diff --git a/src/hal/encoder.cpp b/src/hal/encoder.cpp new file mode 100644 index 0000000..b21dc60 --- /dev/null +++ b/src/hal/encoder.cpp @@ -0,0 +1,20 @@ +/** + * @file encoder.cpp + * + * @brief STM32 encoder HAL wrapper + * + * @date 03/2024 + */ + +#include "hal/encoder.hpp" + +namespace hal { +Encoder::Encoder(Config& encoder_config) : handle{encoder_config.handle} { + encoder_config.init_function(); + HAL_TIM_Encoder_Start(this->handle, encoder_config.timer_channel); +} + +int32_t Encoder::get_counter() { + return static_cast(__HAL_TIM_GET_COUNTER(this->handle)); +} +} // namespace hal From 56bbd1230fbeda1340ff5932ae3e120eaeb7ace3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 21 Mar 2024 23:40:56 -0300 Subject: [PATCH 061/561] =?UTF-8?q?=E2=9C=A8=20Check=20spi=20lock=20on=20h?= =?UTF-8?q?al?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/hal/spi.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index 83948e8..3d751c0 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -13,6 +13,10 @@ Spi::Spi(Config& spi_config) : handle{spi_config.handle}, gpio{spi_config.gpio} } void Spi::select_device() { + if (this->handle->Lock == HAL_LOCKED) { + return; + } + this->gpio.write(false); } From aed9eb00a1782eda32cda029f1c5559bf3ee8c2f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 22 Mar 2024 00:01:16 -0300 Subject: [PATCH 062/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20timer?= =?UTF-8?q?=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/timer.hpp | 84 ++++++++++++++++++-------------------------- inc/proxy/button.hpp | 5 +-- inc/proxy/buzzer.hpp | 3 +- src/hal/timer.cpp | 63 +++++++++++++++++---------------- src/proxy/button.cpp | 11 +++--- src/proxy/buzzer.cpp | 4 +-- src/proxy/imu.cpp | 4 +-- 7 files changed, 82 insertions(+), 92 deletions(-) diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index 62924d2..a1c7eef 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -27,98 +27,84 @@ class Timer { }; /** - * @brief Constructor for the Timer class - * - * @param timer_config Configuration for timer handler + * @brief Construct a new Timer object */ - Timer(Config& timer_config); + Timer() = default; /** - * @brief Initialize timer base generation + * @brief Construct a new Timer object + * + * @param timer_config Configuration for the timer */ - void base_start(); + Timer(Config& timer_config); /** - * @brief Stop timer base generation + * @brief Reset the timer counter in milliseconds */ - void base_stop(); + void reset_ms(); /** - * @brief Set timer counter - * - * @param counter Counter value + * @brief Reset the timer counter in microseconds */ - void set_counter(uint32_t counter); + void reset_us(); /** - * @brief Get timer counter + * @brief Get the time elapsed since the last reset * - * @return uint32_t Counter value + * @return uint32_t Time elapsed in miliseconds */ - uint32_t get_counter(); + uint32_t elapsed_time_ms() const; /** - * @brief Start PWM generation + * @brief Get the time elapsed since the last reset * - * @param channel Timer channel to be enabled + * @return uint32_t Time elapsed in microseconds */ - void pwm_start(uint32_t channel); + uint32_t elapsed_time_us() const; /** - * @brief Stop PWM generation + * @brief Sleep for a given amount of time * - * @param channel Timer channel to be disabled + * @param time Time to sleep in milliseconds */ - void pwm_stop(uint32_t channel); + static void sleep_ms(uint32_t time); /** - * @brief Set timer prescaler + * @brief Sleep for a given amount of time * - * @param prescaler Prescaler new value + * @param time Time to sleep in microseconds */ - void set_prescaler(uint32_t prescaler); - - /** - * @brief Get the timer prescaler - * - * @return uint32_t The value of the prescaler - */ - uint32_t get_prescaler(); + void sleep_us(uint32_t time) const; + private: /** - * @brief Set the timer Capture Compare Register value + * @brief Get the current timer counter * - * @param channel Timer channel to be configured - * @param compare Compare register new value + * @return uint32_t Current timer counter in milliseconds */ - void set_compare(uint32_t channel, uint32_t compare); + uint32_t get_counter_ms() const; /** - * @brief Get the Autoreload register value + * @brief Get the current timer counter * - * @return uint32_t The value of the Autoreload register + * @return uint32_t Current timer counter in microseconds */ - uint32_t get_autoreload(); + uint32_t get_counter_us() const; /** - * @brief Set the Autoreload register value - * - * @param autoreload New value for the Autoreload register + * @brief Timer handle */ - void set_autoreload(uint32_t autoreload); + TIM_HandleTypeDef* handle; /** - * @brief Get the timer clock frequency - * - * @return uint32_t Timer clock frequency + * @brief Timer counter */ - uint32_t get_timer_clock_freq(); + uint32_t counter; - private: /** - * @brief Timer handler + * @brief Flag to enable microseconds */ - TIM_HandleTypeDef* handle; + bool enable_microseconds = false; }; } // namespace hal diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index dab7f64..a574007 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -12,6 +12,7 @@ #include #include "hal/gpio.hpp" +#include "hal/timer.hpp" namespace proxy { /** @@ -116,12 +117,12 @@ class Button { /** * @brief Timer to check if button is debouncing */ - uint32_t debounce_timer = 0; + hal::Timer debounce_timer; /** * @brief Timer to determine type of button press */ - uint32_t status_timer = 0; + hal::Timer status_timer; /** * @brief Flag to know when button is debouncing diff --git a/inc/proxy/buzzer.hpp b/inc/proxy/buzzer.hpp index 10fca10..13c2496 100644 --- a/inc/proxy/buzzer.hpp +++ b/inc/proxy/buzzer.hpp @@ -12,6 +12,7 @@ #include #include "hal/pwm.hpp" +#include "hal/timer.hpp" namespace proxy { /** @@ -60,7 +61,7 @@ class Buzzer { /** * @brief Timer to play the sound */ - uint32_t timer = 0; + hal::Timer timer; /** * @brief Flag to check if the buzzer is playing diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index 088276a..6898579 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -9,55 +9,58 @@ #include "hal/timer.hpp" namespace hal { -Timer::Timer(Config& tim_config) : handle{tim_config.handle} { - tim_config.init_function(); -} +Timer::Timer(Config& timer_config) : handle{timer_config.handle} { + timer_config.init_function(); -void Timer::base_start() { - HAL_TIM_Base_Start(this->handle); -} + uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); + uint32_t prescaler = this->handle->Instance->PSC; -void Timer::base_stop() { - HAL_TIM_Base_Stop(this->handle); + if (base_freq / (prescaler + 1) == 1000000 and __HAL_TIM_GetAutoreload(this->handle) == 1000) { + this->enable_microseconds = true; + } } -void Timer::set_counter(uint32_t counter) { - __HAL_TIM_SET_COUNTER(this->handle, counter); +void Timer::reset_ms() { + this->counter = this->get_counter_ms(); } -uint32_t Timer::get_counter() { - return __HAL_TIM_GET_COUNTER(this->handle); +void Timer::reset_us() { + this->counter = this->get_counter_us(); } -void Timer::pwm_start(uint32_t channel) { - HAL_TIM_PWM_Start(this->handle, channel); +uint32_t Timer::elapsed_time_ms() const { + return this->get_counter_ms() - this->counter; } -void Timer::pwm_stop(uint32_t channel) { - HAL_TIM_PWM_Stop(this->handle, channel); +uint32_t Timer::elapsed_time_us() const { + return this->get_counter_us() - this->counter; } -void Timer::set_prescaler(uint32_t prescaler) { - __HAL_TIM_SET_PRESCALER(this->handle, prescaler); -} +void Timer::sleep_ms(uint32_t time) { + uint32_t start = HAL_GetTick(); -uint32_t Timer::get_prescaler() { - return this->handle->Instance->PSC; + while (HAL_GetTick() - start < time) { + continue; + } } -void Timer::set_compare(uint32_t channel, uint32_t compare) { - __HAL_TIM_SET_COMPARE(this->handle, channel, compare); -} +void Timer::sleep_us(uint32_t time) const { + uint32_t start = this->get_counter_us(); -uint32_t Timer::get_autoreload() { - return __HAL_TIM_GET_AUTORELOAD(this->handle); + while (this->get_counter_us() - start < time) { + continue; + } } -void Timer::set_autoreload(uint32_t autoreload) { - __HAL_TIM_SET_AUTORELOAD(this->handle, autoreload); +uint32_t Timer::get_counter_ms() const { + return HAL_GetTick(); } -uint32_t Timer::get_timer_clock_freq() { - return HAL_RCC_GetPCLK1Freq(); +uint32_t Timer::get_counter_us() const { + if (this->enable_microseconds) { + return 1000 * HAL_GetTick() + __HAL_TIM_GET_COUNTER(this->handle); + } + + return 1000 * HAL_GetTick(); } } // namespace hal diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index a358381..15a8e25 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -6,7 +6,6 @@ * @date 03/2024 */ -#include "hal/mcu.hpp" #include "proxy/button.hpp" namespace proxy { @@ -27,11 +26,11 @@ Button::Status Button::get_status() { this->update_state(); if (this->is_rising_edge()) { - hal::mcu::reset_timer(this->status_timer); + this->status_timer.reset_ms(); } else if (this->is_falling_edge()) { - if (hal::mcu::get_timer_ms(this->status_timer) > extra_long_press_delay) { + if (this->status_timer.elapsed_time_ms() > extra_long_press_delay) { return EXTRA_LONG_PRESS; - } else if (hal::mcu::get_timer_ms(this->status_timer) > long_press_delay) { + } else if (this->status_timer.elapsed_time_ms() > long_press_delay) { return LONG_PRESS; } else { return SHORT_PRESS; @@ -50,8 +49,8 @@ void Button::update_state() { if ((raw_reading != this->current_state) and not this->is_debouncing) { this->is_debouncing = true; - hal::mcu::reset_timer(this->debounce_timer); - } else if ((hal::mcu::get_timer_ms(this->debounce_timer) < debounce_delay) and this->is_debouncing) { + this->debounce_timer.reset_ms(); + } else if ((this->debounce_timer.elapsed_time_ms() < debounce_delay) and this->is_debouncing) { if (this->current_state == raw_reading) { this->is_debouncing = false; } diff --git a/src/proxy/buzzer.cpp b/src/proxy/buzzer.cpp index 286acdb..23ed2ae 100644 --- a/src/proxy/buzzer.cpp +++ b/src/proxy/buzzer.cpp @@ -21,12 +21,12 @@ void Buzzer::play(uint32_t frequency, uint32_t duration) { if (duration > 0) { this->duration = duration; - hal::mcu::reset_timer(this->timer); + this->timer.reset_ms(); } } void Buzzer::update() { - if (this->is_playing and hal::mcu::get_timer_ms(this->timer) > this->duration) { + if (this->is_playing and this->timer.elapsed_time_ms() > this->duration) { this->stop(); } } diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 01aa6af..99f4615 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -6,7 +6,7 @@ * @date 03/2024 */ -#include "hal/mcu.hpp" +#include "hal/timer.hpp" #include "proxy/imu.hpp" namespace proxy { @@ -38,7 +38,7 @@ Imu::Imu(Config& imu_config) : spi{imu_config.spi} { this->dev_ctx.handle = &imu_config.spi; - hal::mcu::sleep(10); + hal::Timer::sleep_ms(10); lsm6dsv_reset_set(&(this->dev_ctx), LSM6DSV_RESTORE_CTRL_REGS); From 18f58b043b4f2e466ef5630905a77a43c1b15f1f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 22 Mar 2024 00:37:59 -0300 Subject: [PATCH 063/561] =?UTF-8?q?=F0=9F=8E=A8=20Fix=20static=20constants?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 10 ++++++++++ inc/proxy/current_sensors.hpp | 4 ---- inc/proxy/distance_sensors.hpp | 5 +++-- src/proxy/current_sensors.cpp | 3 ++- src/proxy/distance_sensors.cpp | 2 +- 5 files changed, 16 insertions(+), 8 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index 991dfd5..f5cc355 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -51,6 +51,16 @@ class AdcDma { * @brief ADC DMA handle */ ADC_HandleTypeDef* handle; + + /** + * @brief Reference voltage for the ADC measurement + */ + static constexpr float reference_voltage{3.3f}; + + /** + * @brief Maximum ADC reading + */ + static constexpr uint32_t max_reading{4095}; }; } diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index bdedfff..a831521 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -67,10 +67,6 @@ class CurrentSensors { * @brief Value of the shunt resistor in ohms */ float shunt_resistor; - - static constexpr float reference_voltage{3.3f}; - - static constexpr uint32_t max_adc_reading{4095}; }; } // namespace proxy diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index d133eb3..8d5fca2 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -76,9 +76,10 @@ class DistanceSensors { */ std::array buffer; + /** + * @brief Maximum distance reading in meters + */ static constexpr float max_distance{0.3f}; - - static constexpr uint32_t max_adc_reading{4095}; }; } // namespace proxy diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp index 45017a3..e66e1a9 100644 --- a/src/proxy/current_sensors.cpp +++ b/src/proxy/current_sensors.cpp @@ -20,7 +20,8 @@ CurrentSensors::CurrentSensors(Config& current_sensors_config) : template float CurrentSensors::get_current(uint8_t sensor_index) const { - return this->reference_voltage * this->buffer.at(sensor_index) / (this->max_adc_reading * this->shunt_resistor); + return hal::AdcDma::reference_voltage * this->buffer.at(sensor_index) / + (hal::AdcDma::max_reading * this->shunt_resistor); } template diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index ea00b0f..eb7096b 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -26,7 +26,7 @@ void DistanceSensors::set_led_intensity(float intensity) { template float DistanceSensors::get_distance(uint8_t sensor_index) const { - return this->max_distance * this->buffer.at(sensor_index) / this->max_adc_reading; + return DistanceSensors::max_distance * this->buffer.at(sensor_index) / hal::AdcDma::max_reading; } template From f2d3b17f08dc887b28c90237868bc94dcc480316 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 22 Mar 2024 00:59:46 -0300 Subject: [PATCH 064/561] =?UTF-8?q?=E2=9E=96=20Remove=20mcu=20include=20fr?= =?UTF-8?q?om=20buzzer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/buzzer.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/proxy/buzzer.cpp b/src/proxy/buzzer.cpp index 23ed2ae..d8cb974 100644 --- a/src/proxy/buzzer.cpp +++ b/src/proxy/buzzer.cpp @@ -6,7 +6,6 @@ * @date 03/2024 */ -#include "hal/mcu.hpp" #include "proxy/buzzer.hpp" namespace proxy { From a62c939a029ee365e4750a77961e1230d7fbc780 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 22 Mar 2024 01:00:13 -0300 Subject: [PATCH 065/561] =?UTF-8?q?=F0=9F=90=9B=20Make=20adc=20constants?= =?UTF-8?q?=20public?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index f5cc355..0a75f97 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -46,12 +46,6 @@ class AdcDma { */ void stop_dma(); - private: - /** - * @brief ADC DMA handle - */ - ADC_HandleTypeDef* handle; - /** * @brief Reference voltage for the ADC measurement */ @@ -61,6 +55,12 @@ class AdcDma { * @brief Maximum ADC reading */ static constexpr uint32_t max_reading{4095}; + + private: + /** + * @brief ADC DMA handle + */ + ADC_HandleTypeDef* handle; }; } From 2304e5ba8ce83b7bd540b85badd06405f0c9b36d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 22 Mar 2024 01:01:03 -0300 Subject: [PATCH 066/561] =?UTF-8?q?=E2=9C=A8=20Add=20battery=20monitoring?= =?UTF-8?q?=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/battery.hpp | 68 +++++++++++++++++++++++++++++++++++++++++++ src/proxy/battery.cpp | 23 +++++++++++++++ 2 files changed, 91 insertions(+) create mode 100644 inc/proxy/battery.hpp create mode 100644 src/proxy/battery.cpp diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp new file mode 100644 index 0000000..25f693e --- /dev/null +++ b/inc/proxy/battery.hpp @@ -0,0 +1,68 @@ +/** + * @file battery.hpp + * + * @brief Proxy Battery class declaration + * + * @date 03/2024 + */ + +#ifndef __BATTERY_HPP__ +#define __BATTERY_HPP__ + +#include + +#include "hal/adc_dma.hpp" + +namespace proxy { +/** + * @brief Class for getting the battery voltage + */ +class Battery { + public: + /** + * @brief Configuration structure for the battery + */ + struct Config { + hal::AdcDma::Config adc; + }; + + /** + * @brief Constructor for the Battery class + * + * @param battery_config Configuration for the battery + */ + Battery(Config& battery_config); + + /** + * @brief Get the battery voltage + * + * @return Battery voltage in volts + */ + float get_voltage(); + + /** + * @brief Get the raw reading from the battery + * + * @return uint32_t Raw reading from the battery + */ + uint32_t get_voltage_raw(); + + /** + * @brief Voltage divider ratio + */ + static constexpr float voltage_divider{3.0f}; + + private: + /** + * @brief Adc object + */ + hal::AdcDma adc; + + /** + * @brief Raw reading from the battery + */ + uint32_t raw_reading; +}; +} // namespace proxy + +#endif // __BATTERY_HPP__ diff --git a/src/proxy/battery.cpp b/src/proxy/battery.cpp new file mode 100644 index 0000000..ab183ec --- /dev/null +++ b/src/proxy/battery.cpp @@ -0,0 +1,23 @@ +/** + * @file battery.cpp + * + * @brief Proxy Battery class implementation + * + * @date 03/2024 + */ + +#include "proxy/battery.hpp" + +namespace proxy { +Battery::Battery(Config& battery_config) : adc{battery_config.adc} { + this->adc.start_dma(&(this->raw_reading), 1); +} + +float Battery::get_voltage() { + return Battery::voltage_divider * this->raw_reading * hal::AdcDma::reference_voltage / hal::AdcDma::max_reading; +} + +uint32_t Battery::get_voltage_raw() { + return this->raw_reading; +} +} // namespace proxy From c93ed7d63844fe52ff160229457b06b0bb8c30e6 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 22 Mar 2024 02:10:56 -0300 Subject: [PATCH 067/561] =?UTF-8?q?=F0=9F=92=A1=20Add=20unit=20to=20curren?= =?UTF-8?q?t=20sensor=20getter?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/current_sensors.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index a831521..506a3f4 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -40,7 +40,7 @@ class CurrentSensors { * @brief Get the current from the sensor * * @param sensor_index Index of the sensor - * @return float Current reading from the sensor + * @return float Current reading from the sensor in amps */ float get_current(uint8_t sensor_index) const; @@ -66,7 +66,7 @@ class CurrentSensors { /** * @brief Value of the shunt resistor in ohms */ - float shunt_resistor; + const float shunt_resistor; }; } // namespace proxy From 1645a2a0e5d1e3480ab45eabc3a4bd0b8d8c0505 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 22 Mar 2024 14:17:13 -0300 Subject: [PATCH 068/561] =?UTF-8?q?=F0=9F=A7=AA=20Add=20torque=20sensors?= =?UTF-8?q?=20proxy=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/torque_sensors.hpp | 68 ++++++++++++++++++++++++++++++++++++ src/proxy/torque_sensors.cpp | 31 ++++++++++++++++ 2 files changed, 99 insertions(+) create mode 100644 inc/proxy/torque_sensors.hpp create mode 100644 src/proxy/torque_sensors.cpp diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp new file mode 100644 index 0000000..0bc56d1 --- /dev/null +++ b/inc/proxy/torque_sensors.hpp @@ -0,0 +1,68 @@ +/** + * @file torque_sensors.hpp + * + * @brief Proxy TorqueSensors class header + * + * @date 03/2024 + */ + +#ifndef __TORQUE_SENSORS_HPP__ +#define __TORQUE_SENSORS_HPP__ + +#include + +#include "proxy/current_sensors.hpp" + +namespace proxy { +/** + * @brief Class for controlling TorqueSensors + */ +template +class TorqueSensors { + public: + /** + * @brief configuration structure for torque sensors + */ + struct Config { + CurrentSensors::Config current_sensor; + }; + + /** + * @brief Constructor for the TorqueSensors class + * + * @param torque_sensors_config Configuration for the torque sensors + */ + TorqueSensors(Config& torque_sensors_config); + + /** + * @brief Get the torque from the sensor + * + * @param sensor_index Index of the sensor + * @return float Torque reading from the sensor in N*m + */ + float get_torque(uint8_t sensor_index) const; + + /** + * @brief Get the current from the sensor + * + * @param sensor_index Index of the sensor + * @return float Current reading from the sensor in amps + */ + float get_current(uint8_t sensor_index) const; + + /** + * @brief Maximum torque that can be measured by the sensor + */ + static constexpr float max_torque = 50.0f; + + private: + /** + * @brief Current sensors object + */ + template CurrentSensors current_sensors; +}; +} // namespace proxy + +#include "../src/proxy/torque_sensors.cpp" + +#endif // __TORQUE_SENSORS_HPP__ diff --git a/src/proxy/torque_sensors.cpp b/src/proxy/torque_sensors.cpp new file mode 100644 index 0000000..d6e18ef --- /dev/null +++ b/src/proxy/torque_sensors.cpp @@ -0,0 +1,31 @@ +/** + * @file torque_sensors.cpp + * + * @brief Proxy TorqueSensors class implementation + * + * @date 03/2024 + */ + +#ifndef __TORQUE_SENSORS_CPP__ +#define __TORQUE_SENSORS_CPP__ + +#include "proxy/torque_sensors.hpp" + +namespace proxy { +template +TorqueSensors::TorqueSensors(Config& torque_sensors_config) : + current_sensors{torque_sensors_config.current_sensors} { +} + +template +float TorqueSensors::get_torque(uint8_t sensor_index) const { + return this->current_sensors.get_current_raw(sensor_index) * TorqueSensor::max_torque / hal::AdcDma::max_reading; +} + +template +float TorqueSensors::get_current(uint8_t sensor_index) const { + return this->current_sensors.get_current(sensor_index); +} +} // namespace proxy + +#endif // __TORQUE_SENSORS_CPP__ From ac319ef4fb703cd023cdab2bcce0fff40ffdaebb Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 22 Mar 2024 16:28:36 -0300 Subject: [PATCH 069/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20microsecond=20time?= =?UTF-8?q?r?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/hal/timer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index 6898579..f8fa917 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -15,7 +15,7 @@ Timer::Timer(Config& timer_config) : handle{timer_config.handle} { uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); uint32_t prescaler = this->handle->Instance->PSC; - if (base_freq / (prescaler + 1) == 1000000 and __HAL_TIM_GetAutoreload(this->handle) == 1000) { + if (base_freq / (prescaler + 1) == 1000000 and __HAL_TIM_GetAutoreload(this->handle) == 999) { this->enable_microseconds = true; } } From f8b5518fd82f73402f09fcfbd00133184d1185e5 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sat, 23 Mar 2024 00:40:13 -0300 Subject: [PATCH 070/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20template=20instant?= =?UTF-8?q?iation=20and=20variable=20naming=20in=20TorqueSensors=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/torque_sensors.hpp | 4 ++-- src/proxy/torque_sensors.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index 0bc56d1..ba648d1 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -24,7 +24,7 @@ class TorqueSensors { * @brief configuration structure for torque sensors */ struct Config { - CurrentSensors::Config current_sensor; + typename CurrentSensors::Config current_sensor; }; /** @@ -59,7 +59,7 @@ class TorqueSensors { /** * @brief Current sensors object */ - template CurrentSensors current_sensors; + CurrentSensors current_sensors; }; } // namespace proxy diff --git a/src/proxy/torque_sensors.cpp b/src/proxy/torque_sensors.cpp index d6e18ef..d220f3f 100644 --- a/src/proxy/torque_sensors.cpp +++ b/src/proxy/torque_sensors.cpp @@ -19,7 +19,7 @@ TorqueSensors::TorqueSensors(Config& torque_sensors_config) : template float TorqueSensors::get_torque(uint8_t sensor_index) const { - return this->current_sensors.get_current_raw(sensor_index) * TorqueSensor::max_torque / hal::AdcDma::max_reading; + return this->current_sensors.get_current_raw(sensor_index) * TorqueSensors::max_torque / hal::AdcDma::max_reading; } template From 0377a0700ca5a5e392a00cbacf36a427780a04e8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 23 Mar 2024 18:37:19 -0300 Subject: [PATCH 071/561] =?UTF-8?q?=F0=9F=92=A1=20Add=20comments=20to=20im?= =?UTF-8?q?u=20attributes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/imu.hpp | 6 ++++++ inc/proxy/torque_sensors.hpp | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 0153bd0..e447d26 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -80,8 +80,14 @@ class Imu { */ stmdev_ctx_t dev_ctx; + /** + * @brief Current angular velocity on each axis + */ std::array angular_velocity; + /** + * @brief Current linear acceleration on each axis + */ std::array linear_acceleration; }; } // namespace proxy diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index ba648d1..61d2915 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -24,7 +24,7 @@ class TorqueSensors { * @brief configuration structure for torque sensors */ struct Config { - typename CurrentSensors::Config current_sensor; + typename CurrentSensors::Config current_sensors; }; /** From f7a56ff945e12730fc59f541b6623c338d1d6c41 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 23 Mar 2024 18:38:53 -0300 Subject: [PATCH 072/561] =?UTF-8?q?=F0=9F=9A=A7=20=E2=9C=A8Add=20rotary=20?= =?UTF-8?q?sensor=20proxy=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/rotary_sensor.hpp | 64 +++++++++++++++++++++++++++++++++++++ src/proxy/rotary_sensor.cpp | 21 ++++++++++++ 2 files changed, 85 insertions(+) create mode 100644 inc/proxy/rotary_sensor.hpp create mode 100644 src/proxy/rotary_sensor.cpp diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp new file mode 100644 index 0000000..dd9b878 --- /dev/null +++ b/inc/proxy/rotary_sensor.hpp @@ -0,0 +1,64 @@ +/** + * @file rotary_sensor.hpp + * + * @brief STM32 rotary sensor HAL wrapper + * + * @date 03/2024 + */ + +#ifndef __ROTARY_SENSOR_HPP__ +#define __ROTARY_SENSOR_HPP__ + +#include + +#include "hal/encoder.hpp" +#include "hal/spi.hpp" + +namespace proxy { +/** + * @brief Class to handle rotary sensor peripheral on STM32 microcontrollers + */ +class RotarySensor { + public: + /** + * @brief rotary sensor configuration struct + */ + struct Config { + hal::Spi::Config spi; + hal::Encoder::Config encoder; + uint32_t resolution; + }; + + /** + * @brief Construct a new RotarySensor object + * + * @param rotary_sensor_config Configuration for the rotary sensor + */ + RotarySensor(Config& rotary_sensor_config); + + /** + * @brief Get the rotary sensor position over an axis + * + * @return Current angular position of the sensor in radians + */ + float get_position(); + + private: + /** + * @brief SPI for the rotary sensor configuration + */ + hal::Spi spi; + + /** + * @brief Encoder for getting the rotary sensor data + */ + hal::Encoder encoder; + + /** + * @brief Resolution of the rotary sensor + */ + uint32_t resolution; +}; +} // namespace proxy + +#endif // __ROTARY_SENSOR_HPP__ diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp new file mode 100644 index 0000000..c333079 --- /dev/null +++ b/src/proxy/rotary_sensor.cpp @@ -0,0 +1,21 @@ +/** + * @file rotary_sensor.cpp + * + * @brief Proxy RotarySensor class source + * + * @date 03/2024 + */ + +#include + +#include "proxy/rotary_sensor.hpp" + +namespace proxy { +RotarySensor::RotarySensor(Config& rotary_sensor_config) : + spi{rotary_sensor_config.spi}, encoder{rotary_sensor_config.encoder} { +} + +float RotarySensor::get_position() { + return 2 * std::numbers::pi_v * encoder.get_counter() * / this->resolution; +} +} // namespace proxy From f75bbf378f517e63aafaca3499d6b958155911c5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 15:12:15 -0300 Subject: [PATCH 073/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20DMA=20from=20SP?= =?UTF-8?q?I?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/spi.hpp | 6 ++++++ src/hal/spi.cpp | 6 +++--- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index d88aec6..5678db5 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -27,6 +27,7 @@ class Spi { SPI_HandleTypeDef* handle; void (* init_function)(void); hal::Gpio::Config gpio; + uint32_t timeout; }; /** @@ -72,6 +73,11 @@ class Spi { * @brief GPIO for the chip select pin */ hal::Gpio gpio; + + /** + * @brief Timeout for the SPI operations in ms + */ + uint32_t timeout; }; } // namespace hal diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index 3d751c0..839f828 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -9,7 +9,7 @@ #include "hal/spi.hpp" namespace hal { -Spi::Spi(Config& spi_config) : handle{spi_config.handle}, gpio{spi_config.gpio} { +Spi::Spi(Config& spi_config) : handle{spi_config.handle}, gpio{spi_config.gpio}, timeout{spi_config.timeout} { } void Spi::select_device() { @@ -25,10 +25,10 @@ void Spi::unselect_device() { } void Spi::transmit(uint8_t data[], uint32_t size) { - HAL_SPI_Transmit_DMA(this->handle, data, size); + HAL_SPI_Transmit(this->handle, data, size, this->timeout); } void Spi::receive(uint8_t data[], uint32_t size) { - HAL_SPI_Receive_DMA(this->handle, data, size); + HAL_SPI_Receive(this->handle, data, size, this->timeout); } } // namespace proxy From 093cab8e7e64b31d8ce0b30920a3321028711708 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 18:10:40 -0300 Subject: [PATCH 074/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20DMA=20init=20to?= =?UTF-8?q?=20mcu?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 4 ++-- inc/hal/mcu.hpp | 18 +++++++++--------- src/hal/adc_dma.cpp | 2 -- src/hal/mcu.cpp | 2 ++ 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index 0a75f97..c501587 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -14,7 +14,7 @@ namespace hal { /** - * @brief Class to handle ADC DMA peripheral on STM32 microcontrollers + * @brief Class to handle ADC peripheral on STM32 microcontrollers using DMA */ class AdcDma { public: @@ -58,7 +58,7 @@ class AdcDma { private: /** - * @brief ADC DMA handle + * @brief ADC handle */ ADC_HandleTypeDef* handle; }; diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index af45afc..9aa36c5 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -11,15 +11,6 @@ #include -extern "C" -{ -/** - * @brief Initializes System Clock - * @note Defined by cube - */ -void SystemClock_Config(void); -} - namespace hal { /** * @brief Microcontroller unit class @@ -32,4 +23,13 @@ class Mcu { static void init(void); }; }; // namespace hal + +extern "C" +{ +/** + * @brief Initializes System Clock + * @note Defined by cube + */ +void SystemClock_Config(void); +} #endif // __MCU_HPP__ diff --git a/src/hal/adc_dma.cpp b/src/hal/adc_dma.cpp index 4b10241..06d8b72 100644 --- a/src/hal/adc_dma.cpp +++ b/src/hal/adc_dma.cpp @@ -6,12 +6,10 @@ * @date 03/2024 */ -#include "dma.h" #include "hal/adc_dma.hpp" namespace hal { AdcDma::AdcDma(Config& adc_config) : handle{adc_config.handle} { - MX_DMA_Init(); adc_config.init_function(); } diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index a1b5de3..7592b39 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -4,6 +4,7 @@ * @brief MCU related */ +#include #include #include @@ -16,5 +17,6 @@ void Mcu::init(void) { SystemClock_Config(); MX_GPIO_Init(); + MX_DMA_Init(); } } From f90ca466cb2235e0bc727da513c9656f10cd4cd3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 18:11:38 -0300 Subject: [PATCH 075/561] =?UTF-8?q?=E2=9C=A8=20Add=20PWM=20HAL=20with=20DM?= =?UTF-8?q?A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/pwm_dma.hpp | 70 +++++++++++++++++++++++++++++++++++++++++++++ inc/hal/timer.hpp | 8 ++++++ src/hal/pwm_dma.cpp | 23 +++++++++++++++ src/hal/timer.cpp | 25 ++++++++++++++++ 4 files changed, 126 insertions(+) create mode 100644 inc/hal/pwm_dma.hpp create mode 100644 src/hal/pwm_dma.cpp diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp new file mode 100644 index 0000000..14598ee --- /dev/null +++ b/inc/hal/pwm_dma.hpp @@ -0,0 +1,70 @@ +/** + * @file pwm_dma.hpp + * + * @brief STM32 PWM DMA HAL wrapper + * + * @date 03/2024 + */ + +#ifndef __PWM_DMA_HPP__ +#define __PWM_DMA_HPP__ + +#include +#include + +#include "hal/timer.hpp" + +namespace hal { +/** + * @brief Class to handle PWM peripheral on STM32 microcontrollers using DMA + */ +class PwmDma { + public: + /** + * @brief PWM configuration struct + */ + struct Config { + TIM_HandleTypeDef* handle; + void (* init_function)(void); + uint32_t timer_channel; + }; + + /** + * @brief Construct a new PwmDma object + * + * @param pwm_config Configuration for the PWM + */ + PwmDma(Config& pwm_config); + + /** + * @brief Start PWM and DMA transfer + * + * @param buffer Buffer to transfer + * @param size Size of the buffer + */ + void start_dma(uint32_t buffer[], uint32_t size); + + /** + * @brief Stop PWM and DMA transfer + */ + void stop_dma(); + + private: + /** + * @brief Timer handle + */ + TIM_HandleTypeDef* handle; + + /** + * @brief Channel number of the timer + */ + uint32_t channel; +}; +} // namespace hal + +extern "C" { +void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef* htim) { + HAL_TIM_PWM_Stop_DMA(htim, hal::Timer::get_channel_from_active(HAL_TIM_GetActiveChannel(htim))); +} +} +#endif // __PWM_DMA_HPP__ diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index a1c7eef..22a06bb 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -76,6 +76,14 @@ class Timer { */ void sleep_us(uint32_t time) const; + /** + * @brief Get the channel number from the active channel flag + * + * @param active_channel Active channel flag + * @return uint32_t Channel number + */ + static constexpr uint32_t get_channel_from_active(HAL_TIM_ActiveChannel active_channel); + private: /** * @brief Get the current timer counter diff --git a/src/hal/pwm_dma.cpp b/src/hal/pwm_dma.cpp new file mode 100644 index 0000000..c2aeac6 --- /dev/null +++ b/src/hal/pwm_dma.cpp @@ -0,0 +1,23 @@ +/** + * @file pwm_dma.cpp + * + * @brief STM32 PWM DMA HAL wrapper + * + * @date 03/2024 + */ + +#include "hal/pwm_dma.hpp" + +namespace hal { +PwmDma::PwmDma(Config& pwm_config) : handle{pwm_config.handle}, channel{pwm_config.timer_channel} { + pwm_config.init_function(); +} + +void PwmDma::start_dma(uint32_t buffer[], uint32_t size) { + HAL_TIM_PWM_Start_DMA(this->handle, this->channel, buffer, size); +} + +void PwmDma::stop_dma() { + HAL_TIM_PWM_Stop_DMA(this->handle, this->channel); +} +} // namespace hal diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index f8fa917..a0e8c8e 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -63,4 +63,29 @@ uint32_t Timer::get_counter_us() const { return 1000 * HAL_GetTick(); } + +constexpr uint32_t Timer::get_channel_from_active(HAL_TIM_ActiveChannel active_channel) { + switch (active_channel) { + case HAL_TIM_ACTIVE_CHANNEL_1: + return TIM_CHANNEL_1; + + case HAL_TIM_ACTIVE_CHANNEL_2: + return TIM_CHANNEL_2; + + case HAL_TIM_ACTIVE_CHANNEL_3: + return TIM_CHANNEL_3; + + case HAL_TIM_ACTIVE_CHANNEL_4: + return TIM_CHANNEL_4; + + case HAL_TIM_ACTIVE_CHANNEL_5: + return TIM_CHANNEL_5; + + case HAL_TIM_ACTIVE_CHANNEL_6: + return TIM_CHANNEL_6; + + default: + return TIM_CHANNEL_ALL; + } +} } // namespace hal From 9e8ee130f65b50331aa3d92a87e2d29329d7d429 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 18:12:29 -0300 Subject: [PATCH 076/561] =?UTF-8?q?=F0=9F=A5=85=20Remove=20extra=20*=20in?= =?UTF-8?q?=20rotary=20sensor=20get=20position?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/rotary_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index c333079..97eacf5 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -16,6 +16,6 @@ RotarySensor::RotarySensor(Config& rotary_sensor_config) : } float RotarySensor::get_position() { - return 2 * std::numbers::pi_v * encoder.get_counter() * / this->resolution; + return 2 * std::numbers::pi_v * encoder.get_counter() / this->resolution; } } // namespace proxy From 5400976a60df83c7479f4837bfa88ec3d16731f7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 20:36:29 -0300 Subject: [PATCH 077/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20DMA=20callback?= =?UTF-8?q?=20function=20from=20timer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/pwm_dma.hpp | 8 -------- inc/hal/timer.hpp | 8 -------- src/hal/timer.cpp | 25 ------------------------- 3 files changed, 41 deletions(-) diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp index 14598ee..85443c0 100644 --- a/inc/hal/pwm_dma.hpp +++ b/inc/hal/pwm_dma.hpp @@ -12,8 +12,6 @@ #include #include -#include "hal/timer.hpp" - namespace hal { /** * @brief Class to handle PWM peripheral on STM32 microcontrollers using DMA @@ -61,10 +59,4 @@ class PwmDma { uint32_t channel; }; } // namespace hal - -extern "C" { -void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef* htim) { - HAL_TIM_PWM_Stop_DMA(htim, hal::Timer::get_channel_from_active(HAL_TIM_GetActiveChannel(htim))); -} -} #endif // __PWM_DMA_HPP__ diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index 22a06bb..a1c7eef 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -76,14 +76,6 @@ class Timer { */ void sleep_us(uint32_t time) const; - /** - * @brief Get the channel number from the active channel flag - * - * @param active_channel Active channel flag - * @return uint32_t Channel number - */ - static constexpr uint32_t get_channel_from_active(HAL_TIM_ActiveChannel active_channel); - private: /** * @brief Get the current timer counter diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index a0e8c8e..f8fa917 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -63,29 +63,4 @@ uint32_t Timer::get_counter_us() const { return 1000 * HAL_GetTick(); } - -constexpr uint32_t Timer::get_channel_from_active(HAL_TIM_ActiveChannel active_channel) { - switch (active_channel) { - case HAL_TIM_ACTIVE_CHANNEL_1: - return TIM_CHANNEL_1; - - case HAL_TIM_ACTIVE_CHANNEL_2: - return TIM_CHANNEL_2; - - case HAL_TIM_ACTIVE_CHANNEL_3: - return TIM_CHANNEL_3; - - case HAL_TIM_ACTIVE_CHANNEL_4: - return TIM_CHANNEL_4; - - case HAL_TIM_ACTIVE_CHANNEL_5: - return TIM_CHANNEL_5; - - case HAL_TIM_ACTIVE_CHANNEL_6: - return TIM_CHANNEL_6; - - default: - return TIM_CHANNEL_ALL; - } -} } // namespace hal From 5fa19fee627499bf03376020873da331afb40da0 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 20:39:23 -0300 Subject: [PATCH 078/561] =?UTF-8?q?=E2=9C=A8=20Add=20get=5Fcompare=20funct?= =?UTF-8?q?ion=20to=20pwm?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/pwm_dma.hpp | 8 ++++++++ src/hal/pwm.cpp | 4 +++- src/hal/pwm_dma.cpp | 6 ++++++ 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp index 85443c0..c40334d 100644 --- a/inc/hal/pwm_dma.hpp +++ b/inc/hal/pwm_dma.hpp @@ -47,6 +47,14 @@ class PwmDma { */ void stop_dma(); + /** + * @brief Get the compare value for ad duty cycle + * + * @param duty_cycle Duty cycle to get the compare value for + * @return uint32_t Compare value for the duty cycle + */ + uint32_t get_compare(float duty_cycle); + private: /** * @brief Timer handle diff --git a/src/hal/pwm.cpp b/src/hal/pwm.cpp index 299249d..d855a89 100644 --- a/src/hal/pwm.cpp +++ b/src/hal/pwm.cpp @@ -6,6 +6,8 @@ * @date 03/2024 */ +#include + #include "hal/pwm.hpp" namespace hal { @@ -16,7 +18,7 @@ Pwm::Pwm(Config& pwm_config) : handle{pwm_config.handle}, channel{pwm_config.tim } void Pwm::set_duty_cycle(float duty_cycle) { - uint32_t compare = (duty_cycle * __HAL_TIM_GET_AUTORELOAD(this->handle)) / 100; + uint32_t compare = std::round((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); } diff --git a/src/hal/pwm_dma.cpp b/src/hal/pwm_dma.cpp index c2aeac6..3e4e95f 100644 --- a/src/hal/pwm_dma.cpp +++ b/src/hal/pwm_dma.cpp @@ -6,6 +6,8 @@ * @date 03/2024 */ +#include + #include "hal/pwm_dma.hpp" namespace hal { @@ -20,4 +22,8 @@ void PwmDma::start_dma(uint32_t buffer[], uint32_t size) { void PwmDma::stop_dma() { HAL_TIM_PWM_Stop_DMA(this->handle, this->channel); } + +uint32_t PwmDma::get_compare(float duty_cycle) { + return std::round((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); +} } // namespace hal From aeef78b303ee25658e2e9756c6a54ee4a38caa10 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 20:40:00 -0300 Subject: [PATCH 079/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20SystemClock=5FCon?= =?UTF-8?q?fig=20to=20mcu.cpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/mcu.hpp | 9 --------- src/hal/mcu.cpp | 8 ++++++++ 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index 9aa36c5..8dcf203 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -23,13 +23,4 @@ class Mcu { static void init(void); }; }; // namespace hal - -extern "C" -{ -/** - * @brief Initializes System Clock - * @note Defined by cube - */ -void SystemClock_Config(void); -} #endif // __MCU_HPP__ diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index 7592b39..8985c6d 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -10,6 +10,14 @@ #include "hal/mcu.hpp" +extern "C" { +/** + * @brief Initializes System Clock + * @note Defined by cube + */ +void SystemClock_Config(void); +} + namespace hal { void Mcu::init(void) { HAL_Init(); From e77b4451ea219415aa734694a985f664fcc4fd86 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 20:40:59 -0300 Subject: [PATCH 080/561] =?UTF-8?q?=F0=9F=94=92=EF=B8=8F=20Make=20battery?= =?UTF-8?q?=20voltage=20divider=20and=20max=20torque=20private?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/battery.hpp | 10 +++++----- inc/proxy/torque_sensors.hpp | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp index 25f693e..acff6ce 100644 --- a/inc/proxy/battery.hpp +++ b/inc/proxy/battery.hpp @@ -47,11 +47,6 @@ class Battery { */ uint32_t get_voltage_raw(); - /** - * @brief Voltage divider ratio - */ - static constexpr float voltage_divider{3.0f}; - private: /** * @brief Adc object @@ -62,6 +57,11 @@ class Battery { * @brief Raw reading from the battery */ uint32_t raw_reading; + + /** + * @brief Voltage divider ratio + */ + static constexpr float voltage_divider{3.0f}; }; } // namespace proxy diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index 61d2915..04d484c 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -50,16 +50,16 @@ class TorqueSensors { */ float get_current(uint8_t sensor_index) const; - /** - * @brief Maximum torque that can be measured by the sensor - */ - static constexpr float max_torque = 50.0f; - private: /** * @brief Current sensors object */ CurrentSensors current_sensors; + + /** + * @brief Maximum torque that can be measured by the sensor + */ + static constexpr float max_torque = 50.0f; }; } // namespace proxy From ea739c2e8c4124d152e7efd0f60c86ac31a992ab Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 21:26:03 -0300 Subject: [PATCH 081/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20constants=20to=20?= =?UTF-8?q?config?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 18 ++++++++++-------- inc/hal/encoder.hpp | 4 ++-- inc/hal/gpio.hpp | 4 ++-- inc/hal/pwm.hpp | 4 ++-- inc/hal/pwm_dma.hpp | 4 ++-- inc/hal/spi.hpp | 4 ++-- inc/hal/timer.hpp | 6 +++--- inc/proxy/battery.hpp | 7 ++++--- inc/proxy/button.hpp | 4 ++-- inc/proxy/buzzer.hpp | 8 ++++---- inc/proxy/current_sensors.hpp | 4 ++-- inc/proxy/dip_switch.hpp | 4 ++-- inc/proxy/distance_sensors.hpp | 7 ++++--- inc/proxy/fan.hpp | 6 +++--- inc/proxy/imu.hpp | 4 ++-- inc/proxy/led.hpp | 4 ++-- inc/proxy/locomotion.hpp | 6 +++--- inc/proxy/rotary_sensor.hpp | 4 ++-- inc/proxy/torque_sensors.hpp | 7 ++++--- src/hal/adc_dma.cpp | 4 ++-- src/hal/encoder.cpp | 6 +++--- src/hal/gpio.cpp | 2 +- src/hal/pwm.cpp | 4 ++-- src/hal/pwm_dma.cpp | 4 ++-- src/hal/spi.cpp | 2 +- src/hal/timer.cpp | 4 ++-- src/proxy/battery.cpp | 4 ++-- src/proxy/button.cpp | 12 ++++++------ src/proxy/buzzer.cpp | 4 ++-- src/proxy/current_sensors.cpp | 3 +-- src/proxy/dip_switch.cpp | 4 ++-- src/proxy/distance_sensors.cpp | 6 +++--- src/proxy/fan.cpp | 5 +---- src/proxy/imu.cpp | 4 ++-- src/proxy/led.cpp | 2 +- src/proxy/locomotion.cpp | 10 ++++------ src/proxy/rotary_sensor.cpp | 5 ++--- src/proxy/torque_sensors.cpp | 4 ++-- 38 files changed, 98 insertions(+), 100 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index c501587..bbb22f2 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -24,14 +24,16 @@ class AdcDma { struct Config { ADC_HandleTypeDef* handle; void (* init_function)(void); + uint32_t max_reading; + float reference_voltage; }; /** * @brief Construct a new AdcDma object * - * @param adc_config ADC DMA configuration struct + * @param config ADC DMA configuration struct */ - AdcDma(Config& adc_config); + AdcDma(Config& config); /** * @brief Enable ADC, start conversion of regular group and transfer result through DMA @@ -46,21 +48,21 @@ class AdcDma { */ void stop_dma(); + private: /** - * @brief Reference voltage for the ADC measurement + * @brief ADC handle */ - static constexpr float reference_voltage{3.3f}; + ADC_HandleTypeDef* handle; /** * @brief Maximum ADC reading */ - static constexpr uint32_t max_reading{4095}; + const uint32_t max_reading; - private: /** - * @brief ADC handle + * @brief Reference voltage for the ADC measurement */ - ADC_HandleTypeDef* handle; + const float reference_voltage; }; } diff --git a/inc/hal/encoder.hpp b/inc/hal/encoder.hpp index b14fd50..16724e7 100644 --- a/inc/hal/encoder.hpp +++ b/inc/hal/encoder.hpp @@ -29,9 +29,9 @@ class Encoder { /** * @brief Construct a new Encoder object * - * @param encoder_config Configuration for the encoder + * @param config Configuration for the encoder */ - Encoder(Config& encoder_config); + Encoder(Config& config); /** * @brief Get the counter value diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index 6cd34ad..475f3be 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -29,9 +29,9 @@ class Gpio { /** * @brief Constructor for the Gpio class * - * @param gpio_config Configuration for the GPIO pin + * @param config Configuration for the GPIO pin */ - Gpio(Config& gpio_config); + Gpio(Config& config); /** * @brief Read the current state of the GPIO pin diff --git a/inc/hal/pwm.hpp b/inc/hal/pwm.hpp index e6d5359..037a6de 100644 --- a/inc/hal/pwm.hpp +++ b/inc/hal/pwm.hpp @@ -30,9 +30,9 @@ class Pwm { /** * @brief Construct a new Pwm object * - * @param pwm_config Configuration for the PWM + * @param config Configuration for the PWM */ - Pwm(Config& pwm_config); + Pwm(Config& config); /** * @brief Set the PWM duty cycle diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp index c40334d..9166aa2 100644 --- a/inc/hal/pwm_dma.hpp +++ b/inc/hal/pwm_dma.hpp @@ -30,9 +30,9 @@ class PwmDma { /** * @brief Construct a new PwmDma object * - * @param pwm_config Configuration for the PWM + * @param config Configuration for the PWM */ - PwmDma(Config& pwm_config); + PwmDma(Config& config); /** * @brief Start PWM and DMA transfer diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index 5678db5..c35fd77 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -33,9 +33,9 @@ class Spi { /** * @brief Construct a new Spi object * - * @param spi_config Configuration for the SPI + * @param config Configuration for the SPI */ - Spi(Config& spi_config); + Spi(Config& config); /** * @brief Activate the chip select diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index a1c7eef..29c3ac0 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -34,9 +34,9 @@ class Timer { /** * @brief Construct a new Timer object * - * @param timer_config Configuration for the timer + * @param config Configuration for the timer */ - Timer(Config& timer_config); + Timer(Config& config); /** * @brief Reset the timer counter in milliseconds @@ -104,7 +104,7 @@ class Timer { /** * @brief Flag to enable microseconds */ - bool enable_microseconds = false; + bool enable_microseconds{false}; }; } // namespace hal diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp index acff6ce..7050611 100644 --- a/inc/proxy/battery.hpp +++ b/inc/proxy/battery.hpp @@ -24,14 +24,15 @@ class Battery { */ struct Config { hal::AdcDma::Config adc; + float voltage_divider; }; /** * @brief Constructor for the Battery class * - * @param battery_config Configuration for the battery + * @param config Configuration for the battery */ - Battery(Config& battery_config); + Battery(Config& config); /** * @brief Get the battery voltage @@ -61,7 +62,7 @@ class Battery { /** * @brief Voltage divider ratio */ - static constexpr float voltage_divider{3.0f}; + const float voltage_divider; }; } // namespace proxy diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index a574007..7c1f17d 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -52,9 +52,9 @@ class Button { /** * @brief Constructor for Button class * - * @param button_config Button configuration + * @param config Button configuration */ - Button(Config& button_config); + Button(Config& config); /** * @brief Check if button is pressed diff --git a/inc/proxy/buzzer.hpp b/inc/proxy/buzzer.hpp index 13c2496..bea453b 100644 --- a/inc/proxy/buzzer.hpp +++ b/inc/proxy/buzzer.hpp @@ -30,9 +30,9 @@ class Buzzer { /** * @brief Constructor for the Buzzer class * - * @param buzzer_config Configuration for the buzzer + * @param config Configuration for the buzzer */ - Buzzer(Config& buzzer_config); + Buzzer(Config& config); /** * @brief Play a tone for a duration @@ -66,12 +66,12 @@ class Buzzer { /** * @brief Flag to check if the buzzer is playing */ - bool is_playing = false; + bool is_playing{false}; /** * @brief Duration of the sound */ - uint32_t duration = 0; + uint32_t duration{0}; }; } // namespace proxy diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index 506a3f4..d7d5c71 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -32,9 +32,9 @@ class CurrentSensors { /** * @brief Constructor for the CurrentSensors class * - * @param current_sensors_config Configuration for the current sensors + * @param config Configuration for the current sensors */ - CurrentSensors(Config& current_sensors_config); + CurrentSensors(Config& config); /** * @brief Get the current from the sensor diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp index 5667f8e..a11d93f 100644 --- a/inc/proxy/dip_switch.hpp +++ b/inc/proxy/dip_switch.hpp @@ -31,9 +31,9 @@ class DipSwitch { /** * @brief Construct a new Dip Switch object * - * @param dip_switch_config Configuration struct for DipSwitch + * @param config Configuration struct for DipSwitch */ - DipSwitch(Config& dip_switch_config); + DipSwitch(Config& config); /** * @brief Get the state of a switch diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index 8d5fca2..e73f0a5 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -28,14 +28,15 @@ class DistanceSensors { struct Config { hal::AdcDma::Config adc; hal::Pwm::Config led_pwm; + float max_distance; }; /** * @brief Constructor for the DistanceSensors class * - * @param distance_sensor_config Configuration for the distance sensors + * @param config Configuration for the distance sensors */ - DistanceSensors(Config& distance_sensor_config); + DistanceSensors(Config& config); /** * @brief Set the distance sensors led intensity @@ -79,7 +80,7 @@ class DistanceSensors { /** * @brief Maximum distance reading in meters */ - static constexpr float max_distance{0.3f}; + const float max_distance; }; } // namespace proxy diff --git a/inc/proxy/fan.hpp b/inc/proxy/fan.hpp index 430e27c..82bc747 100644 --- a/inc/proxy/fan.hpp +++ b/inc/proxy/fan.hpp @@ -16,7 +16,7 @@ namespace proxy { /** - * @brief Class for controlling the fan + * @brief Class for controlling the fan driver */ class Fan { public: @@ -40,9 +40,9 @@ class Fan { /** * @brief Construct a new fan object * - * @param fan_config + * @param config Configuration for the fan driver */ - Fan(Config& fan_config); + Fan(Config& config); /** * @brief Enable the fan diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index e447d26..65602a9 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -31,9 +31,9 @@ class Imu { /** * @brief Construct a new Imu object * - * @param imu_config Configuration for the IMU + * @param config Configuration for the IMU */ - Imu(Config& imu_config); + Imu(Config& config); enum Axis { W, diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp index 543a62d..8861722 100644 --- a/inc/proxy/led.hpp +++ b/inc/proxy/led.hpp @@ -27,9 +27,9 @@ class Led { /** * @brief Constructor for the Led class * - * @param led_config Configuration for the LED + * @param config Configuration for the LED */ - Led(Config& led_config); + Led(Config& config); /** * @brief Turn the LED on diff --git a/inc/proxy/locomotion.hpp b/inc/proxy/locomotion.hpp index ffd03ad..7e2fc5b 100644 --- a/inc/proxy/locomotion.hpp +++ b/inc/proxy/locomotion.hpp @@ -16,7 +16,7 @@ namespace proxy { /** - * @brief Class for controlling the locomotion + * @brief Class for controlling the locomotion driver */ class Locomotion { public: @@ -34,9 +34,9 @@ class Locomotion { /** * @brief Construct a new locomotion object * - * @param locomotion_config + * @param config Configuration for the locomotion driver */ - Locomotion(Config& locomotion_config); + Locomotion(Config& config); /** * @brief Enable the motor driver diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index dd9b878..4829853 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -32,9 +32,9 @@ class RotarySensor { /** * @brief Construct a new RotarySensor object * - * @param rotary_sensor_config Configuration for the rotary sensor + * @param config Configuration for the rotary sensor */ - RotarySensor(Config& rotary_sensor_config); + RotarySensor(Config& config); /** * @brief Get the rotary sensor position over an axis diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index 04d484c..b9ae80c 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -25,14 +25,15 @@ class TorqueSensors { */ struct Config { typename CurrentSensors::Config current_sensors; + float max_torque; }; /** * @brief Constructor for the TorqueSensors class * - * @param torque_sensors_config Configuration for the torque sensors + * @param config Configuration for the torque sensors */ - TorqueSensors(Config& torque_sensors_config); + TorqueSensors(Config& config); /** * @brief Get the torque from the sensor @@ -59,7 +60,7 @@ class TorqueSensors { /** * @brief Maximum torque that can be measured by the sensor */ - static constexpr float max_torque = 50.0f; + const float max_torque; }; } // namespace proxy diff --git a/src/hal/adc_dma.cpp b/src/hal/adc_dma.cpp index 06d8b72..c991bfc 100644 --- a/src/hal/adc_dma.cpp +++ b/src/hal/adc_dma.cpp @@ -9,8 +9,8 @@ #include "hal/adc_dma.hpp" namespace hal { -AdcDma::AdcDma(Config& adc_config) : handle{adc_config.handle} { - adc_config.init_function(); +AdcDma::AdcDma(Config& config) : handle{config.handle} { + config.init_function(); } void AdcDma::start_dma(uint32_t buffer[], uint32_t size) { diff --git a/src/hal/encoder.cpp b/src/hal/encoder.cpp index b21dc60..1f32d33 100644 --- a/src/hal/encoder.cpp +++ b/src/hal/encoder.cpp @@ -9,9 +9,9 @@ #include "hal/encoder.hpp" namespace hal { -Encoder::Encoder(Config& encoder_config) : handle{encoder_config.handle} { - encoder_config.init_function(); - HAL_TIM_Encoder_Start(this->handle, encoder_config.timer_channel); +Encoder::Encoder(Config& config) : handle{config.handle} { + config.init_function(); + HAL_TIM_Encoder_Start(this->handle, config.timer_channel); } int32_t Encoder::get_counter() { diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index 747bbda..92c12cb 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -9,7 +9,7 @@ #include "hal/gpio.hpp" namespace hal { -Gpio::Gpio(Config& gpio_config) : port{gpio_config.port}, pin{gpio_config.pin} { +Gpio::Gpio(Config& config) : port{config.port}, pin{config.pin} { } bool Gpio::read(void) const { diff --git a/src/hal/pwm.cpp b/src/hal/pwm.cpp index d855a89..7a975a0 100644 --- a/src/hal/pwm.cpp +++ b/src/hal/pwm.cpp @@ -11,8 +11,8 @@ #include "hal/pwm.hpp" namespace hal { -Pwm::Pwm(Config& pwm_config) : handle{pwm_config.handle}, channel{pwm_config.timer_channel} { - pwm_config.init_function(); +Pwm::Pwm(Config& config) : handle{config.handle}, channel{config.timer_channel} { + config.init_function(); HAL_TIM_PWM_Start(this->handle, this->channel); __HAL_TIM_SET_COMPARE(this->handle, this->channel, 0); } diff --git a/src/hal/pwm_dma.cpp b/src/hal/pwm_dma.cpp index 3e4e95f..57acb44 100644 --- a/src/hal/pwm_dma.cpp +++ b/src/hal/pwm_dma.cpp @@ -11,8 +11,8 @@ #include "hal/pwm_dma.hpp" namespace hal { -PwmDma::PwmDma(Config& pwm_config) : handle{pwm_config.handle}, channel{pwm_config.timer_channel} { - pwm_config.init_function(); +PwmDma::PwmDma(Config& config) : handle{config.handle}, channel{config.timer_channel} { + config.init_function(); } void PwmDma::start_dma(uint32_t buffer[], uint32_t size) { diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index 839f828..ee46154 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -9,7 +9,7 @@ #include "hal/spi.hpp" namespace hal { -Spi::Spi(Config& spi_config) : handle{spi_config.handle}, gpio{spi_config.gpio}, timeout{spi_config.timeout} { +Spi::Spi(Config& config) : handle{config.handle}, gpio{config.gpio}, timeout{config.timeout} { } void Spi::select_device() { diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index f8fa917..6e17eb9 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -9,8 +9,8 @@ #include "hal/timer.hpp" namespace hal { -Timer::Timer(Config& timer_config) : handle{timer_config.handle} { - timer_config.init_function(); +Timer::Timer(Config& config) : handle{config.handle} { + config.init_function(); uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); uint32_t prescaler = this->handle->Instance->PSC; diff --git a/src/proxy/battery.cpp b/src/proxy/battery.cpp index ab183ec..6702c63 100644 --- a/src/proxy/battery.cpp +++ b/src/proxy/battery.cpp @@ -9,12 +9,12 @@ #include "proxy/battery.hpp" namespace proxy { -Battery::Battery(Config& battery_config) : adc{battery_config.adc} { +Battery::Battery(Config& config) : adc{config.adc}, voltage_divider{config.voltage_divider} { this->adc.start_dma(&(this->raw_reading), 1); } float Battery::get_voltage() { - return Battery::voltage_divider * this->raw_reading * hal::AdcDma::reference_voltage / hal::AdcDma::max_reading; + return this->voltage_divider * this->raw_reading * hal::AdcDma::reference_voltage / hal::AdcDma::max_reading; } uint32_t Battery::get_voltage_raw() { diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index 15a8e25..82502f0 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -9,12 +9,12 @@ #include "proxy/button.hpp" namespace proxy { -Button::Button(Config& button_config) : - debounce_delay{button_config.debounce_delay}, - long_press_delay{button_config.long_press_delay}, - extra_long_press_delay{button_config.extra_long_press_delay}, - gpio{button_config.gpio}, - pull_resistor{button_config.pull_resistor} { +Button::Button(Config& config) : + debounce_delay{config.debounce_delay}, + long_press_delay{config.long_press_delay}, + extra_long_press_delay{config.extra_long_press_delay}, + gpio{config.gpio}, + pull_resistor{config.pull_resistor} { } bool Button::is_pressed() { diff --git a/src/proxy/buzzer.cpp b/src/proxy/buzzer.cpp index d8cb974..1555d13 100644 --- a/src/proxy/buzzer.cpp +++ b/src/proxy/buzzer.cpp @@ -9,7 +9,7 @@ #include "proxy/buzzer.hpp" namespace proxy { -Buzzer::Buzzer(Config& buzzer_config) : pwm{buzzer_config.pwm} { +Buzzer::Buzzer(Config& config) : pwm{config.pwm} { this->stop(); } @@ -25,7 +25,7 @@ void Buzzer::play(uint32_t frequency, uint32_t duration) { } void Buzzer::update() { - if (this->is_playing and this->timer.elapsed_time_ms() > this->duration) { + if (this->is_playing and this->duration > 0 and this->timer.elapsed_time_ms() > this->duration) { this->stop(); } } diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp index e66e1a9..5d6c10d 100644 --- a/src/proxy/current_sensors.cpp +++ b/src/proxy/current_sensors.cpp @@ -13,8 +13,7 @@ namespace proxy { template -CurrentSensors::CurrentSensors(Config& current_sensors_config) : - adc{current_sensors_config.adc}, shunt_resistor{current_sensors_config.shunt_resistor} { +CurrentSensors::CurrentSensors(Config& config) : adc{config.adc}, shunt_resistor{config.shunt_resistor} { this->adc.start_dma(this->buffer.data(), num_of_sensors); } diff --git a/src/proxy/dip_switch.cpp b/src/proxy/dip_switch.cpp index 9f1b37c..97d9da5 100644 --- a/src/proxy/dip_switch.cpp +++ b/src/proxy/dip_switch.cpp @@ -14,9 +14,9 @@ namespace proxy { template -DipSwitch::DipSwitch(Config& dip_switch_config) : +DipSwitch::DipSwitch(Config& config) : gpio_array([&] (std::index_sequence) -> std::array { - return {hal::Gpio{dip_switch_config.gpio_array[I]}...}; + return {hal::Gpio{config.gpio_array[I]}...}; }(std::make_index_sequence())) { } diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index eb7096b..b5a3e7e 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -13,8 +13,8 @@ namespace proxy { template -DistanceSensors::DistanceSensors(Config& distance_sensor_config) : - adc{distance_sensor_config.adc}, led_pwm{distance_sensor_config.led_pwm} { +DistanceSensors::DistanceSensors(Config& config) : + adc{config.adc}, led_pwm{config.led_pwm}, max_distance{config.max_distance} { this->adc.start_dma(this->buffer.data(), num_of_sensors); this->led_pwm.set_duty_cycle(100.0f); } @@ -26,7 +26,7 @@ void DistanceSensors::set_led_intensity(float intensity) { template float DistanceSensors::get_distance(uint8_t sensor_index) const { - return DistanceSensors::max_distance * this->buffer.at(sensor_index) / hal::AdcDma::max_reading; + return this->max_distance * this->buffer.at(sensor_index) / hal::AdcDma::max_reading; } template diff --git a/src/proxy/fan.cpp b/src/proxy/fan.cpp index 995a7b0..17aa3ae 100644 --- a/src/proxy/fan.cpp +++ b/src/proxy/fan.cpp @@ -9,10 +9,7 @@ #include "proxy/fan.hpp" namespace proxy { -Fan::Fan(Config& fan_config) : - pwm{fan_config.pwm}, - direction_gpio{fan_config.direction_gpio}, - enable_gpio{fan_config.enable_gpio} { +Fan::Fan(Config& config) : pwm{config.pwm}, direction_gpio{config.direction_gpio}, enable_gpio{config.enable_gpio} { this->stop(); this->enable(); } diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 99f4615..f55feb4 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -10,7 +10,7 @@ #include "proxy/imu.hpp" namespace proxy { -Imu::Imu(Config& imu_config) : spi{imu_config.spi} { +Imu::Imu(Config& config) : spi{config.spi} { this->dev_ctx.write_reg = [](void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len) -> int32_t { auto spi = static_cast(handle); @@ -36,7 +36,7 @@ Imu::Imu(Config& imu_config) : spi{imu_config.spi} { return 0; }; - this->dev_ctx.handle = &imu_config.spi; + this->dev_ctx.handle = &config.spi; hal::Timer::sleep_ms(10); diff --git a/src/proxy/led.cpp b/src/proxy/led.cpp index f1f6955..976c766 100644 --- a/src/proxy/led.cpp +++ b/src/proxy/led.cpp @@ -9,7 +9,7 @@ #include "proxy/led.hpp" namespace proxy { -Led::Led(Config& led_config) : gpio{led_config.gpio} { +Led::Led(Config& config) : gpio{config.gpio} { } void Led::turn_on() { diff --git a/src/proxy/locomotion.cpp b/src/proxy/locomotion.cpp index 8dc6e62..3749a11 100644 --- a/src/proxy/locomotion.cpp +++ b/src/proxy/locomotion.cpp @@ -9,12 +9,10 @@ #include "proxy/locomotion.hpp" namespace proxy { -Locomotion::Locomotion(Config& locomotion_config) : - pwm_left_fwd{locomotion_config.pwm_left_fwd}, - pwm_left_bwd{locomotion_config.pwm_left_bwd}, - pwm_right_fwd{locomotion_config.pwm_right_fwd}, - pwm_right_bwd{locomotion_config.pwm_right_bwd}, - enable_gpio{locomotion_config.enable_gpio} { +Locomotion::Locomotion(Config& config) : + pwm_left_fwd{config.pwm_left_fwd}, pwm_left_bwd{config.pwm_left_bwd}, + pwm_right_fwd{config.pwm_right_fwd}, pwm_right_bwd{config.pwm_right_bwd}, + enable_gpio{config.enable_gpio} { this->stop(); this->enable(); } diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index 97eacf5..5c838b8 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -11,11 +11,10 @@ #include "proxy/rotary_sensor.hpp" namespace proxy { -RotarySensor::RotarySensor(Config& rotary_sensor_config) : - spi{rotary_sensor_config.spi}, encoder{rotary_sensor_config.encoder} { +RotarySensor::RotarySensor(Config& config) : spi{config.spi}, encoder{config.encoder}, resolution{config.resolution} { } float RotarySensor::get_position() { - return 2 * std::numbers::pi_v * encoder.get_counter() / this->resolution; + return encoder.get_counter() * 2 * std::numbers::pi_v / this->resolution; } } // namespace proxy diff --git a/src/proxy/torque_sensors.cpp b/src/proxy/torque_sensors.cpp index d220f3f..392ced8 100644 --- a/src/proxy/torque_sensors.cpp +++ b/src/proxy/torque_sensors.cpp @@ -13,8 +13,8 @@ namespace proxy { template -TorqueSensors::TorqueSensors(Config& torque_sensors_config) : - current_sensors{torque_sensors_config.current_sensors} { +TorqueSensors::TorqueSensors(Config& config) : + current_sensors{config.current_sensors}, max_torque{config.max_torque} { } template From 149cbf9cb1b1d3766b7be1d0aec53349b541555e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 21:41:59 -0300 Subject: [PATCH 082/561] =?UTF-8?q?=E2=9C=A8=20Add=20addressable=20rgb=20l?= =?UTF-8?q?ed=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/argb.hpp | 111 +++++++++++++++++++++++++++++++++++++++++++++ src/proxy/argb.cpp | 60 ++++++++++++++++++++++++ 2 files changed, 171 insertions(+) create mode 100644 inc/proxy/argb.hpp create mode 100644 src/proxy/argb.cpp diff --git a/inc/proxy/argb.hpp b/inc/proxy/argb.hpp new file mode 100644 index 0000000..6247052 --- /dev/null +++ b/inc/proxy/argb.hpp @@ -0,0 +1,111 @@ +/** + * @file argb.hpp + * + * @brief Proxy Argb class declaration + * + * @date 03/2024 + */ + +#ifndef __ARGB_HPP__ +#define __ARGB_HPP__ + +#include +#include + +#include "hal/pwm_dma.hpp" + +namespace proxy { +/** + * @brief Class for controlling an addressable RGB LED + */ +template +class Argb { + public: + /** + * @brief Configuration structure for the addressable RGB LED + */ + struct Config { + hal::PwmDma::Config pwm; + }; + + /** + * @brief Structure for storing color information + */ + struct Color { + uint8_t red; + uint8_t green; + uint8_t blue; + }; + + /** + * @brief Constructor for the Argb class + * + * @param config Configuration for the addressable RGB LED + */ + Argb(Config& config); + + /** + * @brief Set the color of the ARGB at the specified index + * + * @param index The index of the ARGB to set the color of + * @param color The color to set the ARGB to + */ + void set_color(const Color& color, uint8_t index); + + /** + * @brief Set the color of all ARGBs + * + * @param color The color to set all ARGBs to + */ + void set_color(const Color& color); + + /** + * @brief Turn off the ARGB at the specified index + * + * @param index The index of the ARGB to turn off + */ + void turn_off(uint8_t index); + + /** + * @brief Turn off all ARGBs + */ + void turn_off(); + + private: + void encode_color(const Color& color, uint8_t index); + + /** + * @brief PWM control object + */ + hal::PwmDma pwm; + + /** + * @brief PWM autoreload value for the low signal + */ + const uint32_t low_bit; + + /** + * @brief PWM autoreload value for the high signal + */ + const uint32_t high_bit; + + /** + * @brief Protocol constants for the addressable RGB LED communication + */ + static constexpr uint8_t bits_per_color{8}; + static constexpr uint8_t colors_per_led{3}; + static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; + static constexpr float low_duty_cycle{0.32f}; + static constexpr float high_duty_cycle{0.64f}; + static constexpr uint8_t reset_length{40}; + + /** + * @brief Data buffer to send to the addressable RGB LED + */ + std::array buffer; +}; +} // namespace proxy + +#include "../src/proxy/argb.cpp" + +#endif // __ARGB_HPP__ diff --git a/src/proxy/argb.cpp b/src/proxy/argb.cpp new file mode 100644 index 0000000..cf4ab1a --- /dev/null +++ b/src/proxy/argb.cpp @@ -0,0 +1,60 @@ +/** + * @file argb.cpp + * + * @brief Proxy Argb class implementation + * + * @date 03/2024 + */ + +#ifndef __ARGB_CPP__ +#define __ARGB_CPP__ + +#include "proxy/argb.hpp" + +namespace proxy { +template +Argb::Argb(Config& config) : pwm{config.pwm}, + low_bit{pwm.get_compare(low_duty_cycle)}, high_bit{pwm.get_compare(high_duty_cycle)} { +} + +template +void Argb::set_color(const Color& color, uint8_t index) { + if (index >= num_of_leds) { + return; + } + + this->encode_color(color, index); + this->pwm.start_dma(this->buffer.data(), this->buffer.size()); +} + +template +void Argb::set_color(const Color& color) { + for (uint8_t i = 0; i < num_of_leds; i++) { + this->encode_color(color, i); + } + + this->pwm.start_dma(this->buffer.data(), this->buffer.size()); +} + +template +void Argb::turn_off(uint8_t index) { + this->set_color(index, {0, 0, 0}); +} + +template +void Argb::turn_off() { + this->set_color({0, 0, 0}); +} + +template +void Argb::encode_color(const Color& color, uint8_t index) { + uint32_t data = color.green << (2 * bits_per_color) | color.red << bits_per_color | color.blue; + + for (uint8_t i = bits_per_led * index, j = 0; i < bits_per_led * (index + 1); i++) { + this->buffer.at(i) = ((data >> j) & 1) ? this->high_bit : this->low_bit; + j++; + } +} +} // namespace proxy + +#endif // __ARGB_CPP__ From ac3053a509ec61c7f1f113207fec0ccde24cd449 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 24 Mar 2024 21:56:25 -0300 Subject: [PATCH 083/561] =?UTF-8?q?=F0=9F=A5=85=20Fix=20adc=20constants=20?= =?UTF-8?q?utilization?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 12 ++++++------ src/hal/adc_dma.cpp | 3 ++- src/proxy/battery.cpp | 2 +- src/proxy/current_sensors.cpp | 7 ++++--- src/proxy/distance_sensors.cpp | 2 +- src/proxy/torque_sensors.cpp | 2 +- 6 files changed, 15 insertions(+), 13 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index bbb22f2..fc28edb 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -48,12 +48,6 @@ class AdcDma { */ void stop_dma(); - private: - /** - * @brief ADC handle - */ - ADC_HandleTypeDef* handle; - /** * @brief Maximum ADC reading */ @@ -63,6 +57,12 @@ class AdcDma { * @brief Reference voltage for the ADC measurement */ const float reference_voltage; + + private: + /** + * @brief ADC handle + */ + ADC_HandleTypeDef* handle; }; } diff --git a/src/hal/adc_dma.cpp b/src/hal/adc_dma.cpp index c991bfc..e2b5409 100644 --- a/src/hal/adc_dma.cpp +++ b/src/hal/adc_dma.cpp @@ -9,7 +9,8 @@ #include "hal/adc_dma.hpp" namespace hal { -AdcDma::AdcDma(Config& config) : handle{config.handle} { +AdcDma::AdcDma(Config& config) : + max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { config.init_function(); } diff --git a/src/proxy/battery.cpp b/src/proxy/battery.cpp index 6702c63..08044dd 100644 --- a/src/proxy/battery.cpp +++ b/src/proxy/battery.cpp @@ -14,7 +14,7 @@ Battery::Battery(Config& config) : adc{config.adc}, voltage_divider{config.volta } float Battery::get_voltage() { - return this->voltage_divider * this->raw_reading * hal::AdcDma::reference_voltage / hal::AdcDma::max_reading; + return this->voltage_divider * this->raw_reading * this->adc.reference_voltage / this->adc.max_reading; } uint32_t Battery::get_voltage_raw() { diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp index 5d6c10d..8fd2e5d 100644 --- a/src/proxy/current_sensors.cpp +++ b/src/proxy/current_sensors.cpp @@ -13,14 +13,15 @@ namespace proxy { template -CurrentSensors::CurrentSensors(Config& config) : adc{config.adc}, shunt_resistor{config.shunt_resistor} { +CurrentSensors::CurrentSensors(Config& config) : + adc{config.adc}, shunt_resistor{config.shunt_resistor} { this->adc.start_dma(this->buffer.data(), num_of_sensors); } template float CurrentSensors::get_current(uint8_t sensor_index) const { - return hal::AdcDma::reference_voltage * this->buffer.at(sensor_index) / - (hal::AdcDma::max_reading * this->shunt_resistor); + return this->adc.reference_voltage * this->buffer.at(sensor_index) / + (this->adc.max_reading * this->shunt_resistor); } template diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index b5a3e7e..9e4cd49 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -26,7 +26,7 @@ void DistanceSensors::set_led_intensity(float intensity) { template float DistanceSensors::get_distance(uint8_t sensor_index) const { - return this->max_distance * this->buffer.at(sensor_index) / hal::AdcDma::max_reading; + return this->max_distance * this->buffer.at(sensor_index) / this->adc.max_reading; } template diff --git a/src/proxy/torque_sensors.cpp b/src/proxy/torque_sensors.cpp index 392ced8..8e36fab 100644 --- a/src/proxy/torque_sensors.cpp +++ b/src/proxy/torque_sensors.cpp @@ -19,7 +19,7 @@ TorqueSensors::TorqueSensors(Config& config) : template float TorqueSensors::get_torque(uint8_t sensor_index) const { - return this->current_sensors.get_current_raw(sensor_index) * TorqueSensors::max_torque / hal::AdcDma::max_reading; + return this->current_sensors.get_current_raw(sensor_index) * TorqueSensors::max_torque / this->adc.max_reading; } template From d3710bee581d5dc80b30842d86ae9b6c21153b45 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 25 Mar 2024 01:31:14 -0300 Subject: [PATCH 084/561] =?UTF-8?q?=F0=9F=9A=B8=20Add=20imu=20constants?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/spi.hpp | 4 +- inc/proxy/imu.hpp | 53 +++++++++++++++++++++- src/hal/spi.cpp | 5 ++- src/proxy/imu.cpp | 110 +++++++++++++++++++++++++++++----------------- 4 files changed, 128 insertions(+), 44 deletions(-) diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index c35fd77..6dad345 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -39,8 +39,10 @@ class Spi { /** * @brief Activate the chip select + * + * @return true if the device was successfully selected, false otherwise */ - void select_device(); + bool select_device(); /** * @brief Deactivate the chip select diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 65602a9..0cf25e8 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -25,7 +25,15 @@ class Imu { * @brief IMU configuration struct */ struct Config { - hal::Spi::Config spi; + hal::Spi::Config spi; + lsm6dsv_data_rate_t gyroscope_data_rate; + lsm6dsv_data_rate_t accelerometer_data_rate; + lsm6dsv_gy_full_scale_t gyroscope_full_scale; + lsm6dsv_xl_full_scale_t accelerometer_full_scale; + float (* convert_ang_vel)(int16_t); + float (* convert_lin_acc)(int16_t); + lsm6dsv_filt_gy_lp1_bandwidth_t gyroscope_filter; + lsm6dsv_filt_xl_lp2_bandwidth_t accelerometer_filter; }; /** @@ -44,6 +52,7 @@ class Imu { /** * @brief Get the IMU orientation over an axis + * @todo implement function using sensior fusion * * @param axis Axis to get the orientation from * @@ -70,6 +79,48 @@ class Imu { float get_linear_acceleration(Axis axis); private: + /** + * @brief Read data from the IMU + * + * @param handle Pointer to a SPI object + * @param reg Register to read from + * @param bufp Buffer to read + * @param len Length of the buffer + * + * @return 0 if the operation was successful, -1 otherwise + */ + static int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len); + + /** + * @brief Write data to the IMU + * + * @param handle Pointer to a SPI object + * @param reg Register to write to + * @param bufp Buffer to write + * @param len Length of the buffer + * + * @return 0 if the operation was successful, -1 otherwise + */ + static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len); + + /** + * @brief Function to convert raw data to angular velocity + * + * @param raw_data Raw data from the IMU + * + * @return Angular velocity in md/s + */ + float (* convert_ang_vel)(int16_t); + + /** + * @brief Function to convert raw data to linear acceleration + * + * @param raw_data Raw data from the IMU + * + * @return Linear acceleration in mg + */ + float (* convert_lin_acc)(int16_t); + /** * @brief SPI for the IMU communication */ diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index ee46154..c067587 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -12,12 +12,13 @@ namespace hal { Spi::Spi(Config& config) : handle{config.handle}, gpio{config.gpio}, timeout{config.timeout} { } -void Spi::select_device() { +bool Spi::select_device() { if (this->handle->Lock == HAL_LOCKED) { - return; + return false; } this->gpio.write(false); + return true; } void Spi::unselect_device() { diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index f55feb4..5b6d603 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -6,36 +6,19 @@ * @date 03/2024 */ +#include + #include "hal/timer.hpp" #include "proxy/imu.hpp" namespace proxy { -Imu::Imu(Config& config) : spi{config.spi} { - this->dev_ctx.write_reg = - [](void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len) -> int32_t { - auto spi = static_cast(handle); - - spi->select_device(); - spi->transmit(®, 1); - spi->transmit((uint8_t*) bufp, len); - spi->unselect_device(); - - return 0; - }; - - this->dev_ctx.read_reg = - [](void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) -> int32_t { - auto spi = static_cast(handle); - - reg |= 0x80; - spi->select_device(); - spi->transmit(®, 1); - spi->receive(bufp, len); - spi->unselect_device(); - - return 0; - }; +static constexpr float mg_to_mps2{0.00980665}; +static constexpr float mdps_to_radps{std::numbers::pi_v / 180000}; +Imu::Imu(Config& config) : + convert_ang_vel{config.convert_ang_vel}, convert_lin_acc{config.convert_lin_acc}, spi{config.spi} { + this->dev_ctx.read_reg = Imu::platform_read; + this->dev_ctx.write_reg = Imu::platform_write; this->dev_ctx.handle = &config.spi; hal::Timer::sleep_ms(10); @@ -50,55 +33,102 @@ Imu::Imu(Config& config) : spi{config.spi} { lsm6dsv_block_data_update_set(&(this->dev_ctx), PROPERTY_ENABLE); - lsm6dsv_xl_data_rate_set(&(this->dev_ctx), LSM6DSV_ODR_HA02_AT_6400Hz); - lsm6dsv_gy_data_rate_set(&(this->dev_ctx), LSM6DSV_ODR_HA02_AT_6400Hz); + lsm6dsv_gy_data_rate_set(&(this->dev_ctx), config.gyroscope_data_rate); + lsm6dsv_xl_data_rate_set(&(this->dev_ctx), config.accelerometer_data_rate); + + lsm6dsv_gy_full_scale_set(&(this->dev_ctx), config.gyroscope_full_scale); + lsm6dsv_xl_full_scale_set(&(this->dev_ctx), config.accelerometer_full_scale); - lsm6dsv_xl_full_scale_set(&(this->dev_ctx), LSM6DSV_8g); - lsm6dsv_gy_full_scale_set(&(this->dev_ctx), LSM6DSV_4000dps); + lsm6dsv_filt_settling_mask_t filt_settling_mask; + filt_settling_mask.drdy = PROPERTY_ENABLE; + filt_settling_mask.irq_xl = PROPERTY_ENABLE; + filt_settling_mask.irq_g = PROPERTY_ENABLE; + lsm6dsv_filt_settling_mask_set(&dev_ctx, filt_settling_mask); lsm6dsv_filt_gy_lp1_set(&(this->dev_ctx), PROPERTY_ENABLE); - lsm6dsv_filt_gy_lp1_bandwidth_set(&(this->dev_ctx), LSM6DSV_GY_ULTRA_LIGHT); + lsm6dsv_filt_gy_lp1_bandwidth_set(&(this->dev_ctx), config.gyroscope_filter); lsm6dsv_filt_xl_lp2_set(&(this->dev_ctx), PROPERTY_ENABLE); - lsm6dsv_filt_xl_lp2_bandwidth_set(&(this->dev_ctx), LSM6DSV_XL_STRONG); + lsm6dsv_filt_xl_lp2_bandwidth_set(&(this->dev_ctx), config.accelerometer_filter); } -float Imu::get_orientation(Axis axis) { +float Imu::get_orientation(Axis /*axis*/) { return 0.0f; } float Imu::get_angular_velocity(Axis axis) { lsm6dsv_angular_rate_raw_get(&(this->dev_ctx), this->angular_velocity.data()); + float ang_vel; switch (axis) { case Axis::X: - return lsm6dsv_from_fs4000_to_mdps(this->angular_velocity[0]); + return ang_vel = convert_ang_vel(this->angular_velocity[0]); + break; case Axis::Y: - return lsm6dsv_from_fs4000_to_mdps(this->angular_velocity[1]); + return ang_vel = convert_ang_vel(this->angular_velocity[1]); + break; case Axis::Z: - return lsm6dsv_from_fs4000_to_mdps(this->angular_velocity[2]); + return ang_vel = convert_ang_vel(this->angular_velocity[2]); + break; default: - return 0.0; + return 0.0f; } + + return ang_vel * mdps_to_radps; } float Imu::get_linear_acceleration(Axis axis) { lsm6dsv_acceleration_raw_get(&(this->dev_ctx), this->linear_acceleration.data()); + float lin_acc; switch (axis) { case Axis::X: - return lsm6dsv_from_fs8_to_mg(this->linear_acceleration[0]); + lin_acc = this->convert_lin_acc(this->linear_acceleration[0]); + break; case Axis::Y: - return lsm6dsv_from_fs8_to_mg(this->linear_acceleration[1]); + lin_acc = this->convert_lin_acc(this->linear_acceleration[1]); + break; case Axis::Z: - return lsm6dsv_from_fs8_to_mg(this->linear_acceleration[2]); + lin_acc = this->convert_lin_acc(this->linear_acceleration[2]); + break; default: - return 0.0; + return 0.0f; + } + + return lin_acc * mg_to_mps2; +} + +int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) { + auto spi = static_cast(handle); + + while (not spi->select_device()) { + continue; } + + reg |= 0x80; + spi->transmit(®, 1); + spi->receive(bufp, len); + spi->unselect_device(); + + return 0; } + +int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len) { + auto spi = static_cast(handle); + + while (not spi->select_device()) { + continue; + } + + spi->transmit(®, 1); + spi->transmit((uint8_t*) bufp, len); + spi->unselect_device(); + + return 0; +}; } // namespace proxy From ad768b4c36ab57559d16eebec71a0ff341a2c38c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 25 Mar 2024 15:22:28 -0300 Subject: [PATCH 085/561] =?UTF-8?q?=E2=9C=A8=20Add=20linear=20and=20angula?= =?UTF-8?q?r=20speed=20function=20to=20locomotion=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/locomotion.hpp | 24 ++++++++++++++++-------- src/proxy/locomotion.cpp | 33 ++++++++++++++++++++++++++------- 2 files changed, 42 insertions(+), 15 deletions(-) diff --git a/inc/proxy/locomotion.hpp b/inc/proxy/locomotion.hpp index 7e2fc5b..da30fba 100644 --- a/inc/proxy/locomotion.hpp +++ b/inc/proxy/locomotion.hpp @@ -39,22 +39,30 @@ class Locomotion { Locomotion(Config& config); /** - * @brief Enable the motor driver + * @brief Enable the locomotion driver */ void enable(); /** - * @brief Disable the motor driver + * @brief Disable the locomotion driver */ void disable(); /** - * @brief Set the speed of the motors + * @brief Set the speed of the wheels * - * @param left_speed Speed of the left motor - * @param right_speed Speed of the right motor + * @param left_speed Speed of the left wheels + * @param right_speed Speed of the right wheels */ - void set_speed(float left_speed, float right_speed); + void set_wheel_speed(float left_speed, float right_speed); + + /** + * @brief Set the linear and angular speeds of the robot + * + * @param linear Linear speed of the robot + * @param angular Angular speed of the robot + */ + void set_speed(float linear, float angular); /** * @brief Stop the motors @@ -64,12 +72,12 @@ class Locomotion { /** * @brief Stop the left motor */ - void stop_left_motor(); + void stop_left(); /** * @brief Stop the right motor */ - void stop_right_motor(); + void stop_right(); private: /** diff --git a/src/proxy/locomotion.cpp b/src/proxy/locomotion.cpp index 3749a11..89865a8 100644 --- a/src/proxy/locomotion.cpp +++ b/src/proxy/locomotion.cpp @@ -6,6 +6,8 @@ * @date 03/2024 */ +#include + #include "proxy/locomotion.hpp" namespace proxy { @@ -25,7 +27,7 @@ void Locomotion::disable() { this->enable_gpio.write(false); } -void Locomotion::set_speed(float left_speed, float right_speed) { +void Locomotion::set_wheel_speed(float left_speed, float right_speed) { if (left_speed > 0.0f) { this->pwm_left_fwd.set_duty_cycle(left_speed); this->pwm_left_bwd.set_duty_cycle(0.0f); @@ -33,7 +35,7 @@ void Locomotion::set_speed(float left_speed, float right_speed) { this->pwm_left_fwd.set_duty_cycle(0.0f); this->pwm_left_bwd.set_duty_cycle(-left_speed); } else { - this->stop_left_motor(); + this->stop_left(); } if (right_speed > 0.0f) { @@ -43,21 +45,38 @@ void Locomotion::set_speed(float left_speed, float right_speed) { this->pwm_right_fwd.set_duty_cycle(0.0f); this->pwm_right_bwd.set_duty_cycle(-right_speed); } else { - this->stop_right_motor(); + this->stop_right(); + } +} + +void Locomotion::set_speed(float linear, float angular) { + float left_speed = linear - angular; + float right_speed = linear + angular; + + if (std::abs(left_speed) > 100.0f) { + left_speed *= 100.0f / std::abs(left_speed); + right_speed *= 100.0f / std::abs(left_speed); + } + + if (std::abs(right_speed) > 100.0f) { + left_speed *= 100.0f / std::abs(right_speed); + right_speed *= 100.0f / std::abs(right_speed); } + + this->set_wheel_speed(left_speed, right_speed); } void Locomotion::stop() { - this->stop_left_motor(); - this->stop_right_motor(); + this->stop_left(); + this->stop_right(); } -void Locomotion::stop_left_motor() { +void Locomotion::stop_left() { this->pwm_left_fwd.set_duty_cycle(0.0f); this->pwm_left_bwd.set_duty_cycle(0.0f); } -void Locomotion::stop_right_motor() { +void Locomotion::stop_right() { this->pwm_right_fwd.set_duty_cycle(0.0f); this->pwm_right_bwd.set_duty_cycle(0.0f); } From 7c8f238450a09e6064a70d92e66cbfc415d95692 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 25 Mar 2024 15:34:53 -0300 Subject: [PATCH 086/561] =?UTF-8?q?=E2=9C=A8=20Check=20channel=20state=20b?= =?UTF-8?q?efore=20sending=20message?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/hal/pwm_dma.cpp | 4 ++++ src/hal/spi.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/hal/pwm_dma.cpp b/src/hal/pwm_dma.cpp index 57acb44..47bdb0a 100644 --- a/src/hal/pwm_dma.cpp +++ b/src/hal/pwm_dma.cpp @@ -16,6 +16,10 @@ PwmDma::PwmDma(Config& config) : handle{config.handle}, channel{config.timer_cha } void PwmDma::start_dma(uint32_t buffer[], uint32_t size) { + if (TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_STATE_BUSY) { + HAL_TIM_PWM_Stop_DMA(this->handle, this->channel); + } + HAL_TIM_PWM_Start_DMA(this->handle, this->channel, buffer, size); } diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index c067587..aebe8a4 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -13,7 +13,7 @@ Spi::Spi(Config& config) : handle{config.handle}, gpio{config.gpio}, timeout{con } bool Spi::select_device() { - if (this->handle->Lock == HAL_LOCKED) { + if (HAL_SPI_GetState(this->handle) != HAL_SPI_STATE_READY) { return false; } From eb03de8b6e20284198506c16488de2e2b2227495 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 25 Mar 2024 15:35:17 -0300 Subject: [PATCH 087/561] =?UTF-8?q?=F0=9F=92=A1=20Add=20missing=20comment?= =?UTF-8?q?=20on=20argb=20encode=5Fcolor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/argb.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/inc/proxy/argb.hpp b/inc/proxy/argb.hpp index 6247052..d85528c 100644 --- a/inc/proxy/argb.hpp +++ b/inc/proxy/argb.hpp @@ -72,6 +72,12 @@ class Argb { void turn_off(); private: + /** + * @brief Encode a color into the data buffer + * + * @param color The color to encode + * @param index The index to encode the color at + */ void encode_color(const Color& color, uint8_t index); /** From 21d19515711db216580e0240b9854e068a87e1f2 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 25 Mar 2024 17:37:02 -0300 Subject: [PATCH 088/561] =?UTF-8?q?=E2=9C=A8=20Add=20CRC=20hal=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/crc.hpp | 52 +++++++++++++++++++++++++++++++++++++++++++++++ src/hal/crc.cpp | 18 ++++++++++++++++ src/hal/mcu.cpp | 1 + src/hal/spi.cpp | 1 + src/proxy/imu.cpp | 4 ++-- 5 files changed, 74 insertions(+), 2 deletions(-) create mode 100644 inc/hal/crc.hpp create mode 100644 src/hal/crc.cpp diff --git a/inc/hal/crc.hpp b/inc/hal/crc.hpp new file mode 100644 index 0000000..aa978a3 --- /dev/null +++ b/inc/hal/crc.hpp @@ -0,0 +1,52 @@ +/** + * @file crc.hpp + * + * @brief STM32 CRC HAL wrapper + * + * @date 03/2024 + */ + +#ifndef __CRC_HPP__ +#define __CRC_HPP__ + +#include + +namespace hal { +/** + * @brief Class to handle CRC peripheral on STM32 microcontrollers + */ +class Crc { + public: + /** + * @brief CRC configuration struct + */ + struct Config { + TIM_HandleTypeDef* handle; + }; + + /** + * @brief Construct a new Crc object + * + * @param config Configuration for the CRC + */ + Crc(Config& config); + + /** + * @brief Calculate the CRC value + * + * @param data Data to calculate the CRC + * @param size Size of the buffer + * + * @return CRC value + */ + uint32_t Crc::calculate(uint32_t data[], uint32_t size); + + private: + /** + * @brief CRC handle + */ + CRC_HandleTypeDef* handle; +}; +} // namespace hal + +#endif // __CRC_HPP__ diff --git a/src/hal/crc.cpp b/src/hal/crc.cpp new file mode 100644 index 0000000..d7bc2d7 --- /dev/null +++ b/src/hal/crc.cpp @@ -0,0 +1,18 @@ +/** + * @file crc.cpp + * + * @brief STM32 CRC HAL wrapper + * + * @date 03/2024 + */ + +#include "hal/crc.hpp" + +namespace hal { +Crc::Crc(Config& config) : handle{config.handle} { +} + +uint32_t Crc::calculate(uint32_t data[], uint32_t size) { + return HAL_CRC_Calculate(this->crc_handle, data, size); +} +} // namespace hal diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index 8985c6d..bc69a39 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -26,5 +26,6 @@ void Mcu::init(void) { MX_GPIO_Init(); MX_DMA_Init(); + MX_CRC_Init(); } } diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index aebe8a4..c40ba76 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -10,6 +10,7 @@ namespace hal { Spi::Spi(Config& config) : handle{config.handle}, gpio{config.gpio}, timeout{config.timeout} { + config.init_function(); } bool Spi::select_device() { diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 5b6d603..0e8bf2c 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -17,8 +17,8 @@ static constexpr float mdps_to_radps{std::numbers::pi_v / 180000}; Imu::Imu(Config& config) : convert_ang_vel{config.convert_ang_vel}, convert_lin_acc{config.convert_lin_acc}, spi{config.spi} { - this->dev_ctx.read_reg = Imu::platform_read; - this->dev_ctx.write_reg = Imu::platform_write; + this->dev_ctx.read_reg = platform_read; + this->dev_ctx.write_reg = platform_write; this->dev_ctx.handle = &config.spi; hal::Timer::sleep_ms(10); From 9a7b7e57d525f3ab03d07d6421a9cd82225ef448 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 25 Mar 2024 17:39:07 -0300 Subject: [PATCH 089/561] =?UTF-8?q?=F0=9F=90=9B=20Remove=20extra=20specifi?= =?UTF-8?q?er=20from=20CRC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/crc.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/inc/hal/crc.hpp b/inc/hal/crc.hpp index aa978a3..75815ac 100644 --- a/inc/hal/crc.hpp +++ b/inc/hal/crc.hpp @@ -39,7 +39,7 @@ class Crc { * * @return CRC value */ - uint32_t Crc::calculate(uint32_t data[], uint32_t size); + uint32_t calculate(uint32_t data[], uint32_t size); private: /** From 19b49ea72ca739ce5ad9aa3965d74262a6ac1e23 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 25 Mar 2024 18:17:43 -0300 Subject: [PATCH 090/561] =?UTF-8?q?=E2=9C=A8=20Add=20rotary=20sensor=20reg?= =?UTF-8?q?ister=20configuration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/crc.hpp | 2 +- inc/proxy/rotary_sensor.hpp | 38 ++++++++++++ inc/proxy/rotary_sensor_reg.hpp | 100 ++++++++++++++++++++++++++++++++ src/hal/crc.cpp | 2 +- src/hal/mcu.cpp | 1 + src/hal/pwm_dma.cpp | 2 +- src/proxy/rotary_sensor.cpp | 46 ++++++++++++++- 7 files changed, 187 insertions(+), 4 deletions(-) create mode 100644 inc/proxy/rotary_sensor_reg.hpp diff --git a/inc/hal/crc.hpp b/inc/hal/crc.hpp index 75815ac..ec0a7d4 100644 --- a/inc/hal/crc.hpp +++ b/inc/hal/crc.hpp @@ -21,7 +21,7 @@ class Crc { * @brief CRC configuration struct */ struct Config { - TIM_HandleTypeDef* handle; + CRC_HandleTypeDef* handle; }; /** diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index 4829853..1f98d62 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -11,6 +11,7 @@ #include +#include "hal/crc.hpp" #include "hal/encoder.hpp" #include "hal/spi.hpp" @@ -20,13 +21,17 @@ namespace proxy { */ class RotarySensor { public: + #include "proxy/rotary_sensor_reg.hpp" + /** * @brief rotary sensor configuration struct */ struct Config { hal::Spi::Config spi; hal::Encoder::Config encoder; + hal::Crc::Config crc; uint32_t resolution; + Registers registers; }; /** @@ -44,6 +49,34 @@ class RotarySensor { float get_position(); private: + union CommandFrame { + uint32_t raw; + struct __attribute__((packed)) { + uint8_t do_not_care : 1; + uint8_t rw : 1; + uint16_t address : 14; + uint8_t crc : 8; + }; + }; + + union DataFrame { + uint32_t raw; + struct __attribute__((packed)) { + uint8_t warning : 1; + uint8_t error : 1; + uint16_t data : 14; + uint8_t crc : 8; + }; + }; + + /** + * @brief Write a register to the rotary sensor + * + * @param command_frame Command frame to send trough SPI + * @param data_frame Data frame to send trough SPI + */ + void write_register(CommandFrame& command_frame, DataFrame& data_frame); + /** * @brief SPI for the rotary sensor configuration */ @@ -54,6 +87,11 @@ class RotarySensor { */ hal::Encoder encoder; + /** + * @brief CRC for the rotary sensor configuration + */ + hal::Crc crc; + /** * @brief Resolution of the rotary sensor */ diff --git a/inc/proxy/rotary_sensor_reg.hpp b/inc/proxy/rotary_sensor_reg.hpp new file mode 100644 index 0000000..2164540 --- /dev/null +++ b/inc/proxy/rotary_sensor_reg.hpp @@ -0,0 +1,100 @@ + +/** + * @file rotary_sensor_reg.hpp + * + * @brief AS5047U rotary sensor registers definition + * + * @date 03/2024 + */ + +#ifndef __ROTARY_SENSOR_REG_HPP__ +#define __ROTARY_SENSOR_REG_HPP__ + +#include + +struct Registers { + union Disable { + uint8_t raw; + struct __attribute__((packed)) { + uint8_t UVW_off : 1; + uint8_t ABI_off : 1; + uint8_t na : 4; + uint8_t FILTER_disable : 1; + }; + }; + + union Zposm { + uint8_t raw; + struct __attribute__((packed)) { + uint8_t ZPOSM : 8; + }; + }; + + union Zpols { + uint8_t raw; + struct __attribute__((packed)) { + uint8_t ZPOSL : 6; + uint8_t Dia1_en : 1; + uint8_t Dia2_en : 1; + }; + }; + + union Settings1 { + uint8_t raw; + struct __attribute__((packed)) { + uint8_t K_max : 3; + uint8_t K_min : 3; + uint8_t Dia3_en : 1; + uint8_t Dia4_en : 1; + }; + }; + + union Settings2 { + uint8_t raw; + struct __attribute__((packed)) { + uint8_t IWIDTH : 1; + uint8_t NOISESET : 1; + uint8_t DIR : 1; + uint8_t UVW_ABI : 1; + uint8_t DAECDIS : 1; + uint8_t ABI_DEC : 1; + uint8_t Data_select : 1; + uint8_t PWMon : 1; + }; + }; + + union Settings3 { + uint8_t raw; + struct __attribute__((packed)) { + uint8_t UVWPP : 3; + uint8_t HYS : 2; + uint8_t ABIRES : 3; + }; + }; + + union Ecc { + uint8_t raw; + struct __attribute__((packed)) { + uint8_t ECC_chsum : 7; + uint8_t ECC_en : 1; + }; + }; + + static constexpr uint16_t disable_addr{0x0015}; + static constexpr uint16_t zposm_addr{0x0016}; + static constexpr uint16_t zpos_addr{0x0017}; + static constexpr uint16_t settings1_addr{0x0018}; + static constexpr uint16_t settings2_addr{0x0019}; + static constexpr uint16_t settings3_addr{0x001A}; + static constexpr uint16_t ecc_addr{0x001B}; + + Disable disable; + Zposm zposm; + Zpols zpos; + Settings1 settings1; + Settings2 settings2; + Settings3 settings3; + Ecc ecc; +}; + +#endif // __ROTARY_SENSOR_REG_HPP__ diff --git a/src/hal/crc.cpp b/src/hal/crc.cpp index d7bc2d7..18c57cb 100644 --- a/src/hal/crc.cpp +++ b/src/hal/crc.cpp @@ -13,6 +13,6 @@ Crc::Crc(Config& config) : handle{config.handle} { } uint32_t Crc::calculate(uint32_t data[], uint32_t size) { - return HAL_CRC_Calculate(this->crc_handle, data, size); + return HAL_CRC_Calculate(this->handle, data, size); } } // namespace hal diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index bc69a39..e02efe7 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -4,6 +4,7 @@ * @brief MCU related */ +#include #include #include #include diff --git a/src/hal/pwm_dma.cpp b/src/hal/pwm_dma.cpp index 47bdb0a..1124dde 100644 --- a/src/hal/pwm_dma.cpp +++ b/src/hal/pwm_dma.cpp @@ -16,7 +16,7 @@ PwmDma::PwmDma(Config& config) : handle{config.handle}, channel{config.timer_cha } void PwmDma::start_dma(uint32_t buffer[], uint32_t size) { - if (TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_STATE_BUSY) { + if (TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_CHANNEL_STATE_BUSY) { HAL_TIM_PWM_Stop_DMA(this->handle, this->channel); } diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index 5c838b8..d44c0b4 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -11,10 +11,54 @@ #include "proxy/rotary_sensor.hpp" namespace proxy { -RotarySensor::RotarySensor(Config& config) : spi{config.spi}, encoder{config.encoder}, resolution{config.resolution} { +RotarySensor::RotarySensor(Config& config) : + spi{config.spi}, encoder{config.encoder}, crc{config.crc}, resolution{config.resolution} { + CommandFrame command_frame; + DataFrame data_frame; + + command_frame.address = Registers::disable_addr; + data_frame.data = config.registers.disable.raw; + this->write_register(command_frame, data_frame); + + command_frame.address = Registers::zposm_addr; + data_frame.data = config.registers.zposm.raw; + this->write_register(command_frame, data_frame); + + command_frame.address = Registers::zpos_addr; + data_frame.data = config.registers.zpos.raw; + this->write_register(command_frame, data_frame); + + command_frame.address = Registers::settings1_addr; + data_frame.data = config.registers.settings1.raw; + this->write_register(command_frame, data_frame); + + command_frame.address = Registers::settings2_addr; + data_frame.data = config.registers.settings2.raw; + this->write_register(command_frame, data_frame); + + command_frame.address = Registers::settings3_addr; + data_frame.data = config.registers.settings3.raw; + this->write_register(command_frame, data_frame); + + command_frame.address = Registers::ecc_addr; + data_frame.data = config.registers.ecc.raw; + this->write_register(command_frame, data_frame); } float RotarySensor::get_position() { return encoder.get_counter() * 2 * std::numbers::pi_v / this->resolution; } + +void RotarySensor::write_register(CommandFrame& command_frame, DataFrame& data_frame) { + while (not this->spi.select_device()) { + continue; + } + + command_frame.crc = this->crc.calculate(&command_frame.raw, 2); + data_frame.crc = this->crc.calculate(&data_frame.raw, 2); + + this->spi.transmit(reinterpret_cast(&command_frame.raw), 3); + this->spi.transmit(reinterpret_cast(&data_frame.raw), 3); + this->spi.unselect_device(); +} } // namespace proxy From cfb4351f3059f7dc09bca5b038931d67ccb4cdd1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 25 Mar 2024 18:29:53 -0300 Subject: [PATCH 091/561] =?UTF-8?q?=E2=9C=A8=20Add=20rotary=20sensor=20reg?= =?UTF-8?q?isters=20configuration=20to=20target?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/rotary_sensor_reg.hpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/inc/proxy/rotary_sensor_reg.hpp b/inc/proxy/rotary_sensor_reg.hpp index 2164540..4287f77 100644 --- a/inc/proxy/rotary_sensor_reg.hpp +++ b/inc/proxy/rotary_sensor_reg.hpp @@ -14,43 +14,46 @@ struct Registers { union Disable { - uint8_t raw; struct __attribute__((packed)) { uint8_t UVW_off : 1; uint8_t ABI_off : 1; uint8_t na : 4; uint8_t FILTER_disable : 1; }; + + uint8_t raw; }; union Zposm { - uint8_t raw; struct __attribute__((packed)) { uint8_t ZPOSM : 8; }; + + uint8_t raw; }; union Zpols { - uint8_t raw; struct __attribute__((packed)) { uint8_t ZPOSL : 6; uint8_t Dia1_en : 1; uint8_t Dia2_en : 1; }; + + uint8_t raw; }; union Settings1 { - uint8_t raw; struct __attribute__((packed)) { uint8_t K_max : 3; uint8_t K_min : 3; uint8_t Dia3_en : 1; uint8_t Dia4_en : 1; }; + + uint8_t raw; }; union Settings2 { - uint8_t raw; struct __attribute__((packed)) { uint8_t IWIDTH : 1; uint8_t NOISESET : 1; @@ -61,23 +64,27 @@ struct Registers { uint8_t Data_select : 1; uint8_t PWMon : 1; }; + + uint8_t raw; }; union Settings3 { - uint8_t raw; struct __attribute__((packed)) { uint8_t UVWPP : 3; uint8_t HYS : 2; uint8_t ABIRES : 3; }; + + uint8_t raw; }; union Ecc { - uint8_t raw; struct __attribute__((packed)) { uint8_t ECC_chsum : 7; uint8_t ECC_en : 1; }; + + uint8_t raw; }; static constexpr uint16_t disable_addr{0x0015}; From b475f1def51cb33c41ad2a6258d9e53a5bff8b2d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 25 Mar 2024 19:27:18 -0300 Subject: [PATCH 092/561] =?UTF-8?q?=F0=9F=92=A1=20Update=20CRC=20class=20c?= =?UTF-8?q?omment?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/crc.hpp | 2 +- inc/proxy/rotary_sensor.hpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/inc/hal/crc.hpp b/inc/hal/crc.hpp index ec0a7d4..3d323e6 100644 --- a/inc/hal/crc.hpp +++ b/inc/hal/crc.hpp @@ -13,7 +13,7 @@ namespace hal { /** - * @brief Class to handle CRC peripheral on STM32 microcontrollers + * @brief Class to handle the cyclic redundancy check peripheral on STM32 microcontrollers */ class Crc { public: diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index 1f98d62..d4f49e7 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -50,23 +50,25 @@ class RotarySensor { private: union CommandFrame { - uint32_t raw; struct __attribute__((packed)) { uint8_t do_not_care : 1; uint8_t rw : 1; uint16_t address : 14; uint8_t crc : 8; }; + + uint32_t raw; }; union DataFrame { - uint32_t raw; struct __attribute__((packed)) { uint8_t warning : 1; uint8_t error : 1; uint16_t data : 14; uint8_t crc : 8; }; + + uint32_t raw; }; /** From ec5b6f57a4d92431eccb7ecf065233e58d4d9093 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 26 Mar 2024 22:43:01 -0300 Subject: [PATCH 093/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20static=20constant?= =?UTF-8?q?s=20to=20header=20file?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/argb.hpp | 20 ++++++++++---------- inc/proxy/imu.hpp | 7 +++++++ src/proxy/imu.cpp | 5 ----- 3 files changed, 17 insertions(+), 15 deletions(-) diff --git a/inc/proxy/argb.hpp b/inc/proxy/argb.hpp index d85528c..3bcb958 100644 --- a/inc/proxy/argb.hpp +++ b/inc/proxy/argb.hpp @@ -80,6 +80,16 @@ class Argb { */ void encode_color(const Color& color, uint8_t index); + /** + * @brief Protocol constants for the addressable RGB LED communication + */ + static constexpr uint8_t bits_per_color{8}; + static constexpr uint8_t colors_per_led{3}; + static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; + static constexpr float low_duty_cycle{0.32f}; + static constexpr float high_duty_cycle{0.64f}; + static constexpr uint8_t reset_length{40}; + /** * @brief PWM control object */ @@ -95,16 +105,6 @@ class Argb { */ const uint32_t high_bit; - /** - * @brief Protocol constants for the addressable RGB LED communication - */ - static constexpr uint8_t bits_per_color{8}; - static constexpr uint8_t colors_per_led{3}; - static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; - static constexpr float low_duty_cycle{0.32f}; - static constexpr float high_duty_cycle{0.64f}; - static constexpr uint8_t reset_length{40}; - /** * @brief Data buffer to send to the addressable RGB LED */ diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 0cf25e8..6b030b2 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include "hal/spi.hpp" @@ -121,6 +122,12 @@ class Imu { */ float (* convert_lin_acc)(int16_t); + /** + * @brief Conversion constants + */ + static constexpr float mg_to_mps2{0.00980665}; + static constexpr float mdps_to_radps{std::numbers::pi_v / 180000}; + /** * @brief SPI for the IMU communication */ diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 0e8bf2c..a5f208a 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -6,15 +6,10 @@ * @date 03/2024 */ -#include - #include "hal/timer.hpp" #include "proxy/imu.hpp" namespace proxy { -static constexpr float mg_to_mps2{0.00980665}; -static constexpr float mdps_to_radps{std::numbers::pi_v / 180000}; - Imu::Imu(Config& config) : convert_ang_vel{config.convert_ang_vel}, convert_lin_acc{config.convert_lin_acc}, spi{config.spi} { this->dev_ctx.read_reg = platform_read; From b03bfe2da3cda607c865a7e7cb603d95cf91f10f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 26 Mar 2024 23:03:00 -0300 Subject: [PATCH 094/561] =?UTF-8?q?=E2=9C=A8=20Add=20flash=20memory=20hal?= =?UTF-8?q?=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/flash.hpp | 90 +++++++++++++++++++++++++++++++++++++++++++++++ src/hal/flash.cpp | 69 ++++++++++++++++++++++++++++++++++++ 2 files changed, 159 insertions(+) create mode 100644 inc/hal/flash.hpp create mode 100644 src/hal/flash.cpp diff --git a/inc/hal/flash.hpp b/inc/hal/flash.hpp new file mode 100644 index 0000000..0448cbb --- /dev/null +++ b/inc/hal/flash.hpp @@ -0,0 +1,90 @@ +/** + * @file flash.hpp + * + * @brief STM32 flash HAL wrapper + * + * @date 03/2024 + */ + +#ifndef __FLASH_HPP__ +#define __FLASH_HPP__ + +#include +#include + +namespace hal { +/** + * @brief Class to handle flash memory on STM32 microcontrollers + */ +class Flash { + public: + /** + * @brief Deleted constructor for static class + */ + Flash() = delete; + + /** + * @brief Read data from flash memory + * + * @param address address to read from (indexed by double words) + * @param data pointer to store the data read + * @param size size in double words of the data to read + */ + static void read(uint32_t address, uint64_t data[], uint32_t size = 1); + + /** + * @brief Read data from flash memory + * + * @param page page to read from (counting from the last to the first) + * @param page_address address inside the page to read from (indexed by double words) + * @param data pointer to store the data read + * @param size size in double words of the data to read + */ + static void read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); + + /** + * @brief Write data to flash memory + * + * @param address address to write to (indexed by double words) + * @param data pointer to the data to write + * @param size size in double words of the data to write + */ + static void write(uint32_t address, uint64_t data[], uint32_t size = 1); + + /** + * @brief Write data to flash memory + * + * @param page page to write to (counting from the last to the first) + * @param page_address address inside the page to write to (indexed by double words) + * @param data pointer to the data to write + * @param size size in double words of the data to write + */ + static void write(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); + + /** + * @brief Erase flash memory pages + * + * @param page first page to erase (counting from the last to the first) + * @param number_of_pages number of pages to erase + */ + static void erase_pages(uint16_t page, uint16_t number_of_pages = 1); + + private: + /** + * @brief Number of double words per row + */ + static constexpr uint32_t double_words_per_row{32}; + + /** + * @brief Number of bytes per row + */ + static constexpr uint32_t bytes_per_row{8 * double_words_per_row}; + + /** + * @brief Number of double words per page + */ + static constexpr uint32_t double_words_per_page{FLASH_PAGE_SIZE / 8}; +}; +} // namespace hal + +#endif // __FLASH_HPP__ diff --git a/src/hal/flash.cpp b/src/hal/flash.cpp new file mode 100644 index 0000000..15f1266 --- /dev/null +++ b/src/hal/flash.cpp @@ -0,0 +1,69 @@ +/** + * @file flash.cpp + * + * @brief STM32 flash HAL wrapper + * + * @date 03/2024 + */ + +#include "hal/flash.hpp" + +namespace hal { +/** + * @brief Last address of the flash memory + * needs to be here because it is not defined at compile time + */ +static const uint32_t base_address = FLASH_BASE + FLASH_SIZE - 8; + +void Flash::read(uint32_t address, uint64_t data[], uint32_t size) { + uint32_t end = base_address - 8 * address; + + for (address = end - 8 * (size - 1); address <= end; address += 8, data++) { + (*data) = *(reinterpret_cast(address)); + } +} + +void Flash::read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size) { + read(page * double_words_per_page + page_address, data, size); +} + +void Flash::write(uint32_t address, uint64_t data[], uint32_t size) { + HAL_FLASH_Unlock(); + + uint32_t end = base_address - 8 * address; + address = end - 8 * (size - 1); + + while (address <= end) { + if (size >= double_words_per_row) { + HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, reinterpret_cast(data)); + address += bytes_per_row; + data += double_words_per_row; + size -= double_words_per_row; + } else { + HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, address, *data); + address += 8; + data++; + size--; + } + } + + HAL_FLASH_Lock(); +} + +void Flash::write(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size) { + write(page * double_words_per_page + page_address, data, size); +} + +void Flash::erase_pages(uint16_t page, uint16_t number_of_pages) { + FLASH_EraseInitTypeDef erase_struct = { + .TypeErase = FLASH_TYPEERASE_PAGES, + .Banks = 0, + .Page = FLASH_PAGE_NB - page - number_of_pages, + .NbPages = number_of_pages, + }; + + uint32_t page_error; + + HAL_FLASHEx_Erase(&erase_struct, &page_error); +} +} // namespace hal From 8c71d92bc083ae4b8f7b57f6cc04df705bcf271f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 27 Mar 2024 01:38:41 -0300 Subject: [PATCH 095/561] =?UTF-8?q?=F0=9F=92=A1=20Fix=20comments=20startin?= =?UTF-8?q?g=20in=20lowercase?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/current_sensors.hpp | 2 +- inc/proxy/distance_sensors.hpp | 2 +- inc/proxy/rotary_sensor.hpp | 2 +- inc/proxy/torque_sensors.hpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index d7d5c71..54a50d1 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -22,7 +22,7 @@ template class CurrentSensors { public: /** - * @brief configuration structure for current sensors + * @brief Configuration structure for current sensors */ struct Config { hal::AdcDma::Config adc; diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index e73f0a5..f474ec6 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -23,7 +23,7 @@ template class DistanceSensors { public: /** - * @brief configuration structure for distance sensors + * @brief Configuration structure for distance sensors */ struct Config { hal::AdcDma::Config adc; diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index d4f49e7..3453e44 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -24,7 +24,7 @@ class RotarySensor { #include "proxy/rotary_sensor_reg.hpp" /** - * @brief rotary sensor configuration struct + * @brief Rotary sensor configuration struct */ struct Config { hal::Spi::Config spi; diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index b9ae80c..1473d5a 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -21,7 +21,7 @@ template class TorqueSensors { public: /** - * @brief configuration structure for torque sensors + * @brief Configuration structure for torque sensors */ struct Config { typename CurrentSensors::Config current_sensors; From eb6352b080e0aec6b1f8997b65152f9b6b724245 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 27 Mar 2024 05:56:08 -0300 Subject: [PATCH 096/561] =?UTF-8?q?=E2=9C=A8=20=F0=9F=9A=A7=20Add=20Storag?= =?UTF-8?q?e=20proxy=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi --- inc/proxy/serializable_interface.hpp | 22 +++++ inc/proxy/storage.hpp | 69 +++++++++++++++ src/proxy/storage.cpp | 122 +++++++++++++++++++++++++++ 3 files changed, 213 insertions(+) create mode 100644 inc/proxy/serializable_interface.hpp create mode 100644 inc/proxy/storage.hpp create mode 100644 src/proxy/storage.cpp diff --git a/inc/proxy/serializable_interface.hpp b/inc/proxy/serializable_interface.hpp new file mode 100644 index 0000000..8e1103b --- /dev/null +++ b/inc/proxy/serializable_interface.hpp @@ -0,0 +1,22 @@ +/** + * @file serializable_interface.hpp + * + * @brief Serializable interface for all classes that need to be serialized + * + * @date 03/2024 + */ + +#ifndef __SERIALIZABLE_INTERFACE_HPP__ +#define __SERIALIZABLE_INTERFACE_HPP__ + +#include +#include + +class ISerializable { + public: + virtual std::vector && serialize() = 0; + + virtual void deserialize(uint8_t* serial_data, uint32_t size) = 0; +}; + +#endif // __SERIALIZABLE_INTERFACE_HPP__ diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp new file mode 100644 index 0000000..b544ed8 --- /dev/null +++ b/inc/proxy/storage.hpp @@ -0,0 +1,69 @@ +/** + * @file storage.hpp + * + * @brief Proxy Storage class declaration + * + * @date 03/2024 + */ + +#ifndef __STORAGE_HPP__ +#define __STORAGE_HPP__ + +#include +#include +#include +#include +#include + +#include "serializable_interface.hpp" + +namespace proxy { +/** + * @brief Class for controlling the storage + */ +class Storage { + public: + /** + * @brief Configuration structure for the storage + */ + struct Config { + uint16_t start_page; + uint16_t number_of_pages; + }; + + /** + * @brief Constructor for the Storage class + * + * @param config Configuration for the storage + */ + Storage(Config& config); + + template + void create(const std::string& name, T& data); + + void create(const std::string& name, ISerializable& data); + + template + void sync(const std::string& name, T& data); + + void sync(const std::string& name, ISerializable& data); + + void save(); + + private: + struct Variable { + void* ram_address; + uint16_t buffer_address; + uint16_t size; + }; + + void serialize_var_map(); + + std::unordered_map variables; + + std::vector buffer; + uint16_t start_page; +}; +} // namespace proxy + +#endif // __STORAGE_HPP__ diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp new file mode 100644 index 0000000..cf645ed --- /dev/null +++ b/src/proxy/storage.cpp @@ -0,0 +1,122 @@ +/** + * @file storage.cpp + * + * @brief Proxy Storage class source + * + * @date 03/2024 + */ + +#include + +#include "hal/flash.hpp" +#include "proxy/storage.hpp" + +namespace proxy { +Storage::Storage(Config& config) : start_page(config.start_page) { + // [total_size][num_vars][{[name_len][name][buffer_address][size]} + + uint64_t sizes; + hal::Flash::read(this->start_page, &sizes); + + uint32_t total_size = sizes >> 32; + uint32_t num_vars = sizes & 0xFFFFFFFF; + + this->buffer.reserve(total_size); + hal::Flash::read(this->start_page, 1, reinterpret_cast(this->buffer.data()), + std::ceil(total_size / 8.0f)); + + uint16_t i = 0; + + for (uint16_t decoded_vars = 0; decoded_vars < num_vars; decoded_vars++) { + uint8_t var_name_len = buffer.at(i); + std::string var_name(buffer.begin() + i + 1, buffer.begin() + i + 1 + var_name_len); + i += var_name_len + 1; + + // read next 2 bytes as buffer address + this->variables[var_name].buffer_address = buffer.at(i) << 8 | buffer.at(i + 1); + i += 2; + + // read next 2 bytes as size + this->variables.at(var_name).size = buffer.at(i) << 8 | buffer.at(i + 1); + i += 2; + } +} + +template +void Storage::create(const std::string& name, T& data) { + this->variables[name].ram_address = &data; + this->variables.at(name).size = sizeof(T); +} + +void Storage::create(const std::string& name, ISerializable& data) { + this->variables[name].ram_address = &data; +} + +template +void Storage::sync(const std::string& name, T& data) { + if (variables.contains(name)) { + data = *reinterpret_cast(&this->buffer.at(variables.at(name).buffer_address)); + } + + this->variables[name].ram_address = &data; + this->variables.at(name).size = sizeof(T); +} + +void Storage::sync(const std::string& name, ISerializable& data) { + if (variables.contains(name)) { + data.deserialize(&this->buffer.at(variables.at(name).buffer_address), variables.at(name).size); + } + + this->variables[name].ram_address = &data; +} + +void Storage::save() { + this->buffer.clear(); + + for (auto [name, variable]: this->variables) { + ISerializable* serializable = static_cast(variable.ram_address); + + if (serializable == nullptr) { + uint8_t* aux = reinterpret_cast(variable.ram_address); + variable.buffer_address = buffer.size(); + this->buffer.insert(buffer.end(), aux, aux + variable.size); + } else { + std::vector aux = serializable->serialize(); + variable.buffer_address = buffer.size(); + this->buffer.insert(buffer.end(), aux.begin(), aux.end()); + variable.size = aux.size(); + } + } + + this->serialize_var_map(); + hal::Flash::write(this->start_page, 0, reinterpret_cast(buffer.data()), std::ceil(buffer.size() / 8.0f)); +} + +void Storage::serialize_var_map() { + // [total_size][num_vars][{[name_len][name][buffer_address][size]} + + for (auto [name, variable]: this->variables) { + this->buffer.insert(this->buffer.begin(), variable.size); + this->buffer.insert(this->buffer.begin(), variable.size >> 8); + + this->buffer.insert(this->buffer.begin(), variable.buffer_address); + this->buffer.insert(this->buffer.begin(), variable.buffer_address >> 8); + + this->buffer.insert(this->buffer.begin(), name.begin(), name.end()); + + this->buffer.insert(this->buffer.begin(), name.size()); + + uint32_t total_size = this->buffer.size(); + + this->buffer.insert(this->buffer.begin(), this->variables.size()); + this->buffer.insert(this->buffer.begin(), this->variables.size() >> 8); + this->buffer.insert(this->buffer.begin(), this->variables.size() >> 16); + this->buffer.insert(this->buffer.begin(), this->variables.size() >> 24); + + this->buffer.insert(this->buffer.begin(), total_size); + this->buffer.insert(this->buffer.begin(), total_size >> 8); + this->buffer.insert(this->buffer.begin(), total_size >> 16); + this->buffer.insert(this->buffer.begin(), total_size >> 24); + } +} +} // namespace proxy From b44c1c3872e51c19578242ebc03538e11af955a8 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 27 Mar 2024 06:23:32 -0300 Subject: [PATCH 097/561] =?UTF-8?q?=F0=9F=9A=A7=20=F0=9F=90=9B=20Fix=20ser?= =?UTF-8?q?ialization=20bug=20in=20Storage=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/storage.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index cf645ed..f27537f 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -105,18 +105,18 @@ void Storage::serialize_var_map() { this->buffer.insert(this->buffer.begin(), name.begin(), name.end()); this->buffer.insert(this->buffer.begin(), name.size()); + } - uint32_t total_size = this->buffer.size(); + uint32_t total_size = this->buffer.size(); - this->buffer.insert(this->buffer.begin(), this->variables.size()); - this->buffer.insert(this->buffer.begin(), this->variables.size() >> 8); - this->buffer.insert(this->buffer.begin(), this->variables.size() >> 16); - this->buffer.insert(this->buffer.begin(), this->variables.size() >> 24); + this->buffer.insert(this->buffer.begin(), this->variables.size()); + this->buffer.insert(this->buffer.begin(), this->variables.size() >> 8); + this->buffer.insert(this->buffer.begin(), this->variables.size() >> 16); + this->buffer.insert(this->buffer.begin(), this->variables.size() >> 24); - this->buffer.insert(this->buffer.begin(), total_size); - this->buffer.insert(this->buffer.begin(), total_size >> 8); - this->buffer.insert(this->buffer.begin(), total_size >> 16); - this->buffer.insert(this->buffer.begin(), total_size >> 24); - } + this->buffer.insert(this->buffer.begin(), total_size); + this->buffer.insert(this->buffer.begin(), total_size >> 8); + this->buffer.insert(this->buffer.begin(), total_size >> 16); + this->buffer.insert(this->buffer.begin(), total_size >> 24); } } // namespace proxy From 70969aae4d9307b6daf5dbfaa771562f76a1e8d7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 27 Mar 2024 21:24:02 -0300 Subject: [PATCH 098/561] =?UTF-8?q?=F0=9F=9A=91=20=F0=9F=A5=85=20?= =?UTF-8?q?=F0=9F=90=9B=20Fix=20storage=20proxy=20bugs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/serializable_interface.hpp | 6 +- inc/proxy/storage.hpp | 99 +++++++++++++++-- src/proxy/storage.cpp | 154 +++++++++++++++------------ 3 files changed, 179 insertions(+), 80 deletions(-) diff --git a/inc/proxy/serializable_interface.hpp b/inc/proxy/serializable_interface.hpp index 8e1103b..9387d0b 100644 --- a/inc/proxy/serializable_interface.hpp +++ b/inc/proxy/serializable_interface.hpp @@ -14,9 +14,11 @@ class ISerializable { public: - virtual std::vector && serialize() = 0; + virtual ~ISerializable() = default; - virtual void deserialize(uint8_t* serial_data, uint32_t size) = 0; + virtual std::vector serialize() = 0; + + virtual void deserialize(uint8_t* serial_data, uint16_t size) = 0; }; #endif // __SERIALIZABLE_INTERFACE_HPP__ diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp index b544ed8..f9fd42c 100644 --- a/inc/proxy/storage.hpp +++ b/inc/proxy/storage.hpp @@ -10,14 +10,17 @@ #define __STORAGE_HPP__ #include -#include #include +#include #include #include #include "serializable_interface.hpp" namespace proxy { +template +concept Fundamental = std::is_fundamental::value; + /** * @brief Class for controlling the storage */ @@ -38,31 +41,113 @@ class Storage { */ Storage(Config& config); - template + /** + * @brief Create a new primitive variable in the storage + * + * @tparam T Type of the variable + * @param name Name of the variable + * @param data Reference to the variable + */ + template void create(const std::string& name, T& data); + /** + * @brief Create a new serializable variable in the storage + * + * @param name Name of the variable + * @param data Reference to the variable + */ void create(const std::string& name, ISerializable& data); - template + /** + * @brief Sync a primitive variable with the storage + * + * @tparam T Type of the variable + * @param name Name of the variable + * @param data Reference to the variable + */ + template void sync(const std::string& name, T& data); + /** + * @brief Sync a serializable variable with the storage + * + * @param name Name of the variable + * @param data Reference to the variable + */ void sync(const std::string& name, ISerializable& data); + /** + * @brief Save the storage to the flash + */ void save(); private: - struct Variable { - void* ram_address; + /** + * @brief Structure for primitive variables + */ + struct PrimitiveVariable { + void* ram_pointer; uint16_t buffer_address; uint16_t size; }; - void serialize_var_map(); + /** + * @brief Structure for serializable variables + */ + struct SerializableVariable { + ISerializable* ram_pointer; + uint16_t buffer_address; + uint16_t size; + }; - std::unordered_map variables; + /** + * @brief Serialize a map of variables + * + * @tparam T Type of the variables + * @param variables Map of variables + * + * @return Serialized buffer + */ + template + static std::vector serialize_var_map(const std::unordered_map& variables); + + /** + * @brief Deserialize a map of variables + * + * @tparam T Type of the variables + * @param buffer Serialized buffer + * @param num_vars Number of variables + * + * @return Deserialized map of variables + */ + template + static std::unordered_map deserialize_var_map(std::vector& buffer, uint16_t num_vars); + + /** + * @brief Map of primitive variables + */ + std::unordered_map primitives; + /** + * @brief Map of serializable variables + */ + std::unordered_map serializables; + + /** + * @brief Serialized buffer for the storage + */ std::vector buffer; + + /** + * @brief Start page of the storage in the flash memory + */ uint16_t start_page; + + /** + * @brief Maximum number of pages used by the storage in the flash memory + */ + uint16_t number_of_pages; }; } // namespace proxy diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index f27537f..d12627b 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -6,117 +6,129 @@ * @date 03/2024 */ -#include - #include "hal/flash.hpp" #include "proxy/storage.hpp" namespace proxy { -Storage::Storage(Config& config) : start_page(config.start_page) { - // [total_size][num_vars][{[name_len][name][buffer_address][size]} - +Storage::Storage(Config& config) : start_page{config.start_page}, number_of_pages{config.number_of_pages} { uint64_t sizes; - hal::Flash::read(this->start_page, &sizes); + hal::Flash::read(this->start_page, 0, &sizes); uint32_t total_size = sizes >> 32; - uint32_t num_vars = sizes & 0xFFFFFFFF; - - this->buffer.reserve(total_size); - hal::Flash::read(this->start_page, 1, reinterpret_cast(this->buffer.data()), - std::ceil(total_size / 8.0f)); - - uint16_t i = 0; - - for (uint16_t decoded_vars = 0; decoded_vars < num_vars; decoded_vars++) { - uint8_t var_name_len = buffer.at(i); - std::string var_name(buffer.begin() + i + 1, buffer.begin() + i + 1 + var_name_len); - i += var_name_len + 1; + uint16_t num_primitives = sizes >> 16; + uint16_t num_serializables = sizes; - // read next 2 bytes as buffer address - this->variables[var_name].buffer_address = buffer.at(i) << 8 | buffer.at(i + 1); - i += 2; + this->buffer.resize(total_size); + hal::Flash::read(this->start_page, 1, reinterpret_cast(this->buffer.data()), total_size / 8); - // read next 2 bytes as size - this->variables.at(var_name).size = buffer.at(i) << 8 | buffer.at(i + 1); - i += 2; - } + this->primitives = deserialize_var_map(this->buffer, num_primitives); + this->serializables = deserialize_var_map(this->buffer, num_serializables); } -template +template void Storage::create(const std::string& name, T& data) { - this->variables[name].ram_address = &data; - this->variables.at(name).size = sizeof(T); + this->primitives[name].ram_pointer = &data; + this->primitives.at(name).size = sizeof(T); } void Storage::create(const std::string& name, ISerializable& data) { - this->variables[name].ram_address = &data; + this->serializables[name].ram_pointer = &data; } -template +template void Storage::sync(const std::string& name, T& data) { - if (variables.contains(name)) { - data = *reinterpret_cast(&this->buffer.at(variables.at(name).buffer_address)); + if (this->primitives.contains(name)) { + data = *reinterpret_cast(&this->buffer.at(this->primitives.at(name).buffer_address)); } - this->variables[name].ram_address = &data; - this->variables.at(name).size = sizeof(T); + this->create(name, data); } void Storage::sync(const std::string& name, ISerializable& data) { - if (variables.contains(name)) { - data.deserialize(&this->buffer.at(variables.at(name).buffer_address), variables.at(name).size); + if (this->serializables.contains(name)) { + const auto& serializable = this->serializables.at(name); + + data.deserialize(&this->buffer.at(serializable.buffer_address), serializable.size); } - this->variables[name].ram_address = &data; + this->create(name, data); } void Storage::save() { this->buffer.clear(); + hal::Flash::erase_pages(this->start_page, this->number_of_pages); - for (auto [name, variable]: this->variables) { - ISerializable* serializable = static_cast(variable.ram_address); - - if (serializable == nullptr) { - uint8_t* aux = reinterpret_cast(variable.ram_address); - variable.buffer_address = buffer.size(); - this->buffer.insert(buffer.end(), aux, aux + variable.size); - } else { - std::vector aux = serializable->serialize(); - variable.buffer_address = buffer.size(); - this->buffer.insert(buffer.end(), aux.begin(), aux.end()); - variable.size = aux.size(); - } + for (auto& [name, variable]: this->primitives) { + uint8_t* aux = reinterpret_cast(variable.ram_pointer); + variable.buffer_address = buffer.size(); + this->buffer.insert(this->buffer.end(), aux, aux + variable.size); } - this->serialize_var_map(); - hal::Flash::write(this->start_page, 0, reinterpret_cast(buffer.data()), std::ceil(buffer.size() / 8.0f)); -} + for (auto& [name, variable]: this->serializables) { + std::vector aux = variable.ram_pointer->serialize(); + variable.buffer_address = this->buffer.size(); + variable.size = aux.size(); + this->buffer.insert(this->buffer.end(), aux.begin(), aux.end()); + } + + auto serialized_serializables = serialize_var_map(this->serializables); + this->buffer.insert(this->buffer.begin(), serialized_serializables.begin(), serialized_serializables.end()); + + auto serialized_primitives = serialize_var_map(this->primitives); + this->buffer.insert(this->buffer.begin(), serialized_primitives.begin(), serialized_primitives.end()); -void Storage::serialize_var_map() { - // [total_size][num_vars][{[name_len][name][buffer_address][size]} + this->buffer.insert(this->buffer.end(), (8 - (this->buffer.size() % 8)) % 8, 0); + uint32_t total_size = this->buffer.size(); + + this->buffer.emplace_back(this->serializables.size()); + this->buffer.emplace_back(this->serializables.size() >> 8); + this->buffer.emplace_back(this->primitives.size()); + this->buffer.emplace_back(this->primitives.size() >> 8); + + this->buffer.emplace_back(total_size); + this->buffer.emplace_back(total_size >> 8); + this->buffer.emplace_back(total_size >> 16); + this->buffer.emplace_back(total_size >> 24); + + hal::Flash::write(this->start_page, 0, reinterpret_cast(buffer.data()), buffer.size() / 8); +} - for (auto [name, variable]: this->variables) { - this->buffer.insert(this->buffer.begin(), variable.size); - this->buffer.insert(this->buffer.begin(), variable.size >> 8); +template +std::vector Storage::serialize_var_map(const std::unordered_map& variables) { + std::vector buffer; - this->buffer.insert(this->buffer.begin(), variable.buffer_address); - this->buffer.insert(this->buffer.begin(), variable.buffer_address >> 8); + for (auto [name, variable]: variables) { + buffer.emplace_back(name.size()); + buffer.insert(buffer.end(), name.begin(), name.end()); - this->buffer.insert(this->buffer.begin(), name.begin(), name.end()); + buffer.emplace_back(variable.buffer_address); + buffer.emplace_back(variable.buffer_address >> 8); - this->buffer.insert(this->buffer.begin(), name.size()); + buffer.emplace_back(variable.size); + buffer.emplace_back(variable.size >> 8); } - uint32_t total_size = this->buffer.size(); + return buffer; +} + +template +std::unordered_map Storage::deserialize_var_map(std::vector& buffer, uint16_t num_vars) { + std::unordered_map variables; + uint16_t i = 0; + + for (uint16_t decoded_vars = 0; decoded_vars < num_vars; decoded_vars++) { + uint8_t var_name_len = buffer.at(i); + std::string var_name(buffer.begin() + i + 1, buffer.begin() + i + 1 + var_name_len); + i += var_name_len + 1; + + variables[var_name].buffer_address = buffer.at(i) | buffer.at(i + 1) << 8; + i += 2; - this->buffer.insert(this->buffer.begin(), this->variables.size()); - this->buffer.insert(this->buffer.begin(), this->variables.size() >> 8); - this->buffer.insert(this->buffer.begin(), this->variables.size() >> 16); - this->buffer.insert(this->buffer.begin(), this->variables.size() >> 24); + variables.at(var_name).size = buffer.at(i) | buffer.at(i + 1) << 8; + i += 2; + } - this->buffer.insert(this->buffer.begin(), total_size); - this->buffer.insert(this->buffer.begin(), total_size >> 8); - this->buffer.insert(this->buffer.begin(), total_size >> 16); - this->buffer.insert(this->buffer.begin(), total_size >> 24); + buffer.erase(buffer.begin(), buffer.begin() + i); + return variables; } } // namespace proxy From 014d999cda19d8e16c705a3a73b91fa72de225cf Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 27 Mar 2024 22:40:31 -0300 Subject: [PATCH 099/561] =?UTF-8?q?=F0=9F=94=92=EF=B8=8F=20Add=20protectio?= =?UTF-8?q?n=20on=20storage=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/storage.hpp | 4 ++-- src/proxy/storage.cpp | 15 ++++++++++++--- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp index f9fd42c..f65c05f 100644 --- a/inc/proxy/storage.hpp +++ b/inc/proxy/storage.hpp @@ -87,7 +87,7 @@ class Storage { * @brief Structure for primitive variables */ struct PrimitiveVariable { - void* ram_pointer; + void* ram_pointer{nullptr}; uint16_t buffer_address; uint16_t size; }; @@ -96,7 +96,7 @@ class Storage { * @brief Structure for serializable variables */ struct SerializableVariable { - ISerializable* ram_pointer; + ISerializable* ram_pointer{nullptr}; uint16_t buffer_address; uint16_t size; }; diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index d12627b..f851dee 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -37,7 +37,7 @@ void Storage::create(const std::string& name, ISerializable& data) { template void Storage::sync(const std::string& name, T& data) { - if (this->primitives.contains(name)) { + if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr) { data = *reinterpret_cast(&this->buffer.at(this->primitives.at(name).buffer_address)); } @@ -45,9 +45,8 @@ void Storage::sync(const std::string& name, T& data) { } void Storage::sync(const std::string& name, ISerializable& data) { - if (this->serializables.contains(name)) { + if (this->serializables.contains(name) and this->serializables.at(name).ram_pointer == nullptr) { const auto& serializable = this->serializables.at(name); - data.deserialize(&this->buffer.at(serializable.buffer_address), serializable.size); } @@ -59,12 +58,22 @@ void Storage::save() { hal::Flash::erase_pages(this->start_page, this->number_of_pages); for (auto& [name, variable]: this->primitives) { + if (variable.ram_pointer == nullptr) { + this->primitives.erase(name); + continue; + } + uint8_t* aux = reinterpret_cast(variable.ram_pointer); variable.buffer_address = buffer.size(); this->buffer.insert(this->buffer.end(), aux, aux + variable.size); } for (auto& [name, variable]: this->serializables) { + if (variable.ram_pointer == nullptr) { + this->serializables.erase(name); + continue; + } + std::vector aux = variable.ram_pointer->serialize(); variable.buffer_address = this->buffer.size(); variable.size = aux.size(); From 2c7b477ff70ff1ff72b004e291b4a0eb0e74951d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 27 Mar 2024 23:30:01 -0300 Subject: [PATCH 100/561] =?UTF-8?q?=F0=9F=94=92=EF=B8=8F=20Add=20missing?= =?UTF-8?q?=20const=20specifiers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/encoder.hpp | 2 +- inc/hal/flash.hpp | 4 ++-- inc/hal/pwm_dma.hpp | 2 +- inc/proxy/battery.hpp | 4 ++-- inc/proxy/rotary_sensor.hpp | 2 +- src/hal/encoder.cpp | 2 +- src/hal/flash.cpp | 4 ++-- src/hal/pwm_dma.cpp | 2 +- src/proxy/battery.cpp | 4 ++-- src/proxy/rotary_sensor.cpp | 2 +- 10 files changed, 14 insertions(+), 14 deletions(-) diff --git a/inc/hal/encoder.hpp b/inc/hal/encoder.hpp index 16724e7..a69620b 100644 --- a/inc/hal/encoder.hpp +++ b/inc/hal/encoder.hpp @@ -38,7 +38,7 @@ class Encoder { * * @return int32_t Current value of the counter */ - int32_t get_counter(); + int32_t get_counter() const; private: /** diff --git a/inc/hal/flash.hpp b/inc/hal/flash.hpp index 0448cbb..2733107 100644 --- a/inc/hal/flash.hpp +++ b/inc/hal/flash.hpp @@ -49,7 +49,7 @@ class Flash { * @param data pointer to the data to write * @param size size in double words of the data to write */ - static void write(uint32_t address, uint64_t data[], uint32_t size = 1); + static void write(uint32_t address, const uint64_t data[], uint32_t size = 1); /** * @brief Write data to flash memory @@ -59,7 +59,7 @@ class Flash { * @param data pointer to the data to write * @param size size in double words of the data to write */ - static void write(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); + static void write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size = 1); /** * @brief Erase flash memory pages diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp index 9166aa2..3cb7d8b 100644 --- a/inc/hal/pwm_dma.hpp +++ b/inc/hal/pwm_dma.hpp @@ -53,7 +53,7 @@ class PwmDma { * @param duty_cycle Duty cycle to get the compare value for * @return uint32_t Compare value for the duty cycle */ - uint32_t get_compare(float duty_cycle); + uint32_t get_compare(float duty_cycle) const; private: /** diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp index 7050611..9cdc25e 100644 --- a/inc/proxy/battery.hpp +++ b/inc/proxy/battery.hpp @@ -39,14 +39,14 @@ class Battery { * * @return Battery voltage in volts */ - float get_voltage(); + float get_voltage() const; /** * @brief Get the raw reading from the battery * * @return uint32_t Raw reading from the battery */ - uint32_t get_voltage_raw(); + uint32_t get_voltage_raw() const; private: /** diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index 3453e44..044fbbf 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -46,7 +46,7 @@ class RotarySensor { * * @return Current angular position of the sensor in radians */ - float get_position(); + float get_position() const; private: union CommandFrame { diff --git a/src/hal/encoder.cpp b/src/hal/encoder.cpp index 1f32d33..1fa5e73 100644 --- a/src/hal/encoder.cpp +++ b/src/hal/encoder.cpp @@ -14,7 +14,7 @@ Encoder::Encoder(Config& config) : handle{config.handle} { HAL_TIM_Encoder_Start(this->handle, config.timer_channel); } -int32_t Encoder::get_counter() { +int32_t Encoder::get_counter() const { return static_cast(__HAL_TIM_GET_COUNTER(this->handle)); } } // namespace hal diff --git a/src/hal/flash.cpp b/src/hal/flash.cpp index 15f1266..34862fe 100644 --- a/src/hal/flash.cpp +++ b/src/hal/flash.cpp @@ -27,7 +27,7 @@ void Flash::read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t read(page * double_words_per_page + page_address, data, size); } -void Flash::write(uint32_t address, uint64_t data[], uint32_t size) { +void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { HAL_FLASH_Unlock(); uint32_t end = base_address - 8 * address; @@ -50,7 +50,7 @@ void Flash::write(uint32_t address, uint64_t data[], uint32_t size) { HAL_FLASH_Lock(); } -void Flash::write(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size) { +void Flash::write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size) { write(page * double_words_per_page + page_address, data, size); } diff --git a/src/hal/pwm_dma.cpp b/src/hal/pwm_dma.cpp index 1124dde..5eed04f 100644 --- a/src/hal/pwm_dma.cpp +++ b/src/hal/pwm_dma.cpp @@ -27,7 +27,7 @@ void PwmDma::stop_dma() { HAL_TIM_PWM_Stop_DMA(this->handle, this->channel); } -uint32_t PwmDma::get_compare(float duty_cycle) { +uint32_t PwmDma::get_compare(float duty_cycle) const { return std::round((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); } } // namespace hal diff --git a/src/proxy/battery.cpp b/src/proxy/battery.cpp index 08044dd..62055b2 100644 --- a/src/proxy/battery.cpp +++ b/src/proxy/battery.cpp @@ -13,11 +13,11 @@ Battery::Battery(Config& config) : adc{config.adc}, voltage_divider{config.volta this->adc.start_dma(&(this->raw_reading), 1); } -float Battery::get_voltage() { +float Battery::get_voltage() const { return this->voltage_divider * this->raw_reading * this->adc.reference_voltage / this->adc.max_reading; } -uint32_t Battery::get_voltage_raw() { +uint32_t Battery::get_voltage_raw() const { return this->raw_reading; } } // namespace proxy diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index d44c0b4..9b14f8d 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -45,7 +45,7 @@ RotarySensor::RotarySensor(Config& config) : this->write_register(command_frame, data_frame); } -float RotarySensor::get_position() { +float RotarySensor::get_position() const { return encoder.get_counter() * 2 * std::numbers::pi_v / this->resolution; } From eba536abdf75ccf01ec488df3f241364f8fc1311 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 27 Mar 2024 23:35:52 -0300 Subject: [PATCH 101/561] =?UTF-8?q?=F0=9F=94=92=EF=B8=8F=20Add=20const=20s?= =?UTF-8?q?pecifiers=20to=20storage=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/serializable_interface.hpp | 2 +- inc/proxy/storage.hpp | 16 ++++++++-------- src/proxy/storage.cpp | 6 +++--- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/inc/proxy/serializable_interface.hpp b/inc/proxy/serializable_interface.hpp index 9387d0b..2304477 100644 --- a/inc/proxy/serializable_interface.hpp +++ b/inc/proxy/serializable_interface.hpp @@ -16,7 +16,7 @@ class ISerializable { public: virtual ~ISerializable() = default; - virtual std::vector serialize() = 0; + virtual std::vector serialize() const = 0; virtual void deserialize(uint8_t* serial_data, uint16_t size) = 0; }; diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp index f65c05f..9acb48b 100644 --- a/inc/proxy/storage.hpp +++ b/inc/proxy/storage.hpp @@ -49,7 +49,7 @@ class Storage { * @param data Reference to the variable */ template - void create(const std::string& name, T& data); + void create(const std::string& name, const T& data); /** * @brief Create a new serializable variable in the storage @@ -57,7 +57,7 @@ class Storage { * @param name Name of the variable * @param data Reference to the variable */ - void create(const std::string& name, ISerializable& data); + void create(const std::string& name, const ISerializable& data); /** * @brief Sync a primitive variable with the storage @@ -87,18 +87,18 @@ class Storage { * @brief Structure for primitive variables */ struct PrimitiveVariable { - void* ram_pointer{nullptr}; - uint16_t buffer_address; - uint16_t size; + const void* ram_pointer{nullptr}; + uint16_t buffer_address; + uint16_t size; }; /** * @brief Structure for serializable variables */ struct SerializableVariable { - ISerializable* ram_pointer{nullptr}; - uint16_t buffer_address; - uint16_t size; + const ISerializable* ram_pointer{nullptr}; + uint16_t buffer_address; + uint16_t size; }; /** diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index f851dee..10f9d80 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -26,12 +26,12 @@ Storage::Storage(Config& config) : start_page{config.start_page}, number_of_page } template -void Storage::create(const std::string& name, T& data) { +void Storage::create(const std::string& name, const T& data) { this->primitives[name].ram_pointer = &data; this->primitives.at(name).size = sizeof(T); } -void Storage::create(const std::string& name, ISerializable& data) { +void Storage::create(const std::string& name, const ISerializable& data) { this->serializables[name].ram_pointer = &data; } @@ -63,7 +63,7 @@ void Storage::save() { continue; } - uint8_t* aux = reinterpret_cast(variable.ram_pointer); + const uint8_t* aux = reinterpret_cast(variable.ram_pointer); variable.buffer_address = buffer.size(); this->buffer.insert(this->buffer.end(), aux, aux + variable.size); } From fbb1c7e5c375c43a8ea863b68003c6befe01803d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 28 Mar 2024 02:26:35 -0300 Subject: [PATCH 102/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20imu=20proxy=20retu?= =?UTF-8?q?rn=20in=20switch?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/imu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index a5f208a..409c992 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -56,15 +56,15 @@ float Imu::get_angular_velocity(Axis axis) { switch (axis) { case Axis::X: - return ang_vel = convert_ang_vel(this->angular_velocity[0]); + ang_vel = convert_ang_vel(this->angular_velocity[0]); break; case Axis::Y: - return ang_vel = convert_ang_vel(this->angular_velocity[1]); + ang_vel = convert_ang_vel(this->angular_velocity[1]); break; case Axis::Z: - return ang_vel = convert_ang_vel(this->angular_velocity[2]); + ang_vel = convert_ang_vel(this->angular_velocity[2]); break; default: From 50fe7057838969ab0632436c3e6c9afb43d95ce3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 28 Mar 2024 04:23:40 -0300 Subject: [PATCH 103/561] =?UTF-8?q?=E2=9C=A8=20Add=20imu=20proxy=20fifo=20?= =?UTF-8?q?mode=20and=20sensor=20fusion=20orientation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/imu.hpp | 54 +++++++++++----- src/proxy/imu.cpp | 154 ++++++++++++++++++++++++++++++++++++---------- 2 files changed, 157 insertions(+), 51 deletions(-) diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 6b030b2..9e2b2de 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -29,10 +29,9 @@ class Imu { hal::Spi::Config spi; lsm6dsv_data_rate_t gyroscope_data_rate; lsm6dsv_data_rate_t accelerometer_data_rate; - lsm6dsv_gy_full_scale_t gyroscope_full_scale; - lsm6dsv_xl_full_scale_t accelerometer_full_scale; - float (* convert_ang_vel)(int16_t); - float (* convert_lin_acc)(int16_t); + lsm6dsv_sflp_data_rate_t orientation_data_rate; + lsm6dsv_gy_full_scale_t gyroscope_scale; + lsm6dsv_xl_full_scale_t accelerometer_scale; lsm6dsv_filt_gy_lp1_bandwidth_t gyroscope_filter; lsm6dsv_filt_xl_lp2_bandwidth_t accelerometer_filter; }; @@ -51,6 +50,11 @@ class Imu { Z }; + /** + * @brief Update the IMU data + */ + void update_data(); + /** * @brief Get the IMU orientation over an axis * @todo implement function using sensior fusion @@ -59,7 +63,7 @@ class Imu { * * @return Orientation over the desired axis using quaternions */ - float get_orientation(Axis axis); + float get_orientation(Axis axis) const; /** * @brief Get the IMU angular velocity over an axis @@ -68,7 +72,7 @@ class Imu { * * @return Angular velocity over the desired axis in rad/s */ - float get_angular_velocity(Axis axis); + float get_angular_velocity(Axis axis) const; /** * @brief Get the IMU linear acceleration over an axis @@ -77,7 +81,7 @@ class Imu { * * @return Linear acceleration over the desired axis in m/sยฒ */ - float get_linear_acceleration(Axis axis); + float get_linear_acceleration(Axis axis) const; private: /** @@ -105,28 +109,29 @@ class Imu { static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len); /** - * @brief Function to convert raw data to angular velocity + * @brief Function to convert raw data to orientation * - * @param raw_data Raw data from the IMU + * @param quat Quaternion to store the orientation + * @param sflp Raw data from the IMU * - * @return Angular velocity in md/s + * @return Orientation in quaternions */ - float (* convert_ang_vel)(int16_t); + static void convert_orientation(std::array& quat, const uint16_t sflp[3]); /** - * @brief Function to convert raw data to linear acceleration + * @brief Function to convert half precision float to single precision float * - * @param raw_data Raw data from the IMU + * @param x Half precision float * - * @return Linear acceleration in mg + * @return Single precision float */ - float (* convert_lin_acc)(int16_t); + static float half_to_float(uint16_t x); /** * @brief Conversion constants */ - static constexpr float mg_to_mps2{0.00980665}; - static constexpr float mdps_to_radps{std::numbers::pi_v / 180000}; + static constexpr float mdps_to_radps{std::numbers::pi_v / 180000.0f}; + static constexpr float mg_to_mps2{0.00980665f}; /** * @brief SPI for the IMU communication @@ -147,6 +152,21 @@ class Imu { * @brief Current linear acceleration on each axis */ std::array linear_acceleration; + + /** + * @brief Current orientation + */ + std::array orientation; + + /** + * @brief Gyroscope conversion factor + */ + const float gy_factor; + + /** + * @brief Accelerometer conversion factor + */ + const float xl_factor; }; } // namespace proxy diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 409c992..b8005ed 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -6,12 +6,16 @@ * @date 03/2024 */ +#include + #include "hal/timer.hpp" #include "proxy/imu.hpp" namespace proxy { -Imu::Imu(Config& config) : - convert_ang_vel{config.convert_ang_vel}, convert_lin_acc{config.convert_lin_acc}, spi{config.spi} { +Imu::Imu(Config& config) : spi{config.spi}, + gy_factor{mdps_to_radps * 4.375f * + (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale)))}, + xl_factor{mg_to_mps2 * (0.61f * (1 << static_cast(config.accelerometer_scale)))} { this->dev_ctx.read_reg = platform_read; this->dev_ctx.write_reg = platform_write; this->dev_ctx.handle = &config.spi; @@ -27,75 +31,118 @@ Imu::Imu(Config& config) : } while (rst != LSM6DSV_READY); lsm6dsv_block_data_update_set(&(this->dev_ctx), PROPERTY_ENABLE); + lsm6dsv_fifo_watermark_set(&(this->dev_ctx), 3); + lsm6dsv_fifo_stop_on_wtm_set(&(this->dev_ctx), PROPERTY_ENABLE); + lsm6dsv_fifo_mode_set(&(this->dev_ctx), LSM6DSV_STREAM_MODE); lsm6dsv_gy_data_rate_set(&(this->dev_ctx), config.gyroscope_data_rate); lsm6dsv_xl_data_rate_set(&(this->dev_ctx), config.accelerometer_data_rate); + lsm6dsv_sflp_data_rate_set(&(this->dev_ctx), config.orientation_data_rate); + + lsm6dsv_fifo_gy_batch_set(&(this->dev_ctx), static_cast(config.gyroscope_data_rate)); + lsm6dsv_fifo_xl_batch_set(&(this->dev_ctx), static_cast(config.accelerometer_data_rate)); + lsm6dsv_fifo_sflp_batch_set(&dev_ctx, {.game_rotation = 1, .gravity = 0, .gbias = 0}); - lsm6dsv_gy_full_scale_set(&(this->dev_ctx), config.gyroscope_full_scale); - lsm6dsv_xl_full_scale_set(&(this->dev_ctx), config.accelerometer_full_scale); + lsm6dsv_gy_full_scale_set(&(this->dev_ctx), config.gyroscope_scale); + lsm6dsv_xl_full_scale_set(&(this->dev_ctx), config.accelerometer_scale); - lsm6dsv_filt_settling_mask_t filt_settling_mask; - filt_settling_mask.drdy = PROPERTY_ENABLE; - filt_settling_mask.irq_xl = PROPERTY_ENABLE; - filt_settling_mask.irq_g = PROPERTY_ENABLE; - lsm6dsv_filt_settling_mask_set(&dev_ctx, filt_settling_mask); + lsm6dsv_filt_settling_mask_set(&dev_ctx, {.drdy = 1, .ois_drdy = 0, .irq_xl = 1, .irq_g = 1}); lsm6dsv_filt_gy_lp1_set(&(this->dev_ctx), PROPERTY_ENABLE); lsm6dsv_filt_gy_lp1_bandwidth_set(&(this->dev_ctx), config.gyroscope_filter); lsm6dsv_filt_xl_lp2_set(&(this->dev_ctx), PROPERTY_ENABLE); lsm6dsv_filt_xl_lp2_bandwidth_set(&(this->dev_ctx), config.accelerometer_filter); -} -float Imu::get_orientation(Axis /*axis*/) { - return 0.0f; + lsm6dsv_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE); } -float Imu::get_angular_velocity(Axis axis) { - lsm6dsv_angular_rate_raw_get(&(this->dev_ctx), this->angular_velocity.data()); - float ang_vel; +void Imu::update_data() { + lsm6dsv_fifo_status_t fifo_status; + lsm6dsv_fifo_status_get(&(this->dev_ctx), &fifo_status); + + if (not fifo_status.fifo_th) { + return; + } + + uint16_t samples = fifo_status.fifo_level; + + while (samples--) { + lsm6dsv_fifo_out_raw_t f_data; + lsm6dsv_fifo_out_raw_get(&(this->dev_ctx), &f_data); + int16_t* axis = reinterpret_cast(f_data.data); + + switch (f_data.tag) { + case lsm6dsv_fifo_out_raw_t::LSM6DSV_GY_NC_TAG: + this->angular_velocity[0] = axis[0] * gy_factor; + this->angular_velocity[1] = axis[1] * gy_factor; + this->angular_velocity[2] = axis[2] * gy_factor; + break; + + case lsm6dsv_fifo_out_raw_t::LSM6DSV_XL_NC_TAG: + this->linear_acceleration[0] = axis[0] * xl_factor; + this->linear_acceleration[1] = axis[1] * xl_factor; + this->linear_acceleration[2] = axis[2] * xl_factor; + break; + + case lsm6dsv_fifo_out_raw_t::LSM6DSV_SFLP_GAME_ROTATION_VECTOR_TAG: + convert_orientation(this->orientation, reinterpret_cast(axis)); + break; + + default: + break; + } + } +} +float Imu::get_orientation(Axis axis) const { switch (axis) { + case Axis::W: + return this->orientation[3]; + case Axis::X: - ang_vel = convert_ang_vel(this->angular_velocity[0]); - break; + return this->orientation[0]; case Axis::Y: - ang_vel = convert_ang_vel(this->angular_velocity[1]); - break; + return this->orientation[1]; case Axis::Z: - ang_vel = convert_ang_vel(this->angular_velocity[2]); - break; + return this->orientation[2]; default: return 0.0f; } - - return ang_vel * mdps_to_radps; } -float Imu::get_linear_acceleration(Axis axis) { - lsm6dsv_acceleration_raw_get(&(this->dev_ctx), this->linear_acceleration.data()); - float lin_acc; - +float Imu::get_angular_velocity(Axis axis) const { switch (axis) { case Axis::X: - lin_acc = this->convert_lin_acc(this->linear_acceleration[0]); - break; + return this->angular_velocity[0]; case Axis::Y: - lin_acc = this->convert_lin_acc(this->linear_acceleration[1]); - break; + return this->angular_velocity[1]; case Axis::Z: - lin_acc = this->convert_lin_acc(this->linear_acceleration[2]); - break; + return this->angular_velocity[2]; default: return 0.0f; } +} + +float Imu::get_linear_acceleration(Axis axis) const { + switch (axis) { + case Axis::X: + return this->linear_acceleration[0]; + + case Axis::Y: + return this->linear_acceleration[1]; + + case Axis::Z: + return this->linear_acceleration[2]; - return lin_acc * mg_to_mps2; + default: + return 0.0f; + } } int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) { @@ -126,4 +173,43 @@ int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint return 0; }; + +void Imu::convert_orientation(std::array& quat, const uint16_t sflp[3]) { + float sumsq = 0; + + quat[0] = half_to_float(sflp[0]); + quat[1] = half_to_float(sflp[1]); + quat[2] = half_to_float(sflp[2]); + + for (uint8_t i = 0; i < 3; i++) { + sumsq += quat[i] * quat[i]; + } + + if (sumsq > 1.0f) { + float n = std::sqrt(sumsq); + quat[0] /= n; + quat[1] /= n; + quat[2] /= n; + sumsq = 1.0f; + } + + quat[3] = std::sqrt(1.0f - sumsq); +} + +float Imu::half_to_float(const uint16_t x) { + static auto as_float = [](uint32_t x) -> float { + return *reinterpret_cast(&x); + }; + static auto as_uint = [](float x) -> uint32_t { + return *reinterpret_cast(&x); + }; + + const uint32_t e = (x & 0x7C00) >> 10; + const uint32_t m = (x & 0x03FF) << 13; + const uint32_t v = as_uint(static_cast(m)) >> 23; + return as_float( + (x & 0x8000) << 16 | + (e != 0) * ((e + 112) << 23 | m) | + ((e == 0) & (m != 0)) * ((v - 37) << 23 | ((m << (150 - v)) & 0x007FE000))); +} } // namespace proxy From df4d101dc900b0c3383da61f11f81d8c1c6000e2 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 03:00:54 -0300 Subject: [PATCH 104/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20compiler=20warning?= =?UTF-8?q?s?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/mcu.hpp | 2 +- inc/proxy/rotary_sensor.hpp | 6 ++++-- inc/proxy/rotary_sensor_reg.hpp | 27 +++++++++++++++++---------- src/proxy/imu.cpp | 2 +- src/proxy/rotary_sensor.cpp | 32 ++++++++++++++++---------------- 5 files changed, 39 insertions(+), 30 deletions(-) diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index 8dcf203..36d5354 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -22,5 +22,5 @@ class Mcu { */ static void init(void); }; -}; // namespace hal +} // namespace hal #endif // __MCU_HPP__ diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index 044fbbf..4ace2f4 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -50,24 +50,26 @@ class RotarySensor { private: union CommandFrame { - struct __attribute__((packed)) { + struct Fields { uint8_t do_not_care : 1; uint8_t rw : 1; uint16_t address : 14; uint8_t crc : 8; }; + Fields fields; uint32_t raw; }; union DataFrame { - struct __attribute__((packed)) { + struct Fields { uint8_t warning : 1; uint8_t error : 1; uint16_t data : 14; uint8_t crc : 8; }; + Fields fields; uint32_t raw; }; diff --git a/inc/proxy/rotary_sensor_reg.hpp b/inc/proxy/rotary_sensor_reg.hpp index 4287f77..cd01291 100644 --- a/inc/proxy/rotary_sensor_reg.hpp +++ b/inc/proxy/rotary_sensor_reg.hpp @@ -14,47 +14,51 @@ struct Registers { union Disable { - struct __attribute__((packed)) { + struct Fields { uint8_t UVW_off : 1; uint8_t ABI_off : 1; uint8_t na : 4; uint8_t FILTER_disable : 1; }; + Fields fields; uint8_t raw; }; union Zposm { - struct __attribute__((packed)) { + struct Fields { uint8_t ZPOSM : 8; }; + Fields fields; uint8_t raw; }; - union Zpols { - struct __attribute__((packed)) { + union Zposl { + struct Fields { uint8_t ZPOSL : 6; uint8_t Dia1_en : 1; uint8_t Dia2_en : 1; }; + Fields Fields; uint8_t raw; }; union Settings1 { - struct __attribute__((packed)) { + struct Fields { uint8_t K_max : 3; uint8_t K_min : 3; uint8_t Dia3_en : 1; uint8_t Dia4_en : 1; }; + Fields fields; uint8_t raw; }; union Settings2 { - struct __attribute__((packed)) { + struct Fields { uint8_t IWIDTH : 1; uint8_t NOISESET : 1; uint8_t DIR : 1; @@ -65,31 +69,34 @@ struct Registers { uint8_t PWMon : 1; }; + Fields fields; uint8_t raw; }; union Settings3 { - struct __attribute__((packed)) { + struct Fields { uint8_t UVWPP : 3; uint8_t HYS : 2; uint8_t ABIRES : 3; }; + Fields fields; uint8_t raw; }; union Ecc { - struct __attribute__((packed)) { + struct Fields { uint8_t ECC_chsum : 7; uint8_t ECC_en : 1; }; + Fields fields; uint8_t raw; }; static constexpr uint16_t disable_addr{0x0015}; static constexpr uint16_t zposm_addr{0x0016}; - static constexpr uint16_t zpos_addr{0x0017}; + static constexpr uint16_t zposl_addr{0x0017}; static constexpr uint16_t settings1_addr{0x0018}; static constexpr uint16_t settings2_addr{0x0019}; static constexpr uint16_t settings3_addr{0x001A}; @@ -97,7 +104,7 @@ struct Registers { Disable disable; Zposm zposm; - Zpols zpos; + Zposl zposl; Settings1 settings1; Settings2 settings2; Settings3 settings3; diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index b8005ed..fe154a5 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -172,7 +172,7 @@ int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint spi->unselect_device(); return 0; -}; +} void Imu::convert_orientation(std::array& quat, const uint16_t sflp[3]) { float sumsq = 0; diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index 9b14f8d..ae7f848 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -16,32 +16,32 @@ RotarySensor::RotarySensor(Config& config) : CommandFrame command_frame; DataFrame data_frame; - command_frame.address = Registers::disable_addr; - data_frame.data = config.registers.disable.raw; + command_frame.fields.address = Registers::disable_addr; + data_frame.fields.data = config.registers.disable.raw; this->write_register(command_frame, data_frame); - command_frame.address = Registers::zposm_addr; - data_frame.data = config.registers.zposm.raw; + command_frame.fields.address = Registers::zposm_addr; + data_frame.fields.data = config.registers.zposm.raw; this->write_register(command_frame, data_frame); - command_frame.address = Registers::zpos_addr; - data_frame.data = config.registers.zpos.raw; + command_frame.fields.address = Registers::zposl_addr; + data_frame.fields.data = config.registers.zposl.raw; this->write_register(command_frame, data_frame); - command_frame.address = Registers::settings1_addr; - data_frame.data = config.registers.settings1.raw; + command_frame.fields.address = Registers::settings1_addr; + data_frame.fields.data = config.registers.settings1.raw; this->write_register(command_frame, data_frame); - command_frame.address = Registers::settings2_addr; - data_frame.data = config.registers.settings2.raw; + command_frame.fields.address = Registers::settings2_addr; + data_frame.fields.data = config.registers.settings2.raw; this->write_register(command_frame, data_frame); - command_frame.address = Registers::settings3_addr; - data_frame.data = config.registers.settings3.raw; + command_frame.fields.address = Registers::settings3_addr; + data_frame.fields.data = config.registers.settings3.raw; this->write_register(command_frame, data_frame); - command_frame.address = Registers::ecc_addr; - data_frame.data = config.registers.ecc.raw; + command_frame.fields.address = Registers::ecc_addr; + data_frame.fields.data = config.registers.ecc.raw; this->write_register(command_frame, data_frame); } @@ -54,8 +54,8 @@ void RotarySensor::write_register(CommandFrame& command_frame, DataFrame& data_f continue; } - command_frame.crc = this->crc.calculate(&command_frame.raw, 2); - data_frame.crc = this->crc.calculate(&data_frame.raw, 2); + command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2); + data_frame.fields.crc = this->crc.calculate(&data_frame.raw, 2); this->spi.transmit(reinterpret_cast(&command_frame.raw), 3); this->spi.transmit(reinterpret_cast(&data_frame.raw), 3); From 593a7a271549cb9296020c99d5d23e4390cc08a8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 16:28:42 -0300 Subject: [PATCH 105/561] =?UTF-8?q?=F0=9F=A7=90=20Add=20linter=20check=20t?= =?UTF-8?q?o=20CMake?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 .clang-tidy diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..718f9d3 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,24 @@ +--- +Checks: + -*, + bugprone-*, + clang-*, + cppcoreguidelines-*, + -cppcoreguidelines-avoid-magic-numbers, + -cppcoreguidelines-pro-type-union-access, + -cppcoreguidelines-avoid-c-arrays, + -cppcoreguidelines-pro-type-reinterpret-cast, + google-*, + llvm-*, + misc-*, + modernize-*, + -modernize-use-trailing-return-type, + -modernize-avoid-c-arrays, + performance-*, + readability-*, + -readability-magic-numbers, +WarningsAsErrors: '' +HeaderFilterRegex: '' +AnalyzeTemporaryDtors: false +FormatStyle: none +... From 1536699c037110033b7db16923f57b460f833bfc Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 16:55:36 -0300 Subject: [PATCH 106/561] =?UTF-8?q?=F0=9F=8E=A8=20Change=20f=20to=20F=20on?= =?UTF-8?q?=20float=20values?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/argb.hpp | 4 ++-- inc/proxy/imu.hpp | 4 ++-- src/proxy/buzzer.cpp | 4 ++-- src/proxy/distance_sensors.cpp | 2 +- src/proxy/fan.cpp | 6 +++--- src/proxy/imu.cpp | 16 +++++++-------- src/proxy/locomotion.cpp | 36 +++++++++++++++++----------------- 7 files changed, 36 insertions(+), 36 deletions(-) diff --git a/inc/proxy/argb.hpp b/inc/proxy/argb.hpp index 3bcb958..a287d6b 100644 --- a/inc/proxy/argb.hpp +++ b/inc/proxy/argb.hpp @@ -86,8 +86,8 @@ class Argb { static constexpr uint8_t bits_per_color{8}; static constexpr uint8_t colors_per_led{3}; static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; - static constexpr float low_duty_cycle{0.32f}; - static constexpr float high_duty_cycle{0.64f}; + static constexpr float low_duty_cycle{0.32F}; + static constexpr float high_duty_cycle{0.64F}; static constexpr uint8_t reset_length{40}; /** diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 9e2b2de..f98d35a 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -130,8 +130,8 @@ class Imu { /** * @brief Conversion constants */ - static constexpr float mdps_to_radps{std::numbers::pi_v / 180000.0f}; - static constexpr float mg_to_mps2{0.00980665f}; + static constexpr float mdps_to_radps{std::numbers::pi_v / 180000.0F}; + static constexpr float mg_to_mps2{0.00980665F}; /** * @brief SPI for the IMU communication diff --git a/src/proxy/buzzer.cpp b/src/proxy/buzzer.cpp index 1555d13..43349a6 100644 --- a/src/proxy/buzzer.cpp +++ b/src/proxy/buzzer.cpp @@ -14,7 +14,7 @@ Buzzer::Buzzer(Config& config) : pwm{config.pwm} { } void Buzzer::play(uint32_t frequency, uint32_t duration) { - this->pwm.set_duty_cycle(50.0f); + this->pwm.set_duty_cycle(50.0F); this->pwm.set_frequency(frequency); this->is_playing = true; @@ -32,6 +32,6 @@ void Buzzer::update() { void Buzzer::stop() { this->is_playing = false; - this->pwm.set_duty_cycle(0.0f); + this->pwm.set_duty_cycle(0.0F); } } // namespace proxy diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index 9e4cd49..2d38d73 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -16,7 +16,7 @@ template DistanceSensors::DistanceSensors(Config& config) : adc{config.adc}, led_pwm{config.led_pwm}, max_distance{config.max_distance} { this->adc.start_dma(this->buffer.data(), num_of_sensors); - this->led_pwm.set_duty_cycle(100.0f); + this->led_pwm.set_duty_cycle(100.0F); } template diff --git a/src/proxy/fan.cpp b/src/proxy/fan.cpp index 17aa3ae..ef81e07 100644 --- a/src/proxy/fan.cpp +++ b/src/proxy/fan.cpp @@ -23,10 +23,10 @@ void Fan::disable() { } void Fan::set_speed(float speed) { - if (speed > 0.0f) { + if (speed > 0.0F) { this->set_direction(RotationDirection::FORWARD); this->pwm.set_duty_cycle(speed); - } else if (speed < 0.0f) { + } else if (speed < 0.0F) { this->set_direction(RotationDirection::BACKWARD); this->pwm.set_duty_cycle(-speed); } else { @@ -35,7 +35,7 @@ void Fan::set_speed(float speed) { } void Fan::stop() { - this->set_speed(0.0f); + this->set_speed(0.0F); } void Fan::set_direction(RotationDirection direction) { diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index fe154a5..91dfb96 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -13,9 +13,9 @@ namespace proxy { Imu::Imu(Config& config) : spi{config.spi}, - gy_factor{mdps_to_radps * 4.375f * + gy_factor{mdps_to_radps * 4.375F * (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale)))}, - xl_factor{mg_to_mps2 * (0.61f * (1 << static_cast(config.accelerometer_scale)))} { + xl_factor{mg_to_mps2 * (0.61F * (1 << static_cast(config.accelerometer_scale)))} { this->dev_ctx.read_reg = platform_read; this->dev_ctx.write_reg = platform_write; this->dev_ctx.handle = &config.spi; @@ -109,7 +109,7 @@ float Imu::get_orientation(Axis axis) const { return this->orientation[2]; default: - return 0.0f; + return 0.0F; } } @@ -125,7 +125,7 @@ float Imu::get_angular_velocity(Axis axis) const { return this->angular_velocity[2]; default: - return 0.0f; + return 0.0F; } } @@ -141,7 +141,7 @@ float Imu::get_linear_acceleration(Axis axis) const { return this->linear_acceleration[2]; default: - return 0.0f; + return 0.0F; } } @@ -185,15 +185,15 @@ void Imu::convert_orientation(std::array& quat, const uint16_t sflp[3] sumsq += quat[i] * quat[i]; } - if (sumsq > 1.0f) { + if (sumsq > 1.0F) { float n = std::sqrt(sumsq); quat[0] /= n; quat[1] /= n; quat[2] /= n; - sumsq = 1.0f; + sumsq = 1.0F; } - quat[3] = std::sqrt(1.0f - sumsq); + quat[3] = std::sqrt(1.0F - sumsq); } float Imu::half_to_float(const uint16_t x) { diff --git a/src/proxy/locomotion.cpp b/src/proxy/locomotion.cpp index 89865a8..20951be 100644 --- a/src/proxy/locomotion.cpp +++ b/src/proxy/locomotion.cpp @@ -28,21 +28,21 @@ void Locomotion::disable() { } void Locomotion::set_wheel_speed(float left_speed, float right_speed) { - if (left_speed > 0.0f) { + if (left_speed > 0.0F) { this->pwm_left_fwd.set_duty_cycle(left_speed); - this->pwm_left_bwd.set_duty_cycle(0.0f); - } else if (left_speed < 0.0f) { - this->pwm_left_fwd.set_duty_cycle(0.0f); + this->pwm_left_bwd.set_duty_cycle(0.0F); + } else if (left_speed < 0.0F) { + this->pwm_left_fwd.set_duty_cycle(0.0F); this->pwm_left_bwd.set_duty_cycle(-left_speed); } else { this->stop_left(); } - if (right_speed > 0.0f) { + if (right_speed > 0.0F) { this->pwm_right_fwd.set_duty_cycle(right_speed); - this->pwm_right_bwd.set_duty_cycle(0.0f); - } else if (right_speed < 0.0f) { - this->pwm_right_fwd.set_duty_cycle(0.0f); + this->pwm_right_bwd.set_duty_cycle(0.0F); + } else if (right_speed < 0.0F) { + this->pwm_right_fwd.set_duty_cycle(0.0F); this->pwm_right_bwd.set_duty_cycle(-right_speed); } else { this->stop_right(); @@ -53,14 +53,14 @@ void Locomotion::set_speed(float linear, float angular) { float left_speed = linear - angular; float right_speed = linear + angular; - if (std::abs(left_speed) > 100.0f) { - left_speed *= 100.0f / std::abs(left_speed); - right_speed *= 100.0f / std::abs(left_speed); + if (std::abs(left_speed) > 100.0F) { + left_speed *= 100.0F / std::abs(left_speed); + right_speed *= 100.0F / std::abs(left_speed); } - if (std::abs(right_speed) > 100.0f) { - left_speed *= 100.0f / std::abs(right_speed); - right_speed *= 100.0f / std::abs(right_speed); + if (std::abs(right_speed) > 100.0F) { + left_speed *= 100.0F / std::abs(right_speed); + right_speed *= 100.0F / std::abs(right_speed); } this->set_wheel_speed(left_speed, right_speed); @@ -72,12 +72,12 @@ void Locomotion::stop() { } void Locomotion::stop_left() { - this->pwm_left_fwd.set_duty_cycle(0.0f); - this->pwm_left_bwd.set_duty_cycle(0.0f); + this->pwm_left_fwd.set_duty_cycle(0.0F); + this->pwm_left_bwd.set_duty_cycle(0.0F); } void Locomotion::stop_right() { - this->pwm_right_fwd.set_duty_cycle(0.0f); - this->pwm_right_bwd.set_duty_cycle(0.0f); + this->pwm_right_fwd.set_duty_cycle(0.0F); + this->pwm_right_bwd.set_duty_cycle(0.0F); } } // namespace proxy From 99a5e6c55d188d615370092cb3eab10900a04873 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 16:56:18 -0300 Subject: [PATCH 107/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20unnecessary=20v?= =?UTF-8?q?oid=20inside=20function=20parameters?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/mcu.hpp | 2 +- src/hal/gpio.cpp | 4 ++-- src/hal/mcu.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index 36d5354..23fd0ab 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -20,7 +20,7 @@ class Mcu { /** * @brief Initializes MCU and some peripherals */ - static void init(void); + static void init(); }; } // namespace hal #endif // __MCU_HPP__ diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index 92c12cb..a774b25 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -12,7 +12,7 @@ namespace hal { Gpio::Gpio(Config& config) : port{config.port}, pin{config.pin} { } -bool Gpio::read(void) const { +bool Gpio::read() const { return HAL_GPIO_ReadPin(this->port, this->pin); } @@ -20,7 +20,7 @@ void Gpio::write(bool state) { HAL_GPIO_WritePin(this->port, this->pin, static_cast(state)); } -void Gpio::toggle(void) { +void Gpio::toggle() { HAL_GPIO_TogglePin(this->port, this->pin); } } diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index e02efe7..7c2c97b 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -16,11 +16,11 @@ extern "C" { * @brief Initializes System Clock * @note Defined by cube */ -void SystemClock_Config(void); +void SystemClock_Config(); } namespace hal { -void Mcu::init(void) { +void Mcu::init() { HAL_Init(); SystemClock_Config(); From a0a914e58cdb53d0d1f7510b6cce93f4bebacf59 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 17:12:39 -0300 Subject: [PATCH 108/561] =?UTF-8?q?=F0=9F=9A=B8=20Fix=20header=20guards?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 6 +++--- inc/hal/crc.hpp | 6 +++--- inc/hal/encoder.hpp | 6 +++--- inc/hal/flash.hpp | 6 +++--- inc/hal/gpio.hpp | 6 +++--- inc/hal/mcu.hpp | 6 +++--- inc/hal/pwm.hpp | 6 +++--- inc/hal/pwm_dma.hpp | 6 +++--- inc/hal/spi.hpp | 6 +++--- inc/hal/timer.hpp | 6 +++--- inc/proxy/argb.hpp | 6 +++--- inc/proxy/battery.hpp | 6 +++--- inc/proxy/button.hpp | 6 +++--- inc/proxy/buzzer.hpp | 6 +++--- inc/proxy/current_sensors.hpp | 6 +++--- inc/proxy/dip_switch.hpp | 6 +++--- inc/proxy/distance_sensors.hpp | 6 +++--- inc/proxy/fan.hpp | 6 +++--- inc/proxy/imu.hpp | 6 +++--- inc/proxy/led.hpp | 6 +++--- inc/proxy/locomotion.hpp | 6 +++--- inc/proxy/rotary_sensor.hpp | 6 +++--- inc/proxy/rotary_sensor_reg.hpp | 6 +++--- inc/proxy/serializable_interface.hpp | 6 +++--- inc/proxy/storage.hpp | 6 +++--- inc/proxy/torque_sensors.hpp | 6 +++--- src/proxy/argb.cpp | 6 +++--- src/proxy/current_sensors.cpp | 6 +++--- src/proxy/dip_switch.cpp | 6 +++--- src/proxy/distance_sensors.cpp | 6 +++--- src/proxy/torque_sensors.cpp | 6 +++--- 31 files changed, 93 insertions(+), 93 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index fc28edb..5ec1565 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __HAL_ADC_DMA_HPP__ -#define __HAL_ADC_DMA_HPP__ +#ifndef MICRAS_HAL_ADC_DMA_HPP +#define MICRAS_HAL_ADC_DMA_HPP #include #include @@ -66,4 +66,4 @@ class AdcDma { }; } -#endif // __HAL_ADC_DMA_HPP__ +#endif // MICRAS_HAL_ADC_DMA_HPP diff --git a/inc/hal/crc.hpp b/inc/hal/crc.hpp index 3d323e6..e514625 100644 --- a/inc/hal/crc.hpp +++ b/inc/hal/crc.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __CRC_HPP__ -#define __CRC_HPP__ +#ifndef MICRAS_HAL_CRC_HPP +#define MICRAS_HAL_CRC_HPP #include @@ -49,4 +49,4 @@ class Crc { }; } // namespace hal -#endif // __CRC_HPP__ +#endif // MICRAS_HAL_CRC_HPP diff --git a/inc/hal/encoder.hpp b/inc/hal/encoder.hpp index a69620b..db7e9b4 100644 --- a/inc/hal/encoder.hpp +++ b/inc/hal/encoder.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __ENCODER_HPP__ -#define __ENCODER_HPP__ +#ifndef MICRAS_HAL_ENCODER_HPP +#define MICRAS_HAL_ENCODER_HPP #include @@ -48,4 +48,4 @@ class Encoder { }; } // namespace hal -#endif // __ENCODER_HPP__ +#endif // MICRAS_HAL_ENCODER_HPP diff --git a/inc/hal/flash.hpp b/inc/hal/flash.hpp index 2733107..f613a03 100644 --- a/inc/hal/flash.hpp +++ b/inc/hal/flash.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __FLASH_HPP__ -#define __FLASH_HPP__ +#ifndef MICRAS_HAL_FLASH_HPP +#define MICRAS_HAL_FLASH_HPP #include #include @@ -87,4 +87,4 @@ class Flash { }; } // namespace hal -#endif // __FLASH_HPP__ +#endif // MICRAS_HAL_FLASH_HPP diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index 475f3be..829f5d7 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __GPIO_HPP__ -#define __GPIO_HPP__ +#ifndef MICRAS_HAL_GPIO_HPP +#define MICRAS_HAL_GPIO_HPP #include #include @@ -65,4 +65,4 @@ class Gpio { }; } // namespace hal -#endif // __GPIO_HPP__ +#endif // MICRAS_HAL_GPIO_HPP diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index 23fd0ab..c04cd6a 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __MCU_HPP__ -#define __MCU_HPP__ +#ifndef MICRAS_HAL_MCU_HPP +#define MICRAS_HAL_MCU_HPP #include @@ -23,4 +23,4 @@ class Mcu { static void init(); }; } // namespace hal -#endif // __MCU_HPP__ +#endif // MICRAS_HAL_MCU_HPP diff --git a/inc/hal/pwm.hpp b/inc/hal/pwm.hpp index 037a6de..12228c9 100644 --- a/inc/hal/pwm.hpp +++ b/inc/hal/pwm.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __PWM_HPP__ -#define __PWM_HPP__ +#ifndef MICRAS_HAL_PWM_HPP +#define MICRAS_HAL_PWM_HPP #include #include @@ -61,4 +61,4 @@ class Pwm { }; } // namespace hal -#endif // __PWM_HPP__ +#endif // MICRAS_HAL_PWM_HPP diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp index 3cb7d8b..6414ca7 100644 --- a/inc/hal/pwm_dma.hpp +++ b/inc/hal/pwm_dma.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __PWM_DMA_HPP__ -#define __PWM_DMA_HPP__ +#ifndef MICRAS_HAL_PWM_DMA_HPP +#define MICRAS_HAL_PWM_DMA_HPP #include #include @@ -67,4 +67,4 @@ class PwmDma { uint32_t channel; }; } // namespace hal -#endif // __PWM_DMA_HPP__ +#endif // MICRAS_HAL_PWM_DMA_HPP diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index 6dad345..ebbc063 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __SPI_HPP__ -#define __SPI_HPP__ +#ifndef MICRAS_HAL_SPI_HPP +#define MICRAS_HAL_SPI_HPP #include #include @@ -83,4 +83,4 @@ class Spi { }; } // namespace hal -#endif // __SPI_HPP__ +#endif // MICRAS_HAL_SPI_HPP diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index 29c3ac0..3b2987c 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __TIMER_HPP__ -#define __TIMER_HPP__ +#ifndef MICRAS_HAL_TIMER_HPP +#define MICRAS_HAL_TIMER_HPP #include #include @@ -108,4 +108,4 @@ class Timer { }; } // namespace hal -#endif // __TIMER_HPP__ +#endif // MICRAS_HAL_TIMER_HPP diff --git a/inc/proxy/argb.hpp b/inc/proxy/argb.hpp index a287d6b..b43624c 100644 --- a/inc/proxy/argb.hpp +++ b/inc/proxy/argb.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __ARGB_HPP__ -#define __ARGB_HPP__ +#ifndef MICRAS_PROXY_ARGB_HPP +#define MICRAS_PROXY_ARGB_HPP #include #include @@ -114,4 +114,4 @@ class Argb { #include "../src/proxy/argb.cpp" -#endif // __ARGB_HPP__ +#endif // MICRAS_PROXY_ARGB_HPP diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp index 9cdc25e..be124cf 100644 --- a/inc/proxy/battery.hpp +++ b/inc/proxy/battery.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __BATTERY_HPP__ -#define __BATTERY_HPP__ +#ifndef MICRAS_PROXY_BATTERY_HPP +#define MICRAS_PROXY_BATTERY_HPP #include @@ -66,4 +66,4 @@ class Battery { }; } // namespace proxy -#endif // __BATTERY_HPP__ +#endif // MICRAS_PROXY_BATTERY_HPP diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index 7c1f17d..8834e7b 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __BUTTON_HPP__ -#define __BUTTON_HPP__ +#ifndef MICRAS_PROXY_BUTTON_HPP +#define MICRAS_PROXY_BUTTON_HPP #include @@ -141,4 +141,4 @@ class Button { }; } // namespace proxy -#endif // __BUTTON_HPP__ +#endif // MICRAS_PROXY_BUTTON_HPP diff --git a/inc/proxy/buzzer.hpp b/inc/proxy/buzzer.hpp index bea453b..d6bb29d 100644 --- a/inc/proxy/buzzer.hpp +++ b/inc/proxy/buzzer.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __BUZZER_HPP__ -#define __BUZZER_HPP__ +#ifndef MICRAS_PROXY_BUZZER_HPP +#define MICRAS_PROXY_BUZZER_HPP #include @@ -75,4 +75,4 @@ class Buzzer { }; } // namespace proxy -#endif // __BUZZER_HPP__ +#endif // MICRAS_PROXY_BUZZER_HPP diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index 54a50d1..07a8dd4 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __CURRENT_SENSORS_HPP__ -#define __CURRENT_SENSORS_HPP__ +#ifndef MICRAS_PROXY_CURRENT_SENSORS_HPP +#define MICRAS_PROXY_CURRENT_SENSORS_HPP #include #include @@ -72,4 +72,4 @@ class CurrentSensors { #include "../src/proxy/current_sensors.cpp" -#endif // __CURRENT_SENSORS_HPP__ +#endif // MICRAS_PROXY_CURRENT_SENSORS_HPP diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp index a11d93f..714824f 100644 --- a/inc/proxy/dip_switch.hpp +++ b/inc/proxy/dip_switch.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __DIP_SWITCH_HPP__ -#define __DIP_SWITCH_HPP__ +#ifndef MICRAS_PROXY_DIP_SWITCH_HPP +#define MICRAS_PROXY_DIP_SWITCH_HPP #include #include @@ -61,4 +61,4 @@ class DipSwitch { #include "../src/proxy/dip_switch.cpp" -#endif // __DIP_SWITCH_HPP__ +#endif // MICRAS_PROXY_DIP_SWITCH_HPP diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index f474ec6..df97114 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __DISTANCE_SENSORS_HPP__ -#define __DISTANCE_SENSORS_HPP__ +#ifndef MICRAS_PROXY_DISTANCE_SENSORS_HPP +#define MICRAS_PROXY_DISTANCE_SENSORS_HPP #include #include @@ -86,4 +86,4 @@ class DistanceSensors { #include "../src/proxy/distance_sensors.cpp" -#endif // __DISTANCE_SENSORS_HPP__ +#endif // MICRAS_PROXY_DISTANCE_SENSORS_HPP diff --git a/inc/proxy/fan.hpp b/inc/proxy/fan.hpp index 82bc747..056d556 100644 --- a/inc/proxy/fan.hpp +++ b/inc/proxy/fan.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __FAN_HPP__ -#define __FAN_HPP__ +#ifndef MICRAS_PROXY_FAN_HPP +#define MICRAS_PROXY_FAN_HPP #include @@ -91,4 +91,4 @@ class Fan { }; } // namespace proxy -#endif // __FAN_HPP__ +#endif // MICRAS_PROXY_FAN_HPP diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index f98d35a..3b399e7 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __IMU_HPP__ -#define __IMU_HPP__ +#ifndef MICRAS_PROXY_IMU_HPP +#define MICRAS_PROXY_IMU_HPP #include #include @@ -170,4 +170,4 @@ class Imu { }; } // namespace proxy -#endif // __IMU_HPP__ +#endif // MICRAS_PROXY_IMU_HPP diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp index 8861722..86eebf8 100644 --- a/inc/proxy/led.hpp +++ b/inc/proxy/led.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __LED_HPP__ -#define __LED_HPP__ +#ifndef MICRAS_PROXY_LED_HPP +#define MICRAS_PROXY_LED_HPP #include "hal/gpio.hpp" @@ -54,4 +54,4 @@ class Led { }; } // namespace proxy -#endif // __LED_HPP__ +#endif // MICRAS_PROXY_LED_HPP diff --git a/inc/proxy/locomotion.hpp b/inc/proxy/locomotion.hpp index da30fba..9792f6f 100644 --- a/inc/proxy/locomotion.hpp +++ b/inc/proxy/locomotion.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __DUAL_LOCOMOTION_HPP__ -#define __DUAL_LOCOMOTION_HPP__ +#ifndef MICRAS_PROXY_LOCOMOTION_HPP +#define MICRAS_PROXY_LOCOMOTION_HPP #include @@ -107,4 +107,4 @@ class Locomotion { }; } // namespace proxy -#endif // __DUAL_LOCOMOTION_HPP__ +#endif // MICRAS_PROXY_LOCOMOTION_HPP diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index 4ace2f4..526856c 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __ROTARY_SENSOR_HPP__ -#define __ROTARY_SENSOR_HPP__ +#ifndef MICRAS_PROXY_ROTARY_SENSOR_HPP +#define MICRAS_PROXY_ROTARY_SENSOR_HPP #include @@ -103,4 +103,4 @@ class RotarySensor { }; } // namespace proxy -#endif // __ROTARY_SENSOR_HPP__ +#endif // MICRAS_PROXY_ROTARY_SENSOR_HPP diff --git a/inc/proxy/rotary_sensor_reg.hpp b/inc/proxy/rotary_sensor_reg.hpp index cd01291..ada08dc 100644 --- a/inc/proxy/rotary_sensor_reg.hpp +++ b/inc/proxy/rotary_sensor_reg.hpp @@ -7,8 +7,8 @@ * @date 03/2024 */ -#ifndef __ROTARY_SENSOR_REG_HPP__ -#define __ROTARY_SENSOR_REG_HPP__ +#ifndef MICRAS_PROXY_ROTARY_SENSOR_REG_HPP +#define MICRAS_PROXY_ROTARY_SENSOR_REG_HPP #include @@ -111,4 +111,4 @@ struct Registers { Ecc ecc; }; -#endif // __ROTARY_SENSOR_REG_HPP__ +#endif // MICRAS_PROXY_ROTARY_SENSOR_REG_HPP diff --git a/inc/proxy/serializable_interface.hpp b/inc/proxy/serializable_interface.hpp index 2304477..dda86c9 100644 --- a/inc/proxy/serializable_interface.hpp +++ b/inc/proxy/serializable_interface.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __SERIALIZABLE_INTERFACE_HPP__ -#define __SERIALIZABLE_INTERFACE_HPP__ +#ifndef MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP +#define MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP #include #include @@ -21,4 +21,4 @@ class ISerializable { virtual void deserialize(uint8_t* serial_data, uint16_t size) = 0; }; -#endif // __SERIALIZABLE_INTERFACE_HPP__ +#endif // MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp index 9acb48b..1663420 100644 --- a/inc/proxy/storage.hpp +++ b/inc/proxy/storage.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __STORAGE_HPP__ -#define __STORAGE_HPP__ +#ifndef MICRAS_PROXY_STORAGE_HPP +#define MICRAS_PROXY_STORAGE_HPP #include #include @@ -151,4 +151,4 @@ class Storage { }; } // namespace proxy -#endif // __STORAGE_HPP__ +#endif // MICRAS_PROXY_STORAGE_HPP diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index 1473d5a..84e5a09 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __TORQUE_SENSORS_HPP__ -#define __TORQUE_SENSORS_HPP__ +#ifndef MICRAS_PROXY_TORQUE_SENSORS_HPP +#define MICRAS_PROXY_TORQUE_SENSORS_HPP #include @@ -66,4 +66,4 @@ class TorqueSensors { #include "../src/proxy/torque_sensors.cpp" -#endif // __TORQUE_SENSORS_HPP__ +#endif // MICRAS_PROXY_TORQUE_SENSORS_HPP diff --git a/src/proxy/argb.cpp b/src/proxy/argb.cpp index cf4ab1a..c5db008 100644 --- a/src/proxy/argb.cpp +++ b/src/proxy/argb.cpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __ARGB_CPP__ -#define __ARGB_CPP__ +#ifndef MICRAS_PROXY_ARGB_CPP +#define MICRAS_PROXY_ARGB_CPP #include "proxy/argb.hpp" @@ -57,4 +57,4 @@ void Argb::encode_color(const Color& color, uint8_t index) { } } // namespace proxy -#endif // __ARGB_CPP__ +#endif // MICRAS_PROXY_ARGB_CPP diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp index 8fd2e5d..08a7ad5 100644 --- a/src/proxy/current_sensors.cpp +++ b/src/proxy/current_sensors.cpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __CURRENT_SENSORS_CPP__ -#define __CURRENT_SENSORS_CPP__ +#ifndef MICRAS_PROXY_CURRENT_SENSORS_CPP +#define MICRAS_PROXY_CURRENT_SENSORS_CPP #include "proxy/current_sensors.hpp" @@ -30,4 +30,4 @@ uint32_t CurrentSensors::get_current_raw(uint8_t sensor_index) c } } // namespace proxy -#endif // __CURRENT_SENSORS_CPP__ +#endif // MICRAS_PROXY_CURRENT_SENSORS_CPP diff --git a/src/proxy/dip_switch.cpp b/src/proxy/dip_switch.cpp index 97d9da5..af90074 100644 --- a/src/proxy/dip_switch.cpp +++ b/src/proxy/dip_switch.cpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __DIP_SWITCH_CPP__ -#define __DIP_SWITCH_CPP__ +#ifndef MICRAS_PROXY_DIP_SWITCH_CPP +#define MICRAS_PROXY_DIP_SWITCH_CPP #include "hal/mcu.hpp" #include "proxy/dip_switch.hpp" @@ -37,4 +37,4 @@ uint8_t DipSwitch::get_switches_value() const { } } // namespace proxy -#endif // __DIP_SWITCH_CPP__ +#endif // MICRAS_PROXY_DIP_SWITCH_CPP diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index 2d38d73..c009628 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __DISTANCE_SENSORS_CPP__ -#define __DISTANCE_SENSORS_CPP__ +#ifndef MICRAS_PROXY_DISTANCE_SENSORS_CPP +#define MICRAS_PROXY_DISTANCE_SENSORS_CPP #include "proxy/distance_sensors.hpp" @@ -35,4 +35,4 @@ uint32_t DistanceSensors::get_distance_raw(uint8_t sensor_index) } } // namespace proxy -#endif // __DISTANCE_SENSORS_HPP__ +#endif // MICRAS_PROXY_DISTANCE_SENSORS_CPP diff --git a/src/proxy/torque_sensors.cpp b/src/proxy/torque_sensors.cpp index 8e36fab..447bc59 100644 --- a/src/proxy/torque_sensors.cpp +++ b/src/proxy/torque_sensors.cpp @@ -6,8 +6,8 @@ * @date 03/2024 */ -#ifndef __TORQUE_SENSORS_CPP__ -#define __TORQUE_SENSORS_CPP__ +#ifndef MICRAS_PROXY_TORQUE_SENSORS_CPP +#define MICRAS_PROXY_TORQUE_SENSORS_CPP #include "proxy/torque_sensors.hpp" @@ -28,4 +28,4 @@ float TorqueSensors::get_current(uint8_t sensor_index) const { } } // namespace proxy -#endif // __TORQUE_SENSORS_CPP__ +#endif // MICRAS_PROXY_TORQUE_SENSORS_CPP From 78cd356485c4094b2648d4a9592bd514a10e6386 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 17:40:25 -0300 Subject: [PATCH 109/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Make=20timer=20get?= =?UTF-8?q?=5Fcounter=5Fms=20static?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/timer.hpp | 2 +- src/hal/timer.cpp | 8 +++----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index 3b2987c..3190fcc 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -82,7 +82,7 @@ class Timer { * * @return uint32_t Current timer counter in milliseconds */ - uint32_t get_counter_ms() const; + static uint32_t get_counter_ms(); /** * @brief Get the current timer counter diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index 6e17eb9..fa2e9dd 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -21,7 +21,7 @@ Timer::Timer(Config& config) : handle{config.handle} { } void Timer::reset_ms() { - this->counter = this->get_counter_ms(); + this->counter = get_counter_ms(); } void Timer::reset_us() { @@ -29,7 +29,7 @@ void Timer::reset_us() { } uint32_t Timer::elapsed_time_ms() const { - return this->get_counter_ms() - this->counter; + return get_counter_ms() - this->counter; } uint32_t Timer::elapsed_time_us() const { @@ -40,7 +40,6 @@ void Timer::sleep_ms(uint32_t time) { uint32_t start = HAL_GetTick(); while (HAL_GetTick() - start < time) { - continue; } } @@ -48,11 +47,10 @@ void Timer::sleep_us(uint32_t time) const { uint32_t start = this->get_counter_us(); while (this->get_counter_us() - start < time) { - continue; } } -uint32_t Timer::get_counter_ms() const { +uint32_t Timer::get_counter_ms() { return HAL_GetTick(); } From 8bcf567a568fd6c8b7b0aa54acc37706ab606d70 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 17:41:40 -0300 Subject: [PATCH 110/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Add=20initializati?= =?UTF-8?q?on=20to=20variables?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/timer.hpp | 2 +- inc/proxy/battery.hpp | 2 +- inc/proxy/imu.hpp | 8 ++++---- src/hal/flash.cpp | 2 +- src/proxy/imu.cpp | 2 +- src/proxy/rotary_sensor.cpp | 4 ++-- src/proxy/storage.cpp | 2 +- 7 files changed, 11 insertions(+), 11 deletions(-) diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index 3190fcc..ead6bce 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -99,7 +99,7 @@ class Timer { /** * @brief Timer counter */ - uint32_t counter; + uint32_t counter{ }; /** * @brief Flag to enable microseconds diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp index be124cf..23f5eb4 100644 --- a/inc/proxy/battery.hpp +++ b/inc/proxy/battery.hpp @@ -57,7 +57,7 @@ class Battery { /** * @brief Raw reading from the battery */ - uint32_t raw_reading; + uint32_t raw_reading{ }; /** * @brief Voltage divider ratio diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 3b399e7..1a16fee 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -141,22 +141,22 @@ class Imu { /** * @brief Device context for the IMU library */ - stmdev_ctx_t dev_ctx; + stmdev_ctx_t dev_ctx{ }; /** * @brief Current angular velocity on each axis */ - std::array angular_velocity; + std::array angular_velocity{ }; /** * @brief Current linear acceleration on each axis */ - std::array linear_acceleration; + std::array linear_acceleration{ }; /** * @brief Current orientation */ - std::array orientation; + std::array orientation{ }; /** * @brief Gyroscope conversion factor diff --git a/src/hal/flash.cpp b/src/hal/flash.cpp index 34862fe..497d982 100644 --- a/src/hal/flash.cpp +++ b/src/hal/flash.cpp @@ -62,7 +62,7 @@ void Flash::erase_pages(uint16_t page, uint16_t number_of_pages) { .NbPages = number_of_pages, }; - uint32_t page_error; + uint32_t page_error{ }; HAL_FLASHEx_Erase(&erase_struct, &page_error); } diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 91dfb96..09071f3 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -24,7 +24,7 @@ Imu::Imu(Config& config) : spi{config.spi}, lsm6dsv_reset_set(&(this->dev_ctx), LSM6DSV_RESTORE_CTRL_REGS); - lsm6dsv_reset_t rst; + lsm6dsv_reset_t rst{ }; do { lsm6dsv_reset_get(&(this->dev_ctx), &rst); diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index ae7f848..9ecbd1d 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -13,8 +13,8 @@ namespace proxy { RotarySensor::RotarySensor(Config& config) : spi{config.spi}, encoder{config.encoder}, crc{config.crc}, resolution{config.resolution} { - CommandFrame command_frame; - DataFrame data_frame; + CommandFrame command_frame{ }; + DataFrame data_frame{ }; command_frame.fields.address = Registers::disable_addr; data_frame.fields.data = config.registers.disable.raw; diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index 10f9d80..563375a 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -11,7 +11,7 @@ namespace proxy { Storage::Storage(Config& config) : start_page{config.start_page}, number_of_pages{config.number_of_pages} { - uint64_t sizes; + uint64_t sizes{ }; hal::Flash::read(this->start_page, 0, &sizes); uint32_t total_size = sizes >> 32; From 0ddfb5a4d455a2913a3745feb14dab6ef1037a69 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 17:42:07 -0300 Subject: [PATCH 111/561] =?UTF-8?q?=F0=9F=92=A1=20Add=20comments=20to=20na?= =?UTF-8?q?mespace=20ending?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/hal/gpio.cpp | 2 +- src/hal/mcu.cpp | 2 +- src/hal/spi.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index a774b25..f6335db 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -23,4 +23,4 @@ void Gpio::write(bool state) { void Gpio::toggle() { HAL_GPIO_TogglePin(this->port, this->pin); } -} +} // namespace hal diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index 7c2c97b..b5568c0 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -29,4 +29,4 @@ void Mcu::init() { MX_DMA_Init(); MX_CRC_Init(); } -} +} // namespace hal diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index c40ba76..f8621da 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -33,4 +33,4 @@ void Spi::transmit(uint8_t data[], uint32_t size) { void Spi::receive(uint8_t data[], uint32_t size) { HAL_SPI_Receive(this->handle, data, size, this->timeout); } -} // namespace proxy +} // namespace hal From d9b799a85ac7a5490171622aeba74a81080448d6 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 17:42:31 -0300 Subject: [PATCH 112/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20else=20after=20?= =?UTF-8?q?return=20in=20button=20get=5Fstatus?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/button.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index 82502f0..936dcd0 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -30,11 +30,13 @@ Button::Status Button::get_status() { } else if (this->is_falling_edge()) { if (this->status_timer.elapsed_time_ms() > extra_long_press_delay) { return EXTRA_LONG_PRESS; - } else if (this->status_timer.elapsed_time_ms() > long_press_delay) { + } + + if (this->status_timer.elapsed_time_ms() > long_press_delay) { return LONG_PRESS; - } else { - return SHORT_PRESS; } + + return SHORT_PRESS; } return NO_PRESS; From 20833f32124abdcf5690f9938bf85e5066ed2629 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 17:42:55 -0300 Subject: [PATCH 113/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20infinite=20recursi?= =?UTF-8?q?on=20on=20fan=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/fan.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/proxy/fan.cpp b/src/proxy/fan.cpp index ef81e07..aeb2818 100644 --- a/src/proxy/fan.cpp +++ b/src/proxy/fan.cpp @@ -35,7 +35,7 @@ void Fan::set_speed(float speed) { } void Fan::stop() { - this->set_speed(0.0F); + this->pwm.set_duty_cycle(0.0F); } void Fan::set_direction(RotationDirection direction) { From 114d47ca3a4afd08a9e74c0c639a734e279d92d7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 17:44:20 -0300 Subject: [PATCH 114/561] =?UTF-8?q?=F0=9F=8E=A8=20Use=20auto=20on=20variab?= =?UTF-8?q?les=20initialized=20by=20cast?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/imu.cpp | 6 +++--- src/proxy/storage.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 09071f3..94eb7f1 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -69,7 +69,7 @@ void Imu::update_data() { while (samples--) { lsm6dsv_fifo_out_raw_t f_data; lsm6dsv_fifo_out_raw_get(&(this->dev_ctx), &f_data); - int16_t* axis = reinterpret_cast(f_data.data); + auto* axis = reinterpret_cast(f_data.data); switch (f_data.tag) { case lsm6dsv_fifo_out_raw_t::LSM6DSV_GY_NC_TAG: @@ -146,7 +146,7 @@ float Imu::get_linear_acceleration(Axis axis) const { } int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) { - auto spi = static_cast(handle); + auto* spi = static_cast(handle); while (not spi->select_device()) { continue; @@ -161,7 +161,7 @@ int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t le } int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len) { - auto spi = static_cast(handle); + auto* spi = static_cast(handle); while (not spi->select_device()) { continue; diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index 563375a..2b2be34 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -63,7 +63,7 @@ void Storage::save() { continue; } - const uint8_t* aux = reinterpret_cast(variable.ram_pointer); + const auto* aux = reinterpret_cast(variable.ram_pointer); variable.buffer_address = buffer.size(); this->buffer.insert(this->buffer.end(), aux, aux + variable.size); } From 16267a913d41e6a3e51dea382b499d11b80ddc76 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 17:44:44 -0300 Subject: [PATCH 115/561] =?UTF-8?q?=F0=9F=8E=A8=20Remove=20continue=20from?= =?UTF-8?q?=20empty=20while=20loops?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/imu.cpp | 2 -- src/proxy/rotary_sensor.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 94eb7f1..9ec723c 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -149,7 +149,6 @@ int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t le auto* spi = static_cast(handle); while (not spi->select_device()) { - continue; } reg |= 0x80; @@ -164,7 +163,6 @@ int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint auto* spi = static_cast(handle); while (not spi->select_device()) { - continue; } spi->transmit(®, 1); diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index 9ecbd1d..4639ad3 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -51,7 +51,6 @@ float RotarySensor::get_position() const { void RotarySensor::write_register(CommandFrame& command_frame, DataFrame& data_frame) { while (not this->spi.select_device()) { - continue; } command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2); From 49b107ad33ff6b4e085a1db2ca165cbf9ab6cbf2 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 18:05:04 -0300 Subject: [PATCH 116/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Make=20configurati?= =?UTF-8?q?on=20on=20target=20constant?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 2 +- inc/hal/crc.hpp | 2 +- inc/hal/encoder.hpp | 2 +- inc/hal/gpio.hpp | 2 +- inc/hal/mcu.hpp | 5 +++++ inc/hal/pwm.hpp | 2 +- inc/hal/pwm_dma.hpp | 2 +- inc/hal/spi.hpp | 2 +- inc/hal/timer.hpp | 2 +- inc/proxy/argb.hpp | 2 +- inc/proxy/battery.hpp | 2 +- inc/proxy/button.hpp | 2 +- inc/proxy/buzzer.hpp | 2 +- inc/proxy/current_sensors.hpp | 2 +- inc/proxy/dip_switch.hpp | 2 +- inc/proxy/distance_sensors.hpp | 2 +- inc/proxy/fan.hpp | 2 +- inc/proxy/imu.hpp | 2 +- inc/proxy/led.hpp | 2 +- inc/proxy/locomotion.hpp | 2 +- inc/proxy/rotary_sensor.hpp | 2 +- inc/proxy/storage.hpp | 2 +- inc/proxy/torque_sensors.hpp | 2 +- src/hal/adc_dma.cpp | 2 +- src/hal/crc.cpp | 2 +- src/hal/encoder.cpp | 2 +- src/hal/gpio.cpp | 2 +- src/hal/pwm.cpp | 2 +- src/hal/pwm_dma.cpp | 2 +- src/hal/spi.cpp | 2 +- src/hal/timer.cpp | 2 +- src/proxy/argb.cpp | 2 +- src/proxy/battery.cpp | 2 +- src/proxy/button.cpp | 2 +- src/proxy/buzzer.cpp | 2 +- src/proxy/current_sensors.cpp | 2 +- src/proxy/dip_switch.cpp | 2 +- src/proxy/distance_sensors.cpp | 2 +- src/proxy/fan.cpp | 3 ++- src/proxy/imu.cpp | 4 ++-- src/proxy/led.cpp | 2 +- src/proxy/locomotion.cpp | 2 +- src/proxy/rotary_sensor.cpp | 2 +- src/proxy/storage.cpp | 2 +- src/proxy/torque_sensors.cpp | 2 +- 45 files changed, 51 insertions(+), 45 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index 5ec1565..e3bc73d 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -33,7 +33,7 @@ class AdcDma { * * @param config ADC DMA configuration struct */ - AdcDma(Config& config); + AdcDma(const Config& config); /** * @brief Enable ADC, start conversion of regular group and transfer result through DMA diff --git a/inc/hal/crc.hpp b/inc/hal/crc.hpp index e514625..66fd36c 100644 --- a/inc/hal/crc.hpp +++ b/inc/hal/crc.hpp @@ -29,7 +29,7 @@ class Crc { * * @param config Configuration for the CRC */ - Crc(Config& config); + Crc(const Config& config); /** * @brief Calculate the CRC value diff --git a/inc/hal/encoder.hpp b/inc/hal/encoder.hpp index db7e9b4..bd9548c 100644 --- a/inc/hal/encoder.hpp +++ b/inc/hal/encoder.hpp @@ -31,7 +31,7 @@ class Encoder { * * @param config Configuration for the encoder */ - Encoder(Config& config); + Encoder(const Config& config); /** * @brief Get the counter value diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index 829f5d7..8dac60e 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -31,7 +31,7 @@ class Gpio { * * @param config Configuration for the GPIO pin */ - Gpio(Config& config); + Gpio(const Config& config); /** * @brief Read the current state of the GPIO pin diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index c04cd6a..95b0954 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -17,6 +17,11 @@ namespace hal { */ class Mcu { public: + /** + * @brief Deleted constructor for static class + */ + Mcu() = delete; + /** * @brief Initializes MCU and some peripherals */ diff --git a/inc/hal/pwm.hpp b/inc/hal/pwm.hpp index 12228c9..b8ffc55 100644 --- a/inc/hal/pwm.hpp +++ b/inc/hal/pwm.hpp @@ -32,7 +32,7 @@ class Pwm { * * @param config Configuration for the PWM */ - Pwm(Config& config); + Pwm(const Config& config); /** * @brief Set the PWM duty cycle diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp index 6414ca7..4c67402 100644 --- a/inc/hal/pwm_dma.hpp +++ b/inc/hal/pwm_dma.hpp @@ -32,7 +32,7 @@ class PwmDma { * * @param config Configuration for the PWM */ - PwmDma(Config& config); + PwmDma(const Config& config); /** * @brief Start PWM and DMA transfer diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index ebbc063..1fbaf14 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -35,7 +35,7 @@ class Spi { * * @param config Configuration for the SPI */ - Spi(Config& config); + Spi(const Config& config); /** * @brief Activate the chip select diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index ead6bce..245a289 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -36,7 +36,7 @@ class Timer { * * @param config Configuration for the timer */ - Timer(Config& config); + Timer(const Config& config); /** * @brief Reset the timer counter in milliseconds diff --git a/inc/proxy/argb.hpp b/inc/proxy/argb.hpp index b43624c..e9d8287 100644 --- a/inc/proxy/argb.hpp +++ b/inc/proxy/argb.hpp @@ -42,7 +42,7 @@ class Argb { * * @param config Configuration for the addressable RGB LED */ - Argb(Config& config); + Argb(const Config& config); /** * @brief Set the color of the ARGB at the specified index diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp index 23f5eb4..dbe0c8e 100644 --- a/inc/proxy/battery.hpp +++ b/inc/proxy/battery.hpp @@ -32,7 +32,7 @@ class Battery { * * @param config Configuration for the battery */ - Battery(Config& config); + Battery(const Config& config); /** * @brief Get the battery voltage diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index 8834e7b..cfa9202 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -54,7 +54,7 @@ class Button { * * @param config Button configuration */ - Button(Config& config); + Button(const Config& config); /** * @brief Check if button is pressed diff --git a/inc/proxy/buzzer.hpp b/inc/proxy/buzzer.hpp index d6bb29d..0fcff8d 100644 --- a/inc/proxy/buzzer.hpp +++ b/inc/proxy/buzzer.hpp @@ -32,7 +32,7 @@ class Buzzer { * * @param config Configuration for the buzzer */ - Buzzer(Config& config); + Buzzer(const Config& config); /** * @brief Play a tone for a duration diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index 07a8dd4..726050a 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -34,7 +34,7 @@ class CurrentSensors { * * @param config Configuration for the current sensors */ - CurrentSensors(Config& config); + CurrentSensors(const Config& config); /** * @brief Get the current from the sensor diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp index 714824f..392d22b 100644 --- a/inc/proxy/dip_switch.hpp +++ b/inc/proxy/dip_switch.hpp @@ -33,7 +33,7 @@ class DipSwitch { * * @param config Configuration struct for DipSwitch */ - DipSwitch(Config& config); + DipSwitch(const Config& config); /** * @brief Get the state of a switch diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index df97114..dd4d928 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -36,7 +36,7 @@ class DistanceSensors { * * @param config Configuration for the distance sensors */ - DistanceSensors(Config& config); + DistanceSensors(const Config& config); /** * @brief Set the distance sensors led intensity diff --git a/inc/proxy/fan.hpp b/inc/proxy/fan.hpp index 056d556..0453575 100644 --- a/inc/proxy/fan.hpp +++ b/inc/proxy/fan.hpp @@ -42,7 +42,7 @@ class Fan { * * @param config Configuration for the fan driver */ - Fan(Config& config); + Fan(const Config& config); /** * @brief Enable the fan diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 1a16fee..4730d91 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -41,7 +41,7 @@ class Imu { * * @param config Configuration for the IMU */ - Imu(Config& config); + Imu(const Config& config); enum Axis { W, diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp index 86eebf8..b53ed47 100644 --- a/inc/proxy/led.hpp +++ b/inc/proxy/led.hpp @@ -29,7 +29,7 @@ class Led { * * @param config Configuration for the LED */ - Led(Config& config); + Led(const Config& config); /** * @brief Turn the LED on diff --git a/inc/proxy/locomotion.hpp b/inc/proxy/locomotion.hpp index 9792f6f..b0b2bd3 100644 --- a/inc/proxy/locomotion.hpp +++ b/inc/proxy/locomotion.hpp @@ -36,7 +36,7 @@ class Locomotion { * * @param config Configuration for the locomotion driver */ - Locomotion(Config& config); + Locomotion(const Config& config); /** * @brief Enable the locomotion driver diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index 526856c..40f3854 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -39,7 +39,7 @@ class RotarySensor { * * @param config Configuration for the rotary sensor */ - RotarySensor(Config& config); + RotarySensor(const Config& config); /** * @brief Get the rotary sensor position over an axis diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp index 1663420..dbc1fde 100644 --- a/inc/proxy/storage.hpp +++ b/inc/proxy/storage.hpp @@ -39,7 +39,7 @@ class Storage { * * @param config Configuration for the storage */ - Storage(Config& config); + Storage(const Config& config); /** * @brief Create a new primitive variable in the storage diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index 84e5a09..a282378 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -33,7 +33,7 @@ class TorqueSensors { * * @param config Configuration for the torque sensors */ - TorqueSensors(Config& config); + TorqueSensors(const Config& config); /** * @brief Get the torque from the sensor diff --git a/src/hal/adc_dma.cpp b/src/hal/adc_dma.cpp index e2b5409..9afb885 100644 --- a/src/hal/adc_dma.cpp +++ b/src/hal/adc_dma.cpp @@ -9,7 +9,7 @@ #include "hal/adc_dma.hpp" namespace hal { -AdcDma::AdcDma(Config& config) : +AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { config.init_function(); } diff --git a/src/hal/crc.cpp b/src/hal/crc.cpp index 18c57cb..cc3c988 100644 --- a/src/hal/crc.cpp +++ b/src/hal/crc.cpp @@ -9,7 +9,7 @@ #include "hal/crc.hpp" namespace hal { -Crc::Crc(Config& config) : handle{config.handle} { +Crc::Crc(const Config& config) : handle{config.handle} { } uint32_t Crc::calculate(uint32_t data[], uint32_t size) { diff --git a/src/hal/encoder.cpp b/src/hal/encoder.cpp index 1fa5e73..a2d627c 100644 --- a/src/hal/encoder.cpp +++ b/src/hal/encoder.cpp @@ -9,7 +9,7 @@ #include "hal/encoder.hpp" namespace hal { -Encoder::Encoder(Config& config) : handle{config.handle} { +Encoder::Encoder(const Config& config) : handle{config.handle} { config.init_function(); HAL_TIM_Encoder_Start(this->handle, config.timer_channel); } diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index f6335db..46710d1 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -9,7 +9,7 @@ #include "hal/gpio.hpp" namespace hal { -Gpio::Gpio(Config& config) : port{config.port}, pin{config.pin} { +Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { } bool Gpio::read() const { diff --git a/src/hal/pwm.cpp b/src/hal/pwm.cpp index 7a975a0..9b1b344 100644 --- a/src/hal/pwm.cpp +++ b/src/hal/pwm.cpp @@ -11,7 +11,7 @@ #include "hal/pwm.hpp" namespace hal { -Pwm::Pwm(Config& config) : handle{config.handle}, channel{config.timer_channel} { +Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel} { config.init_function(); HAL_TIM_PWM_Start(this->handle, this->channel); __HAL_TIM_SET_COMPARE(this->handle, this->channel, 0); diff --git a/src/hal/pwm_dma.cpp b/src/hal/pwm_dma.cpp index 5eed04f..6321656 100644 --- a/src/hal/pwm_dma.cpp +++ b/src/hal/pwm_dma.cpp @@ -11,7 +11,7 @@ #include "hal/pwm_dma.hpp" namespace hal { -PwmDma::PwmDma(Config& config) : handle{config.handle}, channel{config.timer_channel} { +PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.timer_channel} { config.init_function(); } diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index f8621da..1294591 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -9,7 +9,7 @@ #include "hal/spi.hpp" namespace hal { -Spi::Spi(Config& config) : handle{config.handle}, gpio{config.gpio}, timeout{config.timeout} { +Spi::Spi(const Config& config) : handle{config.handle}, gpio{config.gpio}, timeout{config.timeout} { config.init_function(); } diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index fa2e9dd..bd7febe 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -9,7 +9,7 @@ #include "hal/timer.hpp" namespace hal { -Timer::Timer(Config& config) : handle{config.handle} { +Timer::Timer(const Config& config) : handle{config.handle} { config.init_function(); uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); diff --git a/src/proxy/argb.cpp b/src/proxy/argb.cpp index c5db008..8e1f077 100644 --- a/src/proxy/argb.cpp +++ b/src/proxy/argb.cpp @@ -13,7 +13,7 @@ namespace proxy { template -Argb::Argb(Config& config) : pwm{config.pwm}, +Argb::Argb(const Config& config) : pwm{config.pwm}, low_bit{pwm.get_compare(low_duty_cycle)}, high_bit{pwm.get_compare(high_duty_cycle)} { } diff --git a/src/proxy/battery.cpp b/src/proxy/battery.cpp index 62055b2..3eac595 100644 --- a/src/proxy/battery.cpp +++ b/src/proxy/battery.cpp @@ -9,7 +9,7 @@ #include "proxy/battery.hpp" namespace proxy { -Battery::Battery(Config& config) : adc{config.adc}, voltage_divider{config.voltage_divider} { +Battery::Battery(const Config& config) : adc{config.adc}, voltage_divider{config.voltage_divider} { this->adc.start_dma(&(this->raw_reading), 1); } diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index 936dcd0..f667786 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -9,7 +9,7 @@ #include "proxy/button.hpp" namespace proxy { -Button::Button(Config& config) : +Button::Button(const Config& config) : debounce_delay{config.debounce_delay}, long_press_delay{config.long_press_delay}, extra_long_press_delay{config.extra_long_press_delay}, diff --git a/src/proxy/buzzer.cpp b/src/proxy/buzzer.cpp index 43349a6..eb968e4 100644 --- a/src/proxy/buzzer.cpp +++ b/src/proxy/buzzer.cpp @@ -9,7 +9,7 @@ #include "proxy/buzzer.hpp" namespace proxy { -Buzzer::Buzzer(Config& config) : pwm{config.pwm} { +Buzzer::Buzzer(const Config& config) : pwm{config.pwm} { this->stop(); } diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp index 08a7ad5..490784d 100644 --- a/src/proxy/current_sensors.cpp +++ b/src/proxy/current_sensors.cpp @@ -13,7 +13,7 @@ namespace proxy { template -CurrentSensors::CurrentSensors(Config& config) : +CurrentSensors::CurrentSensors(const Config& config) : adc{config.adc}, shunt_resistor{config.shunt_resistor} { this->adc.start_dma(this->buffer.data(), num_of_sensors); } diff --git a/src/proxy/dip_switch.cpp b/src/proxy/dip_switch.cpp index af90074..47f41b3 100644 --- a/src/proxy/dip_switch.cpp +++ b/src/proxy/dip_switch.cpp @@ -14,7 +14,7 @@ namespace proxy { template -DipSwitch::DipSwitch(Config& config) : +DipSwitch::DipSwitch(const Config& config) : gpio_array([&] (std::index_sequence) -> std::array { return {hal::Gpio{config.gpio_array[I]}...}; }(std::make_index_sequence())) { diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index c009628..702602f 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -13,7 +13,7 @@ namespace proxy { template -DistanceSensors::DistanceSensors(Config& config) : +DistanceSensors::DistanceSensors(const Config& config) : adc{config.adc}, led_pwm{config.led_pwm}, max_distance{config.max_distance} { this->adc.start_dma(this->buffer.data(), num_of_sensors); this->led_pwm.set_duty_cycle(100.0F); diff --git a/src/proxy/fan.cpp b/src/proxy/fan.cpp index aeb2818..9f00b8c 100644 --- a/src/proxy/fan.cpp +++ b/src/proxy/fan.cpp @@ -9,7 +9,8 @@ #include "proxy/fan.hpp" namespace proxy { -Fan::Fan(Config& config) : pwm{config.pwm}, direction_gpio{config.direction_gpio}, enable_gpio{config.enable_gpio} { +Fan::Fan(const Config& config) : + pwm{config.pwm}, direction_gpio{config.direction_gpio}, enable_gpio{config.enable_gpio} { this->stop(); this->enable(); } diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 9ec723c..4918018 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -12,13 +12,13 @@ #include "proxy/imu.hpp" namespace proxy { -Imu::Imu(Config& config) : spi{config.spi}, +Imu::Imu(const Config& config) : spi{config.spi}, gy_factor{mdps_to_radps * 4.375F * (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale)))}, xl_factor{mg_to_mps2 * (0.61F * (1 << static_cast(config.accelerometer_scale)))} { this->dev_ctx.read_reg = platform_read; this->dev_ctx.write_reg = platform_write; - this->dev_ctx.handle = &config.spi; + this->dev_ctx.handle = &this->spi; hal::Timer::sleep_ms(10); diff --git a/src/proxy/led.cpp b/src/proxy/led.cpp index 976c766..56e3db5 100644 --- a/src/proxy/led.cpp +++ b/src/proxy/led.cpp @@ -9,7 +9,7 @@ #include "proxy/led.hpp" namespace proxy { -Led::Led(Config& config) : gpio{config.gpio} { +Led::Led(const Config& config) : gpio{config.gpio} { } void Led::turn_on() { diff --git a/src/proxy/locomotion.cpp b/src/proxy/locomotion.cpp index 20951be..f7afc81 100644 --- a/src/proxy/locomotion.cpp +++ b/src/proxy/locomotion.cpp @@ -11,7 +11,7 @@ #include "proxy/locomotion.hpp" namespace proxy { -Locomotion::Locomotion(Config& config) : +Locomotion::Locomotion(const Config& config) : pwm_left_fwd{config.pwm_left_fwd}, pwm_left_bwd{config.pwm_left_bwd}, pwm_right_fwd{config.pwm_right_fwd}, pwm_right_bwd{config.pwm_right_bwd}, enable_gpio{config.enable_gpio} { diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index 4639ad3..d62493a 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -11,7 +11,7 @@ #include "proxy/rotary_sensor.hpp" namespace proxy { -RotarySensor::RotarySensor(Config& config) : +RotarySensor::RotarySensor(const Config& config) : spi{config.spi}, encoder{config.encoder}, crc{config.crc}, resolution{config.resolution} { CommandFrame command_frame{ }; DataFrame data_frame{ }; diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index 2b2be34..09760ba 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -10,7 +10,7 @@ #include "proxy/storage.hpp" namespace proxy { -Storage::Storage(Config& config) : start_page{config.start_page}, number_of_pages{config.number_of_pages} { +Storage::Storage(const Config& config) : start_page{config.start_page}, number_of_pages{config.number_of_pages} { uint64_t sizes{ }; hal::Flash::read(this->start_page, 0, &sizes); diff --git a/src/proxy/torque_sensors.cpp b/src/proxy/torque_sensors.cpp index 447bc59..d721b2a 100644 --- a/src/proxy/torque_sensors.cpp +++ b/src/proxy/torque_sensors.cpp @@ -13,7 +13,7 @@ namespace proxy { template -TorqueSensors::TorqueSensors(Config& config) : +TorqueSensors::TorqueSensors(const Config& config) : current_sensors{config.current_sensors}, max_torque{config.max_torque} { } From 9ac16fcf8193dd473030c8c8da4a972ad5a194a1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 18:20:30 -0300 Subject: [PATCH 117/561] =?UTF-8?q?=F0=9F=94=A7=20Update=20linter=20checks?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 718f9d3..0447496 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -2,18 +2,20 @@ Checks: -*, bugprone-*, + -bugprone-easily-swappable-parameters, clang-*, + -clang-diagnostic-unused-command-line-argument, cppcoreguidelines-*, + -cppcoreguidelines-avoid-c-arrays, -cppcoreguidelines-avoid-magic-numbers, -cppcoreguidelines-pro-type-union-access, - -cppcoreguidelines-avoid-c-arrays, - -cppcoreguidelines-pro-type-reinterpret-cast, google-*, llvm-*, + -llvm-header-guard, misc-*, modernize-*, - -modernize-use-trailing-return-type, -modernize-avoid-c-arrays, + -modernize-use-trailing-return-type, performance-*, readability-*, -readability-magic-numbers, From 50c6cc01e8261d584cd4fde223c5a436f2fda0f7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 23:28:18 -0300 Subject: [PATCH 118/561] =?UTF-8?q?=F0=9F=94=A7=20Update=20linter=20checks?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 0447496..e44ad17 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -3,18 +3,20 @@ Checks: -*, bugprone-*, -bugprone-easily-swappable-parameters, + -bugprone-narrowing-conversions, clang-*, -clang-diagnostic-unused-command-line-argument, cppcoreguidelines-*, - -cppcoreguidelines-avoid-c-arrays, -cppcoreguidelines-avoid-magic-numbers, + -cppcoreguidelines-pro-bounds-pointer-arithmetic, + -cppcoreguidelines-pro-type-reinterpret-cast, + -cppcoreguidelines-narrowing-conversions, -cppcoreguidelines-pro-type-union-access, google-*, llvm-*, -llvm-header-guard, misc-*, modernize-*, - -modernize-avoid-c-arrays, -modernize-use-trailing-return-type, performance-*, readability-*, From 91554199735beaa6e1495084ce58f5215dc825e7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 2 Apr 2024 23:37:05 -0300 Subject: [PATCH 119/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/imu.hpp | 4 ++-- src/hal/adc_dma.cpp | 1 + src/hal/crc.cpp | 1 + src/hal/flash.cpp | 8 ++++++-- src/hal/mcu.cpp | 1 - src/hal/pwm.cpp | 2 +- src/hal/pwm_dma.cpp | 3 ++- src/hal/spi.cpp | 2 ++ src/proxy/imu.cpp | 13 +++++++------ src/proxy/storage.cpp | 20 ++++++++++---------- 10 files changed, 32 insertions(+), 23 deletions(-) diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 4730d91..589c574 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -146,12 +146,12 @@ class Imu { /** * @brief Current angular velocity on each axis */ - std::array angular_velocity{ }; + std::array angular_velocity{ }; /** * @brief Current linear acceleration on each axis */ - std::array linear_acceleration{ }; + std::array linear_acceleration{ }; /** * @brief Current orientation diff --git a/src/hal/adc_dma.cpp b/src/hal/adc_dma.cpp index 9afb885..7d063c4 100644 --- a/src/hal/adc_dma.cpp +++ b/src/hal/adc_dma.cpp @@ -14,6 +14,7 @@ AdcDma::AdcDma(const Config& config) : config.init_function(); } +// NOLINTNEXTLINE(*-avoid-c-arrays) void AdcDma::start_dma(uint32_t buffer[], uint32_t size) { HAL_ADC_Start_DMA(this->handle, buffer, size); } diff --git a/src/hal/crc.cpp b/src/hal/crc.cpp index cc3c988..353a009 100644 --- a/src/hal/crc.cpp +++ b/src/hal/crc.cpp @@ -12,6 +12,7 @@ namespace hal { Crc::Crc(const Config& config) : handle{config.handle} { } +// NOLINTNEXTLINE(*-avoid-c-arrays) uint32_t Crc::calculate(uint32_t data[], uint32_t size) { return HAL_CRC_Calculate(this->handle, data, size); } diff --git a/src/hal/flash.cpp b/src/hal/flash.cpp index 497d982..1c5eead 100644 --- a/src/hal/flash.cpp +++ b/src/hal/flash.cpp @@ -15,18 +15,21 @@ namespace hal { */ static const uint32_t base_address = FLASH_BASE + FLASH_SIZE - 8; +// NOLINTNEXTLINE(*-avoid-c-arrays) void Flash::read(uint32_t address, uint64_t data[], uint32_t size) { uint32_t end = base_address - 8 * address; for (address = end - 8 * (size - 1); address <= end; address += 8, data++) { - (*data) = *(reinterpret_cast(address)); + (*data) = *(reinterpret_cast(address)); // NOLINT(performance-no-int-to-ptr) } } +// NOLINTNEXTLINE(*-avoid-c-arrays) void Flash::read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size) { read(page * double_words_per_page + page_address, data, size); } +// NOLINTNEXTLINE(*-avoid-c-arrays) void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { HAL_FLASH_Unlock(); @@ -35,7 +38,7 @@ void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { while (address <= end) { if (size >= double_words_per_row) { - HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, reinterpret_cast(data)); + HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, reinterpret_cast(data)); // NOLINT(clang-diagnostic-pointer-to-int-cast) address += bytes_per_row; data += double_words_per_row; size -= double_words_per_row; @@ -50,6 +53,7 @@ void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { HAL_FLASH_Lock(); } +// NOLINTNEXTLINE(*-avoid-c-arrays) void Flash::write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size) { write(page * double_words_per_page + page_address, data, size); } diff --git a/src/hal/mcu.cpp b/src/hal/mcu.cpp index b5568c0..f0731f9 100644 --- a/src/hal/mcu.cpp +++ b/src/hal/mcu.cpp @@ -7,7 +7,6 @@ #include #include #include -#include #include "hal/mcu.hpp" diff --git a/src/hal/pwm.cpp b/src/hal/pwm.cpp index 9b1b344..dda3da7 100644 --- a/src/hal/pwm.cpp +++ b/src/hal/pwm.cpp @@ -18,7 +18,7 @@ Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_cha } void Pwm::set_duty_cycle(float duty_cycle) { - uint32_t compare = std::round((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); + uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); } diff --git a/src/hal/pwm_dma.cpp b/src/hal/pwm_dma.cpp index 6321656..ef84d46 100644 --- a/src/hal/pwm_dma.cpp +++ b/src/hal/pwm_dma.cpp @@ -15,6 +15,7 @@ PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.tim config.init_function(); } +// NOLINTNEXTLINE(*-avoid-c-arrays) void PwmDma::start_dma(uint32_t buffer[], uint32_t size) { if (TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_CHANNEL_STATE_BUSY) { HAL_TIM_PWM_Stop_DMA(this->handle, this->channel); @@ -28,6 +29,6 @@ void PwmDma::stop_dma() { } uint32_t PwmDma::get_compare(float duty_cycle) const { - return std::round((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); + return std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); } } // namespace hal diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index 1294591..0850c8f 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -26,10 +26,12 @@ void Spi::unselect_device() { this->gpio.write(true); } +// NOLINTNEXTLINE(*-avoid-c-arrays) void Spi::transmit(uint8_t data[], uint32_t size) { HAL_SPI_Transmit(this->handle, data, size, this->timeout); } +// NOLINTNEXTLINE(*-avoid-c-arrays) void Spi::receive(uint8_t data[], uint32_t size) { HAL_SPI_Receive(this->handle, data, size, this->timeout); } diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 4918018..00ebc7c 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -166,12 +166,13 @@ int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint } spi->transmit(®, 1); - spi->transmit((uint8_t*) bufp, len); + spi->transmit(const_cast(bufp), len); // NOLINT(cppcoreguidelines-pro-type-const-cast) spi->unselect_device(); return 0; } +// NOLINTNEXTLINE(*-avoid-c-arrays) void Imu::convert_orientation(std::array& quat, const uint16_t sflp[3]) { float sumsq = 0; @@ -180,14 +181,14 @@ void Imu::convert_orientation(std::array& quat, const uint16_t sflp[3] quat[2] = half_to_float(sflp[2]); for (uint8_t i = 0; i < 3; i++) { - sumsq += quat[i] * quat[i]; + sumsq += quat[i] * quat[i]; // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index) } if (sumsq > 1.0F) { - float n = std::sqrt(sumsq); - quat[0] /= n; - quat[1] /= n; - quat[2] /= n; + float norm = std::sqrt(sumsq); + quat[0] /= norm; + quat[1] /= norm; + quat[2] /= norm; sumsq = 1.0F; } diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index 09760ba..afd0ab1 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -38,7 +38,7 @@ void Storage::create(const std::string& name, const ISerializable& data) { template void Storage::sync(const std::string& name, T& data) { if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr) { - data = *reinterpret_cast(&this->buffer.at(this->primitives.at(name).buffer_address)); + data = reinterpret_cast(this->buffer.at(this->primitives.at(name).buffer_address)); } this->create(name, data); @@ -123,21 +123,21 @@ std::vector Storage::serialize_var_map(const std::unordered_map std::unordered_map Storage::deserialize_var_map(std::vector& buffer, uint16_t num_vars) { std::unordered_map variables; - uint16_t i = 0; + uint16_t current_addr = 0; for (uint16_t decoded_vars = 0; decoded_vars < num_vars; decoded_vars++) { - uint8_t var_name_len = buffer.at(i); - std::string var_name(buffer.begin() + i + 1, buffer.begin() + i + 1 + var_name_len); - i += var_name_len + 1; + uint8_t var_name_len = buffer.at(current_addr); + std::string var_name(buffer.begin() + current_addr + 1, buffer.begin() + current_addr + 1 + var_name_len); + current_addr += var_name_len + 1; - variables[var_name].buffer_address = buffer.at(i) | buffer.at(i + 1) << 8; - i += 2; + variables[var_name].buffer_address = buffer.at(current_addr) | buffer.at(current_addr + 1) << 8; + current_addr += 2; - variables.at(var_name).size = buffer.at(i) | buffer.at(i + 1) << 8; - i += 2; + variables.at(var_name).size = buffer.at(current_addr) | buffer.at(current_addr + 1) << 8; + current_addr += 2; } - buffer.erase(buffer.begin(), buffer.begin() + i); + buffer.erase(buffer.begin(), buffer.begin() + current_addr); return variables; } } // namespace proxy From ad71a5e6d63534cbb4482dcf31daabdfa5485da3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 3 Apr 2024 00:20:49 -0300 Subject: [PATCH 120/561] =?UTF-8?q?=F0=9F=92=84=20=F0=9F=9A=A8=20Fix=20boo?= =?UTF-8?q?l=20implicit=20conversion=20linter=20warnings?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/hal/gpio.cpp | 2 +- src/proxy/argb.cpp | 4 ++-- src/proxy/button.cpp | 2 +- src/proxy/imu.cpp | 23 +++++++++++++++-------- 4 files changed, 19 insertions(+), 12 deletions(-) diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index 46710d1..4117ae6 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -13,7 +13,7 @@ Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { } bool Gpio::read() const { - return HAL_GPIO_ReadPin(this->port, this->pin); + return HAL_GPIO_ReadPin(this->port, this->pin) == GPIO_PIN_SET; } void Gpio::write(bool state) { diff --git a/src/proxy/argb.cpp b/src/proxy/argb.cpp index 8e1f077..d004eb4 100644 --- a/src/proxy/argb.cpp +++ b/src/proxy/argb.cpp @@ -50,8 +50,8 @@ template void Argb::encode_color(const Color& color, uint8_t index) { uint32_t data = color.green << (2 * bits_per_color) | color.red << bits_per_color | color.blue; - for (uint8_t i = bits_per_led * index, j = 0; i < bits_per_led * (index + 1); i++) { - this->buffer.at(i) = ((data >> j) & 1) ? this->high_bit : this->low_bit; + for (uint32_t i = bits_per_led * index, j = 0; i < bits_per_led * (index + 1); i++) { + this->buffer.at(i) = ((data >> j) & 1) == 1 ? this->high_bit : this->low_bit; j++; } } diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index f667786..63278b9 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -43,7 +43,7 @@ Button::Status Button::get_status() { } bool Button::get_raw_reading() const { - return this->gpio.read() == this->pull_resistor; + return this->gpio.read() == static_cast(this->pull_resistor); } void Button::update_state() { diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 00ebc7c..8714353 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -66,7 +66,7 @@ void Imu::update_data() { uint16_t samples = fifo_status.fifo_level; - while (samples--) { + while ((samples--) > 0) { lsm6dsv_fifo_out_raw_t f_data; lsm6dsv_fifo_out_raw_get(&(this->dev_ctx), &f_data); auto* axis = reinterpret_cast(f_data.data); @@ -195,13 +195,18 @@ void Imu::convert_orientation(std::array& quat, const uint16_t sflp[3] quat[3] = std::sqrt(1.0F - sumsq); } -float Imu::half_to_float(const uint16_t x) { - static auto as_float = [](uint32_t x) -> float { - return *reinterpret_cast(&x); - }; - static auto as_uint = [](float x) -> uint32_t { - return *reinterpret_cast(&x); - }; +// NOLINTBEGIN(readability-identifier-length, readability-implicit-bool-conversion) +float Imu::half_to_float(uint16_t x) { + static constexpr auto as_float = + [](uint32_t x) -> float { + void* aux = &x; + return *reinterpret_cast(aux); + }; + static constexpr auto as_uint = + [](float x) -> uint32_t { + void* aux = &x; + return *reinterpret_cast(aux); + }; const uint32_t e = (x & 0x7C00) >> 10; const uint32_t m = (x & 0x03FF) << 13; @@ -211,4 +216,6 @@ float Imu::half_to_float(const uint16_t x) { (e != 0) * ((e + 112) << 23 | m) | ((e == 0) & (m != 0)) * ((v - 37) << 23 | ((m << (150 - v)) & 0x007FE000))); } + +// NOLINTEND(readability-identifier-length, readability-implicit-bool-conversion) } // namespace proxy From bf9d6904b9ec3cc85384ae5204e8615b0e741493 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 3 Apr 2024 01:46:33 -0300 Subject: [PATCH 121/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= =?UTF-8?q?=20on=20header=20files?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 12 ++++++------ inc/hal/crc.hpp | 4 ++-- inc/hal/encoder.hpp | 4 ++-- inc/hal/flash.hpp | 8 ++++---- inc/hal/gpio.hpp | 2 +- inc/hal/pwm.hpp | 4 ++-- inc/hal/pwm_dma.hpp | 6 +++--- inc/hal/spi.hpp | 8 ++++---- inc/hal/timer.hpp | 6 +++--- inc/proxy/argb.hpp | 8 ++++---- inc/proxy/battery.hpp | 2 +- inc/proxy/button.hpp | 6 +++--- inc/proxy/buzzer.hpp | 2 +- inc/proxy/current_sensors.hpp | 4 ++-- inc/proxy/dip_switch.hpp | 4 ++-- inc/proxy/distance_sensors.hpp | 4 ++-- inc/proxy/fan.hpp | 4 ++-- inc/proxy/imu.hpp | 6 +++--- inc/proxy/led.hpp | 2 +- inc/proxy/locomotion.hpp | 4 ++-- inc/proxy/rotary_sensor.hpp | 2 +- inc/proxy/serializable_interface.hpp | 29 ++++++++++++++++++++++++++++ inc/proxy/storage.hpp | 10 +++++----- inc/proxy/torque_sensors.hpp | 4 ++-- 24 files changed, 87 insertions(+), 58 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index e3bc73d..3a409d1 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -23,7 +23,7 @@ class AdcDma { */ struct Config { ADC_HandleTypeDef* handle; - void (* init_function)(void); + void (* init_function)(); uint32_t max_reading; float reference_voltage; }; @@ -33,7 +33,7 @@ class AdcDma { * * @param config ADC DMA configuration struct */ - AdcDma(const Config& config); + explicit AdcDma(const Config& config); /** * @brief Enable ADC, start conversion of regular group and transfer result through DMA @@ -41,7 +41,7 @@ class AdcDma { * @param buffer Destination Buffer address * @param size Number of data to be transferred from ADC DMA peripheral to memory */ - void start_dma(uint32_t buffer[], uint32_t size); + void start_dma(uint32_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) /** * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode) @@ -51,12 +51,12 @@ class AdcDma { /** * @brief Maximum ADC reading */ - const uint32_t max_reading; + const uint32_t max_reading; // NOLINT(*-non-private-member-variables-in-classes) /** * @brief Reference voltage for the ADC measurement */ - const float reference_voltage; + const float reference_voltage; // NOLINT(*-non-private-member-variables-in-classes) private: /** @@ -64,6 +64,6 @@ class AdcDma { */ ADC_HandleTypeDef* handle; }; -} +} // namespace hal #endif // MICRAS_HAL_ADC_DMA_HPP diff --git a/inc/hal/crc.hpp b/inc/hal/crc.hpp index 66fd36c..31a8c05 100644 --- a/inc/hal/crc.hpp +++ b/inc/hal/crc.hpp @@ -29,7 +29,7 @@ class Crc { * * @param config Configuration for the CRC */ - Crc(const Config& config); + explicit Crc(const Config& config); /** * @brief Calculate the CRC value @@ -39,7 +39,7 @@ class Crc { * * @return CRC value */ - uint32_t calculate(uint32_t data[], uint32_t size); + uint32_t calculate(uint32_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) private: /** diff --git a/inc/hal/encoder.hpp b/inc/hal/encoder.hpp index bd9548c..397e715 100644 --- a/inc/hal/encoder.hpp +++ b/inc/hal/encoder.hpp @@ -22,7 +22,7 @@ class Encoder { */ struct Config { TIM_HandleTypeDef* handle; - void (* init_function)(void); + void (* init_function)(); uint32_t timer_channel; }; @@ -31,7 +31,7 @@ class Encoder { * * @param config Configuration for the encoder */ - Encoder(const Config& config); + explicit Encoder(const Config& config); /** * @brief Get the counter value diff --git a/inc/hal/flash.hpp b/inc/hal/flash.hpp index f613a03..3641402 100644 --- a/inc/hal/flash.hpp +++ b/inc/hal/flash.hpp @@ -30,7 +30,7 @@ class Flash { * @param data pointer to store the data read * @param size size in double words of the data to read */ - static void read(uint32_t address, uint64_t data[], uint32_t size = 1); + static void read(uint32_t address, uint64_t data[], uint32_t size = 1); // NOLINT(*-avoid-c-arrays) /** * @brief Read data from flash memory @@ -40,7 +40,7 @@ class Flash { * @param data pointer to store the data read * @param size size in double words of the data to read */ - static void read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); + static void read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); // NOLINT(*-avoid-c-arrays) /** * @brief Write data to flash memory @@ -49,7 +49,7 @@ class Flash { * @param data pointer to the data to write * @param size size in double words of the data to write */ - static void write(uint32_t address, const uint64_t data[], uint32_t size = 1); + static void write(uint32_t address, const uint64_t data[], uint32_t size = 1); // NOLINT(*-avoid-c-arrays) /** * @brief Write data to flash memory @@ -59,7 +59,7 @@ class Flash { * @param data pointer to the data to write * @param size size in double words of the data to write */ - static void write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size = 1); + static void write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size = 1); // NOLINT(*-avoid-c-arrays) /** * @brief Erase flash memory pages diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index 8dac60e..fa16041 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -31,7 +31,7 @@ class Gpio { * * @param config Configuration for the GPIO pin */ - Gpio(const Config& config); + explicit Gpio(const Config& config); /** * @brief Read the current state of the GPIO pin diff --git a/inc/hal/pwm.hpp b/inc/hal/pwm.hpp index b8ffc55..efbdbd5 100644 --- a/inc/hal/pwm.hpp +++ b/inc/hal/pwm.hpp @@ -23,7 +23,7 @@ class Pwm { */ struct Config { TIM_HandleTypeDef* handle; - void (* init_function)(void); + void (* init_function)(); uint32_t timer_channel; }; @@ -32,7 +32,7 @@ class Pwm { * * @param config Configuration for the PWM */ - Pwm(const Config& config); + explicit Pwm(const Config& config); /** * @brief Set the PWM duty cycle diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp index 4c67402..0cd993f 100644 --- a/inc/hal/pwm_dma.hpp +++ b/inc/hal/pwm_dma.hpp @@ -23,7 +23,7 @@ class PwmDma { */ struct Config { TIM_HandleTypeDef* handle; - void (* init_function)(void); + void (* init_function)(); uint32_t timer_channel; }; @@ -32,7 +32,7 @@ class PwmDma { * * @param config Configuration for the PWM */ - PwmDma(const Config& config); + explicit PwmDma(const Config& config); /** * @brief Start PWM and DMA transfer @@ -40,7 +40,7 @@ class PwmDma { * @param buffer Buffer to transfer * @param size Size of the buffer */ - void start_dma(uint32_t buffer[], uint32_t size); + void start_dma(uint32_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) /** * @brief Stop PWM and DMA transfer diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index 1fbaf14..aae0923 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -25,7 +25,7 @@ class Spi { */ struct Config { SPI_HandleTypeDef* handle; - void (* init_function)(void); + void (* init_function)(); hal::Gpio::Config gpio; uint32_t timeout; }; @@ -35,7 +35,7 @@ class Spi { * * @param config Configuration for the SPI */ - Spi(const Config& config); + explicit Spi(const Config& config); /** * @brief Activate the chip select @@ -55,7 +55,7 @@ class Spi { * @param data Data to transmit * @param size Size of the buffer */ - void transmit(uint8_t data[], uint32_t size); + void transmit(uint8_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) /** * @brief Receive data over SPI @@ -63,7 +63,7 @@ class Spi { * @param data Data to receive data * @param size Size of the data */ - void receive(uint8_t data[], uint32_t size); + void receive(uint8_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) private: /** diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index 245a289..c963438 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -23,7 +23,7 @@ class Timer { */ struct Config { TIM_HandleTypeDef* handle; - void (* init_function)(void); + void (* init_function)(); }; /** @@ -36,7 +36,7 @@ class Timer { * * @param config Configuration for the timer */ - Timer(const Config& config); + explicit Timer(const Config& config); /** * @brief Reset the timer counter in milliseconds @@ -94,7 +94,7 @@ class Timer { /** * @brief Timer handle */ - TIM_HandleTypeDef* handle; + TIM_HandleTypeDef* handle{ }; /** * @brief Timer counter diff --git a/inc/proxy/argb.hpp b/inc/proxy/argb.hpp index e9d8287..a1db000 100644 --- a/inc/proxy/argb.hpp +++ b/inc/proxy/argb.hpp @@ -42,7 +42,7 @@ class Argb { * * @param config Configuration for the addressable RGB LED */ - Argb(const Config& config); + explicit Argb(const Config& config); /** * @brief Set the color of the ARGB at the specified index @@ -86,8 +86,8 @@ class Argb { static constexpr uint8_t bits_per_color{8}; static constexpr uint8_t colors_per_led{3}; static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; - static constexpr float low_duty_cycle{0.32F}; - static constexpr float high_duty_cycle{0.64F}; + static constexpr float low_duty_cycle{0.32F}; // NOLINT(bugprone-dynamic-static-initializers) + static constexpr float high_duty_cycle{0.64F}; // NOLINT(bugprone-dynamic-static-initializers) static constexpr uint8_t reset_length{40}; /** @@ -112,6 +112,6 @@ class Argb { }; } // namespace proxy -#include "../src/proxy/argb.cpp" +#include "../src/proxy/argb.cpp" // NOLINT(bugprone-suspicious-include) #endif // MICRAS_PROXY_ARGB_HPP diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp index dbe0c8e..4d82ad0 100644 --- a/inc/proxy/battery.hpp +++ b/inc/proxy/battery.hpp @@ -32,7 +32,7 @@ class Battery { * * @param config Configuration for the battery */ - Battery(const Config& config); + explicit Battery(const Config& config); /** * @brief Get the battery voltage diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index cfa9202..f00ad68 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -42,8 +42,8 @@ class Button { * @brief Configuration structure for button */ struct Config { - hal::Gpio::Config gpio; - PullResistor pull_resistor; + hal::Gpio::Config gpio{ }; + PullResistor pull_resistor{ }; uint16_t debounce_delay = 10; uint16_t long_press_delay = 1000; uint16_t extra_long_press_delay = 5000; @@ -54,7 +54,7 @@ class Button { * * @param config Button configuration */ - Button(const Config& config); + explicit Button(const Config& config); /** * @brief Check if button is pressed diff --git a/inc/proxy/buzzer.hpp b/inc/proxy/buzzer.hpp index 0fcff8d..9585441 100644 --- a/inc/proxy/buzzer.hpp +++ b/inc/proxy/buzzer.hpp @@ -32,7 +32,7 @@ class Buzzer { * * @param config Configuration for the buzzer */ - Buzzer(const Config& config); + explicit Buzzer(const Config& config); /** * @brief Play a tone for a duration diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index 726050a..7763346 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -34,7 +34,7 @@ class CurrentSensors { * * @param config Configuration for the current sensors */ - CurrentSensors(const Config& config); + explicit CurrentSensors(const Config& config); /** * @brief Get the current from the sensor @@ -70,6 +70,6 @@ class CurrentSensors { }; } // namespace proxy -#include "../src/proxy/current_sensors.cpp" +#include "../src/proxy/current_sensors.cpp" // NOLINT(bugprone-suspicious-include) #endif // MICRAS_PROXY_CURRENT_SENSORS_HPP diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp index 392d22b..cc7e721 100644 --- a/inc/proxy/dip_switch.hpp +++ b/inc/proxy/dip_switch.hpp @@ -33,7 +33,7 @@ class DipSwitch { * * @param config Configuration struct for DipSwitch */ - DipSwitch(const Config& config); + explicit DipSwitch(const Config& config); /** * @brief Get the state of a switch @@ -59,6 +59,6 @@ class DipSwitch { }; } // namespace proxy -#include "../src/proxy/dip_switch.cpp" +#include "../src/proxy/dip_switch.cpp" // NOLINT(bugprone-suspicious-include) #endif // MICRAS_PROXY_DIP_SWITCH_HPP diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index dd4d928..7558fb2 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -36,7 +36,7 @@ class DistanceSensors { * * @param config Configuration for the distance sensors */ - DistanceSensors(const Config& config); + explicit DistanceSensors(const Config& config); /** * @brief Set the distance sensors led intensity @@ -84,6 +84,6 @@ class DistanceSensors { }; } // namespace proxy -#include "../src/proxy/distance_sensors.cpp" +#include "../src/proxy/distance_sensors.cpp" // NOLINT(bugprone-suspicious-include) #endif // MICRAS_PROXY_DISTANCE_SENSORS_HPP diff --git a/inc/proxy/fan.hpp b/inc/proxy/fan.hpp index 0453575..dcbcc2c 100644 --- a/inc/proxy/fan.hpp +++ b/inc/proxy/fan.hpp @@ -11,8 +11,8 @@ #include -#include "hal/pwm.hpp" #include "hal/gpio.hpp" +#include "hal/pwm.hpp" namespace proxy { /** @@ -42,7 +42,7 @@ class Fan { * * @param config Configuration for the fan driver */ - Fan(const Config& config); + explicit Fan(const Config& config); /** * @brief Enable the fan diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 589c574..226380a 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -41,7 +41,7 @@ class Imu { * * @param config Configuration for the IMU */ - Imu(const Config& config); + explicit Imu(const Config& config); enum Axis { W, @@ -116,7 +116,7 @@ class Imu { * * @return Orientation in quaternions */ - static void convert_orientation(std::array& quat, const uint16_t sflp[3]); + static void convert_orientation(std::array& quat, const uint16_t sflp[3]); // NOLINT(*-avoid-c-arrays) /** * @brief Function to convert half precision float to single precision float @@ -125,7 +125,7 @@ class Imu { * * @return Single precision float */ - static float half_to_float(uint16_t x); + static float half_to_float(uint16_t x); // NOLINT(readability-identifier-length) /** * @brief Conversion constants diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp index b53ed47..da059ed 100644 --- a/inc/proxy/led.hpp +++ b/inc/proxy/led.hpp @@ -29,7 +29,7 @@ class Led { * * @param config Configuration for the LED */ - Led(const Config& config); + explicit Led(const Config& config); /** * @brief Turn the LED on diff --git a/inc/proxy/locomotion.hpp b/inc/proxy/locomotion.hpp index b0b2bd3..2cd0e09 100644 --- a/inc/proxy/locomotion.hpp +++ b/inc/proxy/locomotion.hpp @@ -11,8 +11,8 @@ #include -#include "hal/pwm.hpp" #include "hal/gpio.hpp" +#include "hal/pwm.hpp" namespace proxy { /** @@ -36,7 +36,7 @@ class Locomotion { * * @param config Configuration for the locomotion driver */ - Locomotion(const Config& config); + explicit Locomotion(const Config& config); /** * @brief Enable the locomotion driver diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index 40f3854..91e337f 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -39,7 +39,7 @@ class RotarySensor { * * @param config Configuration for the rotary sensor */ - RotarySensor(const Config& config); + explicit RotarySensor(const Config& config); /** * @brief Get the rotary sensor position over an axis diff --git a/inc/proxy/serializable_interface.hpp b/inc/proxy/serializable_interface.hpp index dda86c9..4aec07d 100644 --- a/inc/proxy/serializable_interface.hpp +++ b/inc/proxy/serializable_interface.hpp @@ -12,13 +12,42 @@ #include #include +/** + * @brief Interface class for serializable classes + */ class ISerializable { public: + /** + * @brief Virtual destructor for the ISerializable class + */ virtual ~ISerializable() = default; + /** + * @brief Serialize the class instance + * + * @return std::vector Serialized data + */ virtual std::vector serialize() const = 0; + /** + * @brief Deserialize the class instance + * + * @param serial_data Serialized data + * @param size Size of the serialized data + */ virtual void deserialize(uint8_t* serial_data, uint16_t size) = 0; + + protected: + /** + * @brief Special member functions declared as default + */ + ISerializable(const ISerializable&) = default; + + ISerializable(ISerializable&&) = default; + + ISerializable& operator =(const ISerializable&) = default; + + ISerializable& operator =(ISerializable&&) = default; }; #endif // MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp index dbc1fde..dba2cc0 100644 --- a/inc/proxy/storage.hpp +++ b/inc/proxy/storage.hpp @@ -39,7 +39,7 @@ class Storage { * * @param config Configuration for the storage */ - Storage(const Config& config); + explicit Storage(const Config& config); /** * @brief Create a new primitive variable in the storage @@ -88,8 +88,8 @@ class Storage { */ struct PrimitiveVariable { const void* ram_pointer{nullptr}; - uint16_t buffer_address; - uint16_t size; + uint16_t buffer_address{ }; + uint16_t size{ }; }; /** @@ -97,8 +97,8 @@ class Storage { */ struct SerializableVariable { const ISerializable* ram_pointer{nullptr}; - uint16_t buffer_address; - uint16_t size; + uint16_t buffer_address{ }; + uint16_t size{ }; }; /** diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index a282378..282f1e7 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -33,7 +33,7 @@ class TorqueSensors { * * @param config Configuration for the torque sensors */ - TorqueSensors(const Config& config); + explicit TorqueSensors(const Config& config); /** * @brief Get the torque from the sensor @@ -64,6 +64,6 @@ class TorqueSensors { }; } // namespace proxy -#include "../src/proxy/torque_sensors.cpp" +#include "../src/proxy/torque_sensors.cpp" // NOLINT(bugprone-suspicious-include) #endif // MICRAS_PROXY_TORQUE_SENSORS_HPP From eb0fd661f46c0db9f1695b0f52a2b15ae3a5d13a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 3 Apr 2024 01:47:43 -0300 Subject: [PATCH 122/561] =?UTF-8?q?=F0=9F=94=A7=20Add=20header=20files=20t?= =?UTF-8?q?o=20linter?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index e44ad17..ce2c114 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -8,21 +8,23 @@ Checks: -clang-diagnostic-unused-command-line-argument, cppcoreguidelines-*, -cppcoreguidelines-avoid-magic-numbers, + -cppcoreguidelines-narrowing-conversions, -cppcoreguidelines-pro-bounds-pointer-arithmetic, -cppcoreguidelines-pro-type-reinterpret-cast, - -cppcoreguidelines-narrowing-conversions, -cppcoreguidelines-pro-type-union-access, google-*, llvm-*, -llvm-header-guard, misc-*, modernize-*, + -modernize-use-nodiscard, -modernize-use-trailing-return-type, performance-*, readability-*, + -readability-duplicate-include, -readability-magic-numbers, WarningsAsErrors: '' -HeaderFilterRegex: '' +HeaderFilterRegex: 'inc/.*.hpp' AnalyzeTemporaryDtors: false FormatStyle: none ... From b03c4317f0c7a2dcaf1cc3341eb711c4832a1111 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 3 Apr 2024 02:03:21 -0300 Subject: [PATCH 123/561] =?UTF-8?q?=F0=9F=92=A1=20Fix=20doxygen=20return?= =?UTF-8?q?=20comments?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/crc.hpp | 2 +- inc/hal/gpio.hpp | 2 +- inc/hal/mcu.hpp | 1 + inc/hal/pwm_dma.hpp | 1 + inc/hal/spi.hpp | 2 +- inc/proxy/battery.hpp | 2 +- inc/proxy/button.hpp | 6 +++--- inc/proxy/dip_switch.hpp | 3 +-- inc/proxy/imu.hpp | 14 ++++++-------- inc/proxy/rotary_sensor.hpp | 2 +- inc/proxy/storage.hpp | 4 ++-- 11 files changed, 19 insertions(+), 20 deletions(-) diff --git a/inc/hal/crc.hpp b/inc/hal/crc.hpp index 31a8c05..687da18 100644 --- a/inc/hal/crc.hpp +++ b/inc/hal/crc.hpp @@ -37,7 +37,7 @@ class Crc { * @param data Data to calculate the CRC * @param size Size of the buffer * - * @return CRC value + * @return uint32_t CRC value */ uint32_t calculate(uint32_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index fa16041..eb5baaf 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -36,7 +36,7 @@ class Gpio { /** * @brief Read the current state of the GPIO pin * - * @return The current state of the GPIO pin (true for high, false for low) + * @return bool The current state of the GPIO pin (true for high, false for low) */ bool read() const; diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index 95b0954..d528cef 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -28,4 +28,5 @@ class Mcu { static void init(); }; } // namespace hal + #endif // MICRAS_HAL_MCU_HPP diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp index 0cd993f..05904cd 100644 --- a/inc/hal/pwm_dma.hpp +++ b/inc/hal/pwm_dma.hpp @@ -67,4 +67,5 @@ class PwmDma { uint32_t channel; }; } // namespace hal + #endif // MICRAS_HAL_PWM_DMA_HPP diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index aae0923..43fa004 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -40,7 +40,7 @@ class Spi { /** * @brief Activate the chip select * - * @return true if the device was successfully selected, false otherwise + * @return bool True if the device was successfully selected, false otherwise */ bool select_device(); diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp index 4d82ad0..97851a8 100644 --- a/inc/proxy/battery.hpp +++ b/inc/proxy/battery.hpp @@ -37,7 +37,7 @@ class Battery { /** * @brief Get the battery voltage * - * @return Battery voltage in volts + * @return float Battery voltage in volts */ float get_voltage() const; diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index f00ad68..ce9ed1c 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -59,14 +59,14 @@ class Button { /** * @brief Check if button is pressed * - * @return True if button is pressed, false otherwise + * @return bool True if button is pressed, false otherwise */ bool is_pressed(); /** * @brief Get button status * - * @return Button status + * @return Status Button status */ Status get_status(); @@ -74,7 +74,7 @@ class Button { /** * @brief Get raw button reading * - * @return Button reading + * @return bool Button reading without debounce */ bool get_raw_reading() const; diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp index cc7e721..a9d534c 100644 --- a/inc/proxy/dip_switch.hpp +++ b/inc/proxy/dip_switch.hpp @@ -39,8 +39,7 @@ class DipSwitch { * @brief Get the state of a switch * * @param switch_index Index of the switch - * @return true if the switch is on - * @return false if the switch is off + * @return bool True if the switch is on, false otherwise */ bool get_switch_state(uint8_t switch_index) const; diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 226380a..51d62df 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -61,7 +61,7 @@ class Imu { * * @param axis Axis to get the orientation from * - * @return Orientation over the desired axis using quaternions + * @return float Orientation over the desired axis using quaternions */ float get_orientation(Axis axis) const; @@ -70,7 +70,7 @@ class Imu { * * @param axis Axis to get the angular velocity from * - * @return Angular velocity over the desired axis in rad/s + * @return float Angular velocity over the desired axis in rad/s */ float get_angular_velocity(Axis axis) const; @@ -79,7 +79,7 @@ class Imu { * * @param axis Axis to get the linear acceleration from * - * @return Linear acceleration over the desired axis in m/sยฒ + * @return float Linear acceleration over the desired axis in m/sยฒ */ float get_linear_acceleration(Axis axis) const; @@ -92,7 +92,7 @@ class Imu { * @param bufp Buffer to read * @param len Length of the buffer * - * @return 0 if the operation was successful, -1 otherwise + * @return int32_t 0 if the operation was successful, -1 otherwise */ static int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len); @@ -104,7 +104,7 @@ class Imu { * @param bufp Buffer to write * @param len Length of the buffer * - * @return 0 if the operation was successful, -1 otherwise + * @return int32_t 0 if the operation was successful, -1 otherwise */ static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len); @@ -113,8 +113,6 @@ class Imu { * * @param quat Quaternion to store the orientation * @param sflp Raw data from the IMU - * - * @return Orientation in quaternions */ static void convert_orientation(std::array& quat, const uint16_t sflp[3]); // NOLINT(*-avoid-c-arrays) @@ -123,7 +121,7 @@ class Imu { * * @param x Half precision float * - * @return Single precision float + * @return float Single precision float */ static float half_to_float(uint16_t x); // NOLINT(readability-identifier-length) diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index 91e337f..1e04b22 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -44,7 +44,7 @@ class RotarySensor { /** * @brief Get the rotary sensor position over an axis * - * @return Current angular position of the sensor in radians + * @return float Current angular position of the sensor in radians */ float get_position() const; diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp index dba2cc0..4ec8875 100644 --- a/inc/proxy/storage.hpp +++ b/inc/proxy/storage.hpp @@ -107,7 +107,7 @@ class Storage { * @tparam T Type of the variables * @param variables Map of variables * - * @return Serialized buffer + * @return std::vector Serialized buffer */ template static std::vector serialize_var_map(const std::unordered_map& variables); @@ -119,7 +119,7 @@ class Storage { * @param buffer Serialized buffer * @param num_vars Number of variables * - * @return Deserialized map of variables + * @return std::unordered_map Deserialized map of variables */ template static std::unordered_map deserialize_var_map(std::vector& buffer, uint16_t num_vars); From a21e7a64f040f7964a320f7a9b9118bf9ed52bae Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 3 Apr 2024 02:07:21 -0300 Subject: [PATCH 124/561] =?UTF-8?q?=F0=9F=93=9D=20Add=20documentation=20fo?= =?UTF-8?q?r=20the=20rotary=20sensor=20registers=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/rotary_sensor_reg.hpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/inc/proxy/rotary_sensor_reg.hpp b/inc/proxy/rotary_sensor_reg.hpp index ada08dc..0426148 100644 --- a/inc/proxy/rotary_sensor_reg.hpp +++ b/inc/proxy/rotary_sensor_reg.hpp @@ -12,7 +12,13 @@ #include +/** + * @brief Registers class for the rotary sensor configuration + */ struct Registers { + /** + * @brief Registers union types definition + */ union Disable { struct Fields { uint8_t UVW_off : 1; @@ -94,6 +100,9 @@ struct Registers { uint8_t raw; }; + /** + * @brief Register addresses in the rotary sensor memory + */ static constexpr uint16_t disable_addr{0x0015}; static constexpr uint16_t zposm_addr{0x0016}; static constexpr uint16_t zposl_addr{0x0017}; @@ -102,6 +111,9 @@ struct Registers { static constexpr uint16_t settings3_addr{0x001A}; static constexpr uint16_t ecc_addr{0x001B}; + /** + * @brief Member variables to be configured in the rotary sensor + */ Disable disable; Zposm zposm; Zposl zposl; From 17e1bde25fd651584789e2915ea5ffe97a6ffc16 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 4 Apr 2024 13:17:09 -0300 Subject: [PATCH 125/561] =?UTF-8?q?=E2=9C=A8=20Add=20clang-format=20and=20?= =?UTF-8?q?remove=20uncrustify?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-format | 192 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 192 insertions(+) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..48b2c67 --- /dev/null +++ b/.clang-format @@ -0,0 +1,192 @@ +--- +Language: Cpp +# BasedOnStyle: LLVM +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveMacros: None +AlignConsecutiveAssignments: None +AlignConsecutiveBitFields: None +AlignConsecutiveDeclarations: None +AlignEscapedNewlines: Right +AlignOperands: Align +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortEnumsOnASingleLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: MultiLine +AttributeMacros: + - __capability +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeConceptDeclarations: true +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeColon +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +QualifierAlignment: Leave +CompactNamespaces: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DeriveLineEnding: true +DerivePointerAlignment: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +ExperimentalAutoDetectBinPacking: false +PackConstructorInitializers: BinPack +BasedOnStyle: '' +ConstructorInitializerAllOnOneLineOrOnePerLine: false +AllowAllConstructorInitializersOnNextLine: true +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + SortPriority: 0 + CaseSensitive: false + - Regex: '.*' + Priority: 1 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: '(Test)?$' +IncludeIsMainSourceRegex: '' +IndentAccessModifiers: false +IndentCaseLabels: false +IndentCaseBlocks: false +IndentGotoLabels: true +IndentPPDirectives: None +IndentExternBlock: AfterExternBlock +IndentRequires: false +IndentWidth: 2 +IndentWrappedFunctionNames: false +InsertTrailingCommas: None +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +LambdaBodyIndentation: Signature +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 60 +PenaltyIndentedWhitespace: 0 +PointerAlignment: Right +PPIndentWidth: -1 +ReferenceAlignment: Pointer +ReflowComments: true +RemoveBracesLLVM: false +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SortIncludes: CaseSensitive +SortJavaStaticImport: Before +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDefinitionName: false + AfterFunctionDeclarationName: false + AfterIfMacros: true + AfterOverloadedOperator: false + BeforeNonEmptyParentheses: false +SpaceAroundPointerQualifiers: Default +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: Never +SpacesInConditionalStatement: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParentheses: false +SpacesInSquareBrackets: false +SpaceBeforeSquareBrackets: false +BitFieldColonSpacing: Both +Standard: Latest +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseCRLF: false +UseTab: Never +WhitespaceSensitiveMacros: + - STRINGIZE + - PP_STRINGIZE + - BOOST_PP_STRINGIZE + - NS_SWIFT_NAME + - CF_SWIFT_NAME +... + From 09bb2206dac1abbab7327794cd7b3e59c831b746 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 4 Apr 2024 13:20:15 -0300 Subject: [PATCH 126/561] =?UTF-8?q?=F0=9F=94=A7=20Add=20custom=20configura?= =?UTF-8?q?tion=20to=20clang-format?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-format | 67 +++++++++++++++++++++++++-------------------------- 1 file changed, 33 insertions(+), 34 deletions(-) diff --git a/.clang-format b/.clang-format index 48b2c67..22283a9 100644 --- a/.clang-format +++ b/.clang-format @@ -2,28 +2,28 @@ Language: Cpp # BasedOnStyle: LLVM AccessModifierOffset: -2 -AlignAfterOpenBracket: Align +AlignAfterOpenBracket: BlockIndent AlignArrayOfStructures: None AlignConsecutiveMacros: None AlignConsecutiveAssignments: None -AlignConsecutiveBitFields: None +AlignConsecutiveBitFields: Consecutive AlignConsecutiveDeclarations: None -AlignEscapedNewlines: Right +AlignEscapedNewlines: Left AlignOperands: Align AlignTrailingComments: true AllowAllArgumentsOnNextLine: true AllowAllParametersOfDeclarationOnNextLine: true AllowShortEnumsOnASingleLine: true -AllowShortBlocksOnASingleLine: Never +AllowShortBlocksOnASingleLine: Empty AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: All +AllowShortFunctionsOnASingleLine: Inline AllowShortLambdasOnASingleLine: All AllowShortIfStatementsOnASingleLine: Never AllowShortLoopsOnASingleLine: false AlwaysBreakAfterDefinitionReturnType: None AlwaysBreakAfterReturnType: None AlwaysBreakBeforeMultilineStrings: false -AlwaysBreakTemplateDeclarations: MultiLine +AlwaysBreakTemplateDeclarations: Yes AttributeMacros: - __capability BinPackArguments: true @@ -51,13 +51,13 @@ BreakBeforeBinaryOperators: None BreakBeforeConceptDeclarations: true BreakBeforeBraces: Attach BreakBeforeInheritanceComma: false -BreakInheritanceList: BeforeColon -BreakBeforeTernaryOperators: true +BreakInheritanceList: AfterColon +BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: false -BreakConstructorInitializers: BeforeColon +BreakConstructorInitializers: AfterColon BreakAfterJavaFieldAnnotations: false BreakStringLiterals: true -ColumnLimit: 80 +ColumnLimit: 120 CommentPragmas: '^ IWYU pragma:' QualifierAlignment: Leave CompactNamespaces: false @@ -68,9 +68,9 @@ DeriveLineEnding: true DerivePointerAlignment: false DisableFormat: false EmptyLineAfterAccessModifier: Never -EmptyLineBeforeAccessModifier: LogicalBlock +EmptyLineBeforeAccessModifier: Always ExperimentalAutoDetectBinPacking: false -PackConstructorInitializers: BinPack +PackConstructorInitializers: NextLine BasedOnStyle: '' ConstructorInitializerAllOnOneLineOrOnePerLine: false AllowAllConstructorInitializersOnNextLine: true @@ -81,35 +81,35 @@ ForEachMacros: - BOOST_FOREACH IfMacros: - KJ_IF_MAYBE -IncludeBlocks: Preserve +IncludeBlocks: Regroup IncludeCategories: - - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + - Regex: ^<.*>' Priority: 2 - SortPriority: 0 + SortPriority: 2 CaseSensitive: false - - Regex: '^(<|"(gtest|gmock|isl|json)/)' - Priority: 3 - SortPriority: 0 + - Regex: '^".*"' + Priority: 1 + SortPriority: 1 CaseSensitive: false - Regex: '.*' - Priority: 1 + Priority: 0 SortPriority: 0 CaseSensitive: false IncludeIsMainRegex: '(Test)?$' IncludeIsMainSourceRegex: '' IndentAccessModifiers: false -IndentCaseLabels: false +IndentCaseLabels: true IndentCaseBlocks: false IndentGotoLabels: true IndentPPDirectives: None -IndentExternBlock: AfterExternBlock +IndentExternBlock: NoIndent IndentRequires: false -IndentWidth: 2 -IndentWrappedFunctionNames: false +IndentWidth: 4 +IndentWrappedFunctionNames: true InsertTrailingCommas: None JavaScriptQuotes: Leave JavaScriptWrapImports: true -KeepEmptyLinesAtTheStartOfBlocks: true +KeepEmptyLinesAtTheStartOfBlocks: false LambdaBodyIndentation: Signature MacroBlockBegin: '' MacroBlockEnd: '' @@ -130,19 +130,19 @@ PenaltyBreakTemplateDeclaration: 10 PenaltyExcessCharacter: 1000000 PenaltyReturnTypeOnItsOwnLine: 60 PenaltyIndentedWhitespace: 0 -PointerAlignment: Right +PointerAlignment: Left PPIndentWidth: -1 ReferenceAlignment: Pointer ReflowComments: true RemoveBracesLLVM: false -SeparateDefinitionBlocks: Leave -ShortNamespaceLines: 1 -SortIncludes: CaseSensitive +SeparateDefinitionBlocks: Always +ShortNamespaceLines: 0 +SortIncludes: CaseInsensitive SortJavaStaticImport: Before SortUsingDeclarations: true SpaceAfterCStyleCast: false SpaceAfterLogicalNot: false -SpaceAfterTemplateKeyword: true +SpaceAfterTemplateKeyword: false SpaceBeforeAssignmentOperators: true SpaceBeforeCaseColon: false SpaceBeforeCpp11BracedList: false @@ -159,9 +159,9 @@ SpaceBeforeParensOptions: BeforeNonEmptyParentheses: false SpaceAroundPointerQualifiers: Default SpaceBeforeRangeBasedForLoopColon: true -SpaceInEmptyBlock: false +SpaceInEmptyBlock: true SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 1 +SpacesBeforeTrailingComments: 2 SpacesInAngles: Never SpacesInConditionalStatement: false SpacesInContainerLiterals: true @@ -173,13 +173,13 @@ SpacesInParentheses: false SpacesInSquareBrackets: false SpaceBeforeSquareBrackets: false BitFieldColonSpacing: Both -Standard: Latest +Standard: c++20 StatementAttributeLikeMacros: - Q_EMIT StatementMacros: - Q_UNUSED - QT_REQUIRE_VERSION -TabWidth: 8 +TabWidth: 4 UseCRLF: false UseTab: Never WhitespaceSensitiveMacros: @@ -189,4 +189,3 @@ WhitespaceSensitiveMacros: - NS_SWIFT_NAME - CF_SWIFT_NAME ... - From fa4ef68031ae30446179ecc8e5b6f34dd0815b61 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 4 Apr 2024 14:22:09 -0300 Subject: [PATCH 127/561] =?UTF-8?q?=F0=9F=94=A7=20Update=20clang-format=20?= =?UTF-8?q?configuration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-format | 26 +++++++++++--------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/.clang-format b/.clang-format index 22283a9..ef22191 100644 --- a/.clang-format +++ b/.clang-format @@ -7,13 +7,13 @@ AlignArrayOfStructures: None AlignConsecutiveMacros: None AlignConsecutiveAssignments: None AlignConsecutiveBitFields: Consecutive -AlignConsecutiveDeclarations: None +AlignConsecutiveDeclarations: Consecutive AlignEscapedNewlines: Left AlignOperands: Align AlignTrailingComments: true AllowAllArgumentsOnNextLine: true AllowAllParametersOfDeclarationOnNextLine: true -AllowShortEnumsOnASingleLine: true +AllowShortEnumsOnASingleLine: false AllowShortBlocksOnASingleLine: Empty AllowShortCaseLabelsOnASingleLine: false AllowShortFunctionsOnASingleLine: Inline @@ -81,27 +81,23 @@ ForEachMacros: - BOOST_FOREACH IfMacros: - KJ_IF_MAYBE -IncludeBlocks: Regroup +IncludeBlocks: Preserve IncludeCategories: - - Regex: ^<.*>' - Priority: 2 - SortPriority: 2 - CaseSensitive: false - - Regex: '^".*"' + - Regex: '^<.*>' Priority: 1 SortPriority: 1 CaseSensitive: false - - Regex: '.*' - Priority: 0 - SortPriority: 0 + - Regex: '^".*"' + Priority: 2 + SortPriority: 2 CaseSensitive: false -IncludeIsMainRegex: '(Test)?$' +IncludeIsMainRegex: '$' IncludeIsMainSourceRegex: '' IndentAccessModifiers: false IndentCaseLabels: true IndentCaseBlocks: false IndentGotoLabels: true -IndentPPDirectives: None +IndentPPDirectives: BeforeHash IndentExternBlock: NoIndent IndentRequires: false IndentWidth: 4 @@ -137,12 +133,12 @@ ReflowComments: true RemoveBracesLLVM: false SeparateDefinitionBlocks: Always ShortNamespaceLines: 0 -SortIncludes: CaseInsensitive +SortIncludes: Never SortJavaStaticImport: Before SortUsingDeclarations: true SpaceAfterCStyleCast: false SpaceAfterLogicalNot: false -SpaceAfterTemplateKeyword: false +SpaceAfterTemplateKeyword: true SpaceBeforeAssignmentOperators: true SpaceBeforeCaseColon: false SpaceBeforeCpp11BracedList: false From 87cb9ae9ef57b61670590e97f6e5e4b71e89a08e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 4 Apr 2024 14:27:58 -0300 Subject: [PATCH 128/561] =?UTF-8?q?=F0=9F=8E=A8=20Apply=20new=20format=20t?= =?UTF-8?q?o=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/adc_dma.hpp | 82 ++++---- inc/hal/crc.hpp | 56 +++--- inc/hal/encoder.hpp | 54 ++--- inc/hal/flash.hpp | 124 ++++++------ inc/hal/gpio.hpp | 80 ++++---- inc/hal/mcu.hpp | 20 +- inc/hal/pwm.hpp | 74 +++---- inc/hal/pwm_dma.hpp | 86 ++++---- inc/hal/spi.hpp | 108 +++++----- inc/hal/timer.hpp | 178 ++++++++-------- inc/proxy/argb.hpp | 180 ++++++++--------- inc/proxy/battery.hpp | 80 ++++---- inc/proxy/button.hpp | 240 +++++++++++----------- inc/proxy/buzzer.hpp | 92 ++++----- inc/proxy/current_sensors.hpp | 84 ++++---- inc/proxy/dip_switch.hpp | 72 +++---- inc/proxy/distance_sensors.hpp | 106 +++++----- inc/proxy/fan.hpp | 120 +++++------ inc/proxy/imu.hpp | 290 +++++++++++++-------------- inc/proxy/led.hpp | 72 +++---- inc/proxy/locomotion.hpp | 172 ++++++++-------- inc/proxy/rotary_sensor.hpp | 152 +++++++------- inc/proxy/rotary_sensor_reg.hpp | 48 ++--- inc/proxy/serializable_interface.hpp | 66 +++--- inc/proxy/storage.hpp | 244 +++++++++++----------- inc/proxy/torque_sensors.hpp | 76 +++---- src/hal/crc.cpp | 3 +- src/hal/flash.cpp | 6 +- src/hal/gpio.cpp | 3 +- src/hal/timer.cpp | 6 +- src/proxy/argb.cpp | 7 +- src/proxy/button.cpp | 3 +- src/proxy/current_sensors.cpp | 5 +- src/proxy/dip_switch.cpp | 7 +- src/proxy/distance_sensors.cpp | 2 +- src/proxy/imu.cpp | 40 ++-- src/proxy/led.cpp | 3 +- src/proxy/locomotion.cpp | 6 +- src/proxy/rotary_sensor.cpp | 7 +- src/proxy/storage.cpp | 10 +- src/proxy/torque_sensors.cpp | 5 +- 41 files changed, 1533 insertions(+), 1536 deletions(-) diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index 3a409d1..6542a9d 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -17,53 +17,53 @@ namespace hal { * @brief Class to handle ADC peripheral on STM32 microcontrollers using DMA */ class AdcDma { - public: - /** - * @brief Configuration structure for ADC DMA - */ - struct Config { - ADC_HandleTypeDef* handle; - void (* init_function)(); - uint32_t max_reading; - float reference_voltage; - }; + public: + /** + * @brief Configuration structure for ADC DMA + */ + struct Config { + void (*init_function)(); + ADC_HandleTypeDef* handle; + uint32_t max_reading; + float reference_voltage; + }; - /** - * @brief Construct a new AdcDma object - * - * @param config ADC DMA configuration struct - */ - explicit AdcDma(const Config& config); + /** + * @brief Construct a new AdcDma object + * + * @param config ADC DMA configuration struct + */ + explicit AdcDma(const Config& config); - /** - * @brief Enable ADC, start conversion of regular group and transfer result through DMA - * - * @param buffer Destination Buffer address - * @param size Number of data to be transferred from ADC DMA peripheral to memory - */ - void start_dma(uint32_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) + /** + * @brief Enable ADC, start conversion of regular group and transfer result through DMA + * + * @param buffer Destination Buffer address + * @param size Number of data to be transferred from ADC DMA peripheral to memory + */ + void start_dma(uint32_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) - /** - * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode) - */ - void stop_dma(); + /** + * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode) + */ + void stop_dma(); - /** - * @brief Maximum ADC reading - */ - const uint32_t max_reading; // NOLINT(*-non-private-member-variables-in-classes) + /** + * @brief Maximum ADC reading + */ + const uint32_t max_reading; // NOLINT(*-non-private-member-variables-in-classes) - /** - * @brief Reference voltage for the ADC measurement - */ - const float reference_voltage; // NOLINT(*-non-private-member-variables-in-classes) + /** + * @brief Reference voltage for the ADC measurement + */ + const float reference_voltage; // NOLINT(*-non-private-member-variables-in-classes) - private: - /** - * @brief ADC handle - */ - ADC_HandleTypeDef* handle; + private: + /** + * @brief ADC handle + */ + ADC_HandleTypeDef* handle; }; } // namespace hal -#endif // MICRAS_HAL_ADC_DMA_HPP +#endif // MICRAS_HAL_ADC_DMA_HPP diff --git a/inc/hal/crc.hpp b/inc/hal/crc.hpp index 687da18..ede41fd 100644 --- a/inc/hal/crc.hpp +++ b/inc/hal/crc.hpp @@ -16,37 +16,37 @@ namespace hal { * @brief Class to handle the cyclic redundancy check peripheral on STM32 microcontrollers */ class Crc { - public: - /** - * @brief CRC configuration struct - */ - struct Config { - CRC_HandleTypeDef* handle; - }; + public: + /** + * @brief CRC configuration struct + */ + struct Config { + CRC_HandleTypeDef* handle; + }; - /** - * @brief Construct a new Crc object - * - * @param config Configuration for the CRC - */ - explicit Crc(const Config& config); + /** + * @brief Construct a new Crc object + * + * @param config Configuration for the CRC + */ + explicit Crc(const Config& config); - /** - * @brief Calculate the CRC value - * - * @param data Data to calculate the CRC - * @param size Size of the buffer - * - * @return uint32_t CRC value - */ - uint32_t calculate(uint32_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) + /** + * @brief Calculate the CRC value + * + * @param data Data to calculate the CRC + * @param size Size of the buffer + * + * @return uint32_t CRC value + */ + uint32_t calculate(uint32_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) - private: - /** - * @brief CRC handle - */ - CRC_HandleTypeDef* handle; + private: + /** + * @brief CRC handle + */ + CRC_HandleTypeDef* handle; }; } // namespace hal -#endif // MICRAS_HAL_CRC_HPP +#endif // MICRAS_HAL_CRC_HPP diff --git a/inc/hal/encoder.hpp b/inc/hal/encoder.hpp index 397e715..2f14350 100644 --- a/inc/hal/encoder.hpp +++ b/inc/hal/encoder.hpp @@ -16,36 +16,36 @@ namespace hal { * @brief Class to handle encoder peripheral on STM32 microcontrollers */ class Encoder { - public: - /** - * @brief Encoder configuration struct - */ - struct Config { - TIM_HandleTypeDef* handle; - void (* init_function)(); - uint32_t timer_channel; - }; + public: + /** + * @brief Encoder configuration struct + */ + struct Config { + void (*init_function)(); + TIM_HandleTypeDef* handle; + uint32_t timer_channel; + }; - /** - * @brief Construct a new Encoder object - * - * @param config Configuration for the encoder - */ - explicit Encoder(const Config& config); + /** + * @brief Construct a new Encoder object + * + * @param config Configuration for the encoder + */ + explicit Encoder(const Config& config); - /** - * @brief Get the counter value - * - * @return int32_t Current value of the counter - */ - int32_t get_counter() const; + /** + * @brief Get the counter value + * + * @return int32_t Current value of the counter + */ + int32_t get_counter() const; - private: - /** - * @brief Timer handle - */ - TIM_HandleTypeDef* handle; + private: + /** + * @brief Timer handle + */ + TIM_HandleTypeDef* handle; }; } // namespace hal -#endif // MICRAS_HAL_ENCODER_HPP +#endif // MICRAS_HAL_ENCODER_HPP diff --git a/inc/hal/flash.hpp b/inc/hal/flash.hpp index 3641402..78ce641 100644 --- a/inc/hal/flash.hpp +++ b/inc/hal/flash.hpp @@ -17,74 +17,78 @@ namespace hal { * @brief Class to handle flash memory on STM32 microcontrollers */ class Flash { - public: - /** - * @brief Deleted constructor for static class - */ - Flash() = delete; + public: + /** + * @brief Deleted constructor for static class + */ + Flash() = delete; - /** - * @brief Read data from flash memory - * - * @param address address to read from (indexed by double words) - * @param data pointer to store the data read - * @param size size in double words of the data to read - */ - static void read(uint32_t address, uint64_t data[], uint32_t size = 1); // NOLINT(*-avoid-c-arrays) + // NOLINTBEGIN(*-avoid-c-arrays) - /** - * @brief Read data from flash memory - * - * @param page page to read from (counting from the last to the first) - * @param page_address address inside the page to read from (indexed by double words) - * @param data pointer to store the data read - * @param size size in double words of the data to read - */ - static void read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); // NOLINT(*-avoid-c-arrays) + /** + * @brief Read data from flash memory + * + * @param address address to read from (indexed by double words) + * @param data pointer to store the data read + * @param size size in double words of the data to read + */ + static void read(uint32_t address, uint64_t data[], uint32_t size = 1); - /** - * @brief Write data to flash memory - * - * @param address address to write to (indexed by double words) - * @param data pointer to the data to write - * @param size size in double words of the data to write - */ - static void write(uint32_t address, const uint64_t data[], uint32_t size = 1); // NOLINT(*-avoid-c-arrays) + /** + * @brief Read data from flash memory + * + * @param page page to read from (counting from the last to the first) + * @param page_address address inside the page to read from (indexed by double words) + * @param data pointer to store the data read + * @param size size in double words of the data to read + */ + static void read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); - /** - * @brief Write data to flash memory - * - * @param page page to write to (counting from the last to the first) - * @param page_address address inside the page to write to (indexed by double words) - * @param data pointer to the data to write - * @param size size in double words of the data to write - */ - static void write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size = 1); // NOLINT(*-avoid-c-arrays) + /** + * @brief Write data to flash memory + * + * @param address address to write to (indexed by double words) + * @param data pointer to the data to write + * @param size size in double words of the data to write + */ + static void write(uint32_t address, const uint64_t data[], uint32_t size = 1); - /** - * @brief Erase flash memory pages - * - * @param page first page to erase (counting from the last to the first) - * @param number_of_pages number of pages to erase - */ - static void erase_pages(uint16_t page, uint16_t number_of_pages = 1); + /** + * @brief Write data to flash memory + * + * @param page page to write to (counting from the last to the first) + * @param page_address address inside the page to write to (indexed by double words) + * @param data pointer to the data to write + * @param size size in double words of the data to write + */ + static void write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size = 1); - private: - /** - * @brief Number of double words per row - */ - static constexpr uint32_t double_words_per_row{32}; + // NOLINTEND(*-avoid-c-arrays) - /** - * @brief Number of bytes per row - */ - static constexpr uint32_t bytes_per_row{8 * double_words_per_row}; + /** + * @brief Erase flash memory pages + * + * @param page first page to erase (counting from the last to the first) + * @param number_of_pages number of pages to erase + */ + static void erase_pages(uint16_t page, uint16_t number_of_pages = 1); - /** - * @brief Number of double words per page - */ - static constexpr uint32_t double_words_per_page{FLASH_PAGE_SIZE / 8}; + private: + /** + * @brief Number of double words per row + */ + static constexpr uint32_t double_words_per_row{32}; + + /** + * @brief Number of bytes per row + */ + static constexpr uint32_t bytes_per_row{8 * double_words_per_row}; + + /** + * @brief Number of double words per page + */ + static constexpr uint32_t double_words_per_page{FLASH_PAGE_SIZE / 8}; }; } // namespace hal -#endif // MICRAS_HAL_FLASH_HPP +#endif // MICRAS_HAL_FLASH_HPP diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index eb5baaf..428787d 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -17,52 +17,52 @@ namespace hal { * @brief Class for controlling GPIO pins on STM32 microcontrollers */ class Gpio { - public: - /** - * @brief Configuration structure for GPIO pin - */ - struct Config { - GPIO_TypeDef* port; - uint16_t pin; - }; + public: + /** + * @brief Configuration structure for GPIO pin + */ + struct Config { + GPIO_TypeDef* port; + uint16_t pin; + }; - /** - * @brief Constructor for the Gpio class - * - * @param config Configuration for the GPIO pin - */ - explicit Gpio(const Config& config); + /** + * @brief Constructor for the Gpio class + * + * @param config Configuration for the GPIO pin + */ + explicit Gpio(const Config& config); - /** - * @brief Read the current state of the GPIO pin - * - * @return bool The current state of the GPIO pin (true for high, false for low) - */ - bool read() const; + /** + * @brief Read the current state of the GPIO pin + * + * @return bool The current state of the GPIO pin (true for high, false for low) + */ + bool read() const; - /** - * @brief Write a new state to the GPIO pin - * - * @param pin_state The state to be written (true for high, false for low) - */ - void write(bool state); + /** + * @brief Write a new state to the GPIO pin + * + * @param pin_state The state to be written (true for high, false for low) + */ + void write(bool state); - /** - * @brief Toggle the state of the GPIO pin - */ - void toggle(); + /** + * @brief Toggle the state of the GPIO pin + */ + void toggle(); - private: - /** - * @brief The port of the GPIO - */ - GPIO_TypeDef* port; + private: + /** + * @brief The port of the GPIO + */ + GPIO_TypeDef* port; - /** - * @brief The pin number of the GPIO - */ - uint16_t pin; + /** + * @brief The pin number of the GPIO + */ + uint16_t pin; }; } // namespace hal -#endif // MICRAS_HAL_GPIO_HPP +#endif // MICRAS_HAL_GPIO_HPP diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index d528cef..701bb4e 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -16,17 +16,17 @@ namespace hal { * @brief Microcontroller unit class */ class Mcu { - public: - /** - * @brief Deleted constructor for static class - */ - Mcu() = delete; + public: + /** + * @brief Deleted constructor for static class + */ + Mcu() = delete; - /** - * @brief Initializes MCU and some peripherals - */ - static void init(); + /** + * @brief Initializes MCU and some peripherals + */ + static void init(); }; } // namespace hal -#endif // MICRAS_HAL_MCU_HPP +#endif // MICRAS_HAL_MCU_HPP diff --git a/inc/hal/pwm.hpp b/inc/hal/pwm.hpp index efbdbd5..977305a 100644 --- a/inc/hal/pwm.hpp +++ b/inc/hal/pwm.hpp @@ -17,48 +17,48 @@ namespace hal { * @brief Class to handle PWM peripheral on STM32 microcontrollers */ class Pwm { - public: - /** - * @brief PWM configuration struct - */ - struct Config { - TIM_HandleTypeDef* handle; - void (* init_function)(); - uint32_t timer_channel; - }; + public: + /** + * @brief PWM configuration struct + */ + struct Config { + void (*init_function)(); + TIM_HandleTypeDef* handle; + uint32_t timer_channel; + }; - /** - * @brief Construct a new Pwm object - * - * @param config Configuration for the PWM - */ - explicit Pwm(const Config& config); + /** + * @brief Construct a new Pwm object + * + * @param config Configuration for the PWM + */ + explicit Pwm(const Config& config); - /** - * @brief Set the PWM duty cycle - * - * @param duty_cycle Duty cycle value - */ - void set_duty_cycle(float duty_cycle); + /** + * @brief Set the PWM duty cycle + * + * @param duty_cycle Duty cycle value + */ + void set_duty_cycle(float duty_cycle); - /** - * @brief Set the PWM frequency - * - * @param frequency Frequency value in Hz - */ - void set_frequency(uint32_t frequency); + /** + * @brief Set the PWM frequency + * + * @param frequency Frequency value in Hz + */ + void set_frequency(uint32_t frequency); - private: - /** - * @brief Timer handle - */ - TIM_HandleTypeDef* handle; + private: + /** + * @brief Timer handle + */ + TIM_HandleTypeDef* handle; - /** - * @brief Channel number of the timer - */ - uint32_t channel; + /** + * @brief Channel number of the timer + */ + uint32_t channel; }; } // namespace hal -#endif // MICRAS_HAL_PWM_HPP +#endif // MICRAS_HAL_PWM_HPP diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp index 05904cd..1785a44 100644 --- a/inc/hal/pwm_dma.hpp +++ b/inc/hal/pwm_dma.hpp @@ -17,55 +17,55 @@ namespace hal { * @brief Class to handle PWM peripheral on STM32 microcontrollers using DMA */ class PwmDma { - public: - /** - * @brief PWM configuration struct - */ - struct Config { - TIM_HandleTypeDef* handle; - void (* init_function)(); - uint32_t timer_channel; - }; + public: + /** + * @brief PWM configuration struct + */ + struct Config { + void (*init_function)(); + TIM_HandleTypeDef* handle; + uint32_t timer_channel; + }; - /** - * @brief Construct a new PwmDma object - * - * @param config Configuration for the PWM - */ - explicit PwmDma(const Config& config); + /** + * @brief Construct a new PwmDma object + * + * @param config Configuration for the PWM + */ + explicit PwmDma(const Config& config); - /** - * @brief Start PWM and DMA transfer - * - * @param buffer Buffer to transfer - * @param size Size of the buffer - */ - void start_dma(uint32_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) + /** + * @brief Start PWM and DMA transfer + * + * @param buffer Buffer to transfer + * @param size Size of the buffer + */ + void start_dma(uint32_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) - /** - * @brief Stop PWM and DMA transfer - */ - void stop_dma(); + /** + * @brief Stop PWM and DMA transfer + */ + void stop_dma(); - /** - * @brief Get the compare value for ad duty cycle - * - * @param duty_cycle Duty cycle to get the compare value for - * @return uint32_t Compare value for the duty cycle - */ - uint32_t get_compare(float duty_cycle) const; + /** + * @brief Get the compare value for ad duty cycle + * + * @param duty_cycle Duty cycle to get the compare value for + * @return uint32_t Compare value for the duty cycle + */ + uint32_t get_compare(float duty_cycle) const; - private: - /** - * @brief Timer handle - */ - TIM_HandleTypeDef* handle; + private: + /** + * @brief Timer handle + */ + TIM_HandleTypeDef* handle; - /** - * @brief Channel number of the timer - */ - uint32_t channel; + /** + * @brief Channel number of the timer + */ + uint32_t channel; }; } // namespace hal -#endif // MICRAS_HAL_PWM_DMA_HPP +#endif // MICRAS_HAL_PWM_DMA_HPP diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index 43fa004..1f4d1bc 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -19,68 +19,68 @@ namespace hal { * @brief Class to handle SPI peripheral on STM32 microcontrollers */ class Spi { - public: - /** - * @brief SPI configuration struct - */ - struct Config { - SPI_HandleTypeDef* handle; - void (* init_function)(); - hal::Gpio::Config gpio; - uint32_t timeout; - }; + public: + /** + * @brief SPI configuration struct + */ + struct Config { + void (*init_function)(); + SPI_HandleTypeDef* handle; + hal::Gpio::Config gpio; + uint32_t timeout; + }; - /** - * @brief Construct a new Spi object - * - * @param config Configuration for the SPI - */ - explicit Spi(const Config& config); + /** + * @brief Construct a new Spi object + * + * @param config Configuration for the SPI + */ + explicit Spi(const Config& config); - /** - * @brief Activate the chip select - * - * @return bool True if the device was successfully selected, false otherwise - */ - bool select_device(); + /** + * @brief Activate the chip select + * + * @return bool True if the device was successfully selected, false otherwise + */ + bool select_device(); - /** - * @brief Deactivate the chip select - */ - void unselect_device(); + /** + * @brief Deactivate the chip select + */ + void unselect_device(); - /** - * @brief Transmit data over SPI - * - * @param data Data to transmit - * @param size Size of the buffer - */ - void transmit(uint8_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) + /** + * @brief Transmit data over SPI + * + * @param data Data to transmit + * @param size Size of the buffer + */ + void transmit(uint8_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) - /** - * @brief Receive data over SPI - * - * @param data Data to receive data - * @param size Size of the data - */ - void receive(uint8_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) + /** + * @brief Receive data over SPI + * + * @param data Data to receive data + * @param size Size of the data + */ + void receive(uint8_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) - private: - /** - * @brief Handle for the SPI - */ - SPI_HandleTypeDef* handle; + private: + /** + * @brief Handle for the SPI + */ + SPI_HandleTypeDef* handle; - /** - * @brief GPIO for the chip select pin - */ - hal::Gpio gpio; + /** + * @brief GPIO for the chip select pin + */ + hal::Gpio gpio; - /** - * @brief Timeout for the SPI operations in ms - */ - uint32_t timeout; + /** + * @brief Timeout for the SPI operations in ms + */ + uint32_t timeout; }; } // namespace hal -#endif // MICRAS_HAL_SPI_HPP +#endif // MICRAS_HAL_SPI_HPP diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index c963438..4439010 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -17,95 +17,95 @@ namespace hal { * @brief Class to handle timer peripheral on STM32 microcontrollers */ class Timer { - public: - /** - * @brief Timer configuration struct - */ - struct Config { - TIM_HandleTypeDef* handle; - void (* init_function)(); - }; - - /** - * @brief Construct a new Timer object - */ - Timer() = default; - - /** - * @brief Construct a new Timer object - * - * @param config Configuration for the timer - */ - explicit Timer(const Config& config); - - /** - * @brief Reset the timer counter in milliseconds - */ - void reset_ms(); - - /** - * @brief Reset the timer counter in microseconds - */ - void reset_us(); - - /** - * @brief Get the time elapsed since the last reset - * - * @return uint32_t Time elapsed in miliseconds - */ - uint32_t elapsed_time_ms() const; - - /** - * @brief Get the time elapsed since the last reset - * - * @return uint32_t Time elapsed in microseconds - */ - uint32_t elapsed_time_us() const; - - /** - * @brief Sleep for a given amount of time - * - * @param time Time to sleep in milliseconds - */ - static void sleep_ms(uint32_t time); - - /** - * @brief Sleep for a given amount of time - * - * @param time Time to sleep in microseconds - */ - void sleep_us(uint32_t time) const; - - private: - /** - * @brief Get the current timer counter - * - * @return uint32_t Current timer counter in milliseconds - */ - static uint32_t get_counter_ms(); - - /** - * @brief Get the current timer counter - * - * @return uint32_t Current timer counter in microseconds - */ - uint32_t get_counter_us() const; - - /** - * @brief Timer handle - */ - TIM_HandleTypeDef* handle{ }; - - /** - * @brief Timer counter - */ - uint32_t counter{ }; - - /** - * @brief Flag to enable microseconds - */ - bool enable_microseconds{false}; + public: + /** + * @brief Timer configuration struct + */ + struct Config { + void (*init_function)(); + TIM_HandleTypeDef* handle; + }; + + /** + * @brief Construct a new Timer object + */ + Timer() = default; + + /** + * @brief Construct a new Timer object + * + * @param config Configuration for the timer + */ + explicit Timer(const Config& config); + + /** + * @brief Reset the timer counter in milliseconds + */ + void reset_ms(); + + /** + * @brief Reset the timer counter in microseconds + */ + void reset_us(); + + /** + * @brief Get the time elapsed since the last reset + * + * @return uint32_t Time elapsed in miliseconds + */ + uint32_t elapsed_time_ms() const; + + /** + * @brief Get the time elapsed since the last reset + * + * @return uint32_t Time elapsed in microseconds + */ + uint32_t elapsed_time_us() const; + + /** + * @brief Sleep for a given amount of time + * + * @param time Time to sleep in milliseconds + */ + static void sleep_ms(uint32_t time); + + /** + * @brief Sleep for a given amount of time + * + * @param time Time to sleep in microseconds + */ + void sleep_us(uint32_t time) const; + + private: + /** + * @brief Get the current timer counter + * + * @return uint32_t Current timer counter in milliseconds + */ + static uint32_t get_counter_ms(); + + /** + * @brief Get the current timer counter + * + * @return uint32_t Current timer counter in microseconds + */ + uint32_t get_counter_us() const; + + /** + * @brief Timer handle + */ + TIM_HandleTypeDef* handle{}; + + /** + * @brief Timer counter + */ + uint32_t counter{}; + + /** + * @brief Flag to enable microseconds + */ + bool enable_microseconds{false}; }; } // namespace hal -#endif // MICRAS_HAL_TIMER_HPP +#endif // MICRAS_HAL_TIMER_HPP diff --git a/inc/proxy/argb.hpp b/inc/proxy/argb.hpp index a1db000..9529d2f 100644 --- a/inc/proxy/argb.hpp +++ b/inc/proxy/argb.hpp @@ -20,98 +20,98 @@ namespace proxy { */ template class Argb { - public: - /** - * @brief Configuration structure for the addressable RGB LED - */ - struct Config { - hal::PwmDma::Config pwm; - }; - - /** - * @brief Structure for storing color information - */ - struct Color { - uint8_t red; - uint8_t green; - uint8_t blue; - }; - - /** - * @brief Constructor for the Argb class - * - * @param config Configuration for the addressable RGB LED - */ - explicit Argb(const Config& config); - - /** - * @brief Set the color of the ARGB at the specified index - * - * @param index The index of the ARGB to set the color of - * @param color The color to set the ARGB to - */ - void set_color(const Color& color, uint8_t index); - - /** - * @brief Set the color of all ARGBs - * - * @param color The color to set all ARGBs to - */ - void set_color(const Color& color); - - /** - * @brief Turn off the ARGB at the specified index - * - * @param index The index of the ARGB to turn off - */ - void turn_off(uint8_t index); - - /** - * @brief Turn off all ARGBs - */ - void turn_off(); - - private: - /** - * @brief Encode a color into the data buffer - * - * @param color The color to encode - * @param index The index to encode the color at - */ - void encode_color(const Color& color, uint8_t index); - - /** - * @brief Protocol constants for the addressable RGB LED communication - */ - static constexpr uint8_t bits_per_color{8}; - static constexpr uint8_t colors_per_led{3}; - static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; - static constexpr float low_duty_cycle{0.32F}; // NOLINT(bugprone-dynamic-static-initializers) - static constexpr float high_duty_cycle{0.64F}; // NOLINT(bugprone-dynamic-static-initializers) - static constexpr uint8_t reset_length{40}; - - /** - * @brief PWM control object - */ - hal::PwmDma pwm; - - /** - * @brief PWM autoreload value for the low signal - */ - const uint32_t low_bit; - - /** - * @brief PWM autoreload value for the high signal - */ - const uint32_t high_bit; - - /** - * @brief Data buffer to send to the addressable RGB LED - */ - std::array buffer; + public: + /** + * @brief Configuration structure for the addressable RGB LED + */ + struct Config { + hal::PwmDma::Config pwm; + }; + + /** + * @brief Structure for storing color information + */ + struct Color { + uint8_t red; + uint8_t green; + uint8_t blue; + }; + + /** + * @brief Constructor for the Argb class + * + * @param config Configuration for the addressable RGB LED + */ + explicit Argb(const Config& config); + + /** + * @brief Set the color of the ARGB at the specified index + * + * @param index The index of the ARGB to set the color of + * @param color The color to set the ARGB to + */ + void set_color(const Color& color, uint8_t index); + + /** + * @brief Set the color of all ARGBs + * + * @param color The color to set all ARGBs to + */ + void set_color(const Color& color); + + /** + * @brief Turn off the ARGB at the specified index + * + * @param index The index of the ARGB to turn off + */ + void turn_off(uint8_t index); + + /** + * @brief Turn off all ARGBs + */ + void turn_off(); + + private: + /** + * @brief Encode a color into the data buffer + * + * @param color The color to encode + * @param index The index to encode the color at + */ + void encode_color(const Color& color, uint8_t index); + + /** + * @brief Protocol constants for the addressable RGB LED communication + */ + static constexpr uint8_t bits_per_color{8}; + static constexpr uint8_t colors_per_led{3}; + static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; + static constexpr float low_duty_cycle{0.32F}; // NOLINT(bugprone-dynamic-static-initializers) + static constexpr float high_duty_cycle{0.64F}; // NOLINT(bugprone-dynamic-static-initializers) + static constexpr uint8_t reset_length{40}; + + /** + * @brief PWM control object + */ + hal::PwmDma pwm; + + /** + * @brief PWM autoreload value for the low signal + */ + const uint32_t low_bit; + + /** + * @brief PWM autoreload value for the high signal + */ + const uint32_t high_bit; + + /** + * @brief Data buffer to send to the addressable RGB LED + */ + std::array buffer; }; } // namespace proxy #include "../src/proxy/argb.cpp" // NOLINT(bugprone-suspicious-include) -#endif // MICRAS_PROXY_ARGB_HPP +#endif // MICRAS_PROXY_ARGB_HPP diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp index 97851a8..e8ec70c 100644 --- a/inc/proxy/battery.hpp +++ b/inc/proxy/battery.hpp @@ -18,52 +18,52 @@ namespace proxy { * @brief Class for getting the battery voltage */ class Battery { - public: - /** - * @brief Configuration structure for the battery - */ - struct Config { - hal::AdcDma::Config adc; - float voltage_divider; - }; + public: + /** + * @brief Configuration structure for the battery + */ + struct Config { + hal::AdcDma::Config adc; + float voltage_divider; + }; - /** - * @brief Constructor for the Battery class - * - * @param config Configuration for the battery - */ - explicit Battery(const Config& config); + /** + * @brief Constructor for the Battery class + * + * @param config Configuration for the battery + */ + explicit Battery(const Config& config); - /** - * @brief Get the battery voltage - * - * @return float Battery voltage in volts - */ - float get_voltage() const; + /** + * @brief Get the battery voltage + * + * @return float Battery voltage in volts + */ + float get_voltage() const; - /** - * @brief Get the raw reading from the battery - * - * @return uint32_t Raw reading from the battery - */ - uint32_t get_voltage_raw() const; + /** + * @brief Get the raw reading from the battery + * + * @return uint32_t Raw reading from the battery + */ + uint32_t get_voltage_raw() const; - private: - /** - * @brief Adc object - */ - hal::AdcDma adc; + private: + /** + * @brief Adc object + */ + hal::AdcDma adc; - /** - * @brief Raw reading from the battery - */ - uint32_t raw_reading{ }; + /** + * @brief Raw reading from the battery + */ + uint32_t raw_reading{}; - /** - * @brief Voltage divider ratio - */ - const float voltage_divider; + /** + * @brief Voltage divider ratio + */ + const float voltage_divider; }; } // namespace proxy -#endif // MICRAS_PROXY_BATTERY_HPP +#endif // MICRAS_PROXY_BATTERY_HPP diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index ce9ed1c..5fcd73d 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -19,126 +19,126 @@ namespace proxy { * @brief Class for controlling a button */ class Button { - public: - /** - * @brief Enum for button status - */ - enum Status { - NO_PRESS, - SHORT_PRESS, - LONG_PRESS, - EXTRA_LONG_PRESS - }; - - /** - * @brief Enum for button pull resistor - */ - enum PullResistor { - PULL_UP, - PULL_DOWN, - }; - - /** - * @brief Configuration structure for button - */ - struct Config { - hal::Gpio::Config gpio{ }; - PullResistor pull_resistor{ }; - uint16_t debounce_delay = 10; - uint16_t long_press_delay = 1000; - uint16_t extra_long_press_delay = 5000; - }; - - /** - * @brief Constructor for Button class - * - * @param config Button configuration - */ - explicit Button(const Config& config); - - /** - * @brief Check if button is pressed - * - * @return bool True if button is pressed, false otherwise - */ - bool is_pressed(); - - /** - * @brief Get button status - * - * @return Status Button status - */ - Status get_status(); - - private: - /** - * @brief Get raw button reading - * - * @return bool Button reading without debounce - */ - bool get_raw_reading() const; - - /** - * @brief Update button state - */ - void update_state(); - - /** - * @brief Check if button was just pressed - * - * @return True if button was just pressed, false otherwise - */ - bool is_rising_edge() const; - - /** - * @brief Check if button was just released - * - * @return True if button was just released, false otherwise - */ - bool is_falling_edge() const; - - /** - * @brief Button pressing delays in ms - */ - const uint16_t debounce_delay; - const uint16_t long_press_delay; - const uint16_t extra_long_press_delay; - - /** - * @brief Gpio object for button - */ - const hal::Gpio gpio; - - /** - * @brief Pull resistor configuration - */ - PullResistor pull_resistor; - - /** - * @brief Timer to check if button is debouncing - */ - hal::Timer debounce_timer; - - /** - * @brief Timer to determine type of button press - */ - hal::Timer status_timer; - - /** - * @brief Flag to know when button is debouncing - */ - bool is_debouncing = false; - - /** - * @brief Flag to know if button was being pressed - */ - bool previous_state = false; - - /** - * @brief Flag to know if button is being pressed - */ - bool current_state = false; + public: + /** + * @brief Enum for button status + */ + enum Status { + NO_PRESS, + SHORT_PRESS, + LONG_PRESS, + EXTRA_LONG_PRESS + }; + + /** + * @brief Enum for button pull resistor + */ + enum PullResistor { + PULL_UP, + PULL_DOWN, + }; + + /** + * @brief Configuration structure for button + */ + struct Config { + hal::Gpio::Config gpio{}; + PullResistor pull_resistor{}; + uint16_t debounce_delay = 10; + uint16_t long_press_delay = 1000; + uint16_t extra_long_press_delay = 5000; + }; + + /** + * @brief Constructor for Button class + * + * @param config Button configuration + */ + explicit Button(const Config& config); + + /** + * @brief Check if button is pressed + * + * @return bool True if button is pressed, false otherwise + */ + bool is_pressed(); + + /** + * @brief Get button status + * + * @return Status Button status + */ + Status get_status(); + + private: + /** + * @brief Get raw button reading + * + * @return bool Button reading without debounce + */ + bool get_raw_reading() const; + + /** + * @brief Update button state + */ + void update_state(); + + /** + * @brief Check if button was just pressed + * + * @return True if button was just pressed, false otherwise + */ + bool is_rising_edge() const; + + /** + * @brief Check if button was just released + * + * @return True if button was just released, false otherwise + */ + bool is_falling_edge() const; + + /** + * @brief Button pressing delays in ms + */ + const uint16_t debounce_delay; + const uint16_t long_press_delay; + const uint16_t extra_long_press_delay; + + /** + * @brief Gpio object for button + */ + const hal::Gpio gpio; + + /** + * @brief Pull resistor configuration + */ + PullResistor pull_resistor; + + /** + * @brief Timer to check if button is debouncing + */ + hal::Timer debounce_timer; + + /** + * @brief Timer to determine type of button press + */ + hal::Timer status_timer; + + /** + * @brief Flag to know when button is debouncing + */ + bool is_debouncing = false; + + /** + * @brief Flag to know if button was being pressed + */ + bool previous_state = false; + + /** + * @brief Flag to know if button is being pressed + */ + bool current_state = false; }; } // namespace proxy -#endif // MICRAS_PROXY_BUTTON_HPP +#endif // MICRAS_PROXY_BUTTON_HPP diff --git a/inc/proxy/buzzer.hpp b/inc/proxy/buzzer.hpp index 9585441..0f5a6a6 100644 --- a/inc/proxy/buzzer.hpp +++ b/inc/proxy/buzzer.hpp @@ -19,60 +19,60 @@ namespace proxy { * @brief Class for controlling a buzzer */ class Buzzer { - public: - /** - * @brief Configuration structure for the buzzer - */ - struct Config { - hal::Pwm::Config pwm; - }; + public: + /** + * @brief Configuration structure for the buzzer + */ + struct Config { + hal::Pwm::Config pwm; + }; - /** - * @brief Constructor for the Buzzer class - * - * @param config Configuration for the buzzer - */ - explicit Buzzer(const Config& config); + /** + * @brief Constructor for the Buzzer class + * + * @param config Configuration for the buzzer + */ + explicit Buzzer(const Config& config); - /** - * @brief Play a tone for a duration - * - * @param frequency Buzzer sound frequency in Hz - * @param duration Duration of the sound in ms - */ - void play(uint32_t frequency, uint32_t duration = 0); + /** + * @brief Play a tone for a duration + * + * @param frequency Buzzer sound frequency in Hz + * @param duration Duration of the sound in ms + */ + void play(uint32_t frequency, uint32_t duration = 0); - /** - * @brief Update the buzzer state - */ - void update(); + /** + * @brief Update the buzzer state + */ + void update(); - /** - * @brief Stop the buzzer sound - */ - void stop(); + /** + * @brief Stop the buzzer sound + */ + void stop(); - private: - /** - * @brief PWM object - */ - hal::Pwm pwm; + private: + /** + * @brief PWM object + */ + hal::Pwm pwm; - /** - * @brief Timer to play the sound - */ - hal::Timer timer; + /** + * @brief Timer to play the sound + */ + hal::Timer timer; - /** - * @brief Flag to check if the buzzer is playing - */ - bool is_playing{false}; + /** + * @brief Flag to check if the buzzer is playing + */ + bool is_playing{false}; - /** - * @brief Duration of the sound - */ - uint32_t duration{0}; + /** + * @brief Duration of the sound + */ + uint32_t duration{0}; }; } // namespace proxy -#endif // MICRAS_PROXY_BUZZER_HPP +#endif // MICRAS_PROXY_BUZZER_HPP diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index 7763346..dd8061c 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -20,56 +20,56 @@ namespace proxy { */ template class CurrentSensors { - public: - /** - * @brief Configuration structure for current sensors - */ - struct Config { - hal::AdcDma::Config adc; - float shunt_resistor; - }; + public: + /** + * @brief Configuration structure for current sensors + */ + struct Config { + hal::AdcDma::Config adc; + float shunt_resistor; + }; - /** - * @brief Constructor for the CurrentSensors class - * - * @param config Configuration for the current sensors - */ - explicit CurrentSensors(const Config& config); + /** + * @brief Constructor for the CurrentSensors class + * + * @param config Configuration for the current sensors + */ + explicit CurrentSensors(const Config& config); - /** - * @brief Get the current from the sensor - * - * @param sensor_index Index of the sensor - * @return float Current reading from the sensor in amps - */ - float get_current(uint8_t sensor_index) const; + /** + * @brief Get the current from the sensor + * + * @param sensor_index Index of the sensor + * @return float Current reading from the sensor in amps + */ + float get_current(uint8_t sensor_index) const; - /** - * @brief Get the raw reading from the current sensor - * - * @param sensor_index Index of the sensor - * @return uint16_t Current reading from the sensor - */ - uint32_t get_current_raw(uint8_t sensor_index) const; + /** + * @brief Get the raw reading from the current sensor + * + * @param sensor_index Index of the sensor + * @return uint16_t Current reading from the sensor + */ + uint32_t get_current_raw(uint8_t sensor_index) const; - private: - /** - * @brief ADC DMA handle - */ - hal::AdcDma adc; + private: + /** + * @brief ADC DMA handle + */ + hal::AdcDma adc; - /** - * @brief Buffer to store the ADC values - */ - std::array buffer; + /** + * @brief Buffer to store the ADC values + */ + std::array buffer; - /** - * @brief Value of the shunt resistor in ohms - */ - const float shunt_resistor; + /** + * @brief Value of the shunt resistor in ohms + */ + const float shunt_resistor; }; } // namespace proxy #include "../src/proxy/current_sensors.cpp" // NOLINT(bugprone-suspicious-include) -#endif // MICRAS_PROXY_CURRENT_SENSORS_HPP +#endif // MICRAS_PROXY_CURRENT_SENSORS_HPP diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp index a9d534c..3768cb7 100644 --- a/inc/proxy/dip_switch.hpp +++ b/inc/proxy/dip_switch.hpp @@ -20,44 +20,44 @@ namespace proxy { */ template class DipSwitch { - public: - /** - * @brief Configuration struct for DipSwitch - */ - struct Config { - std::array gpio_array; - }; - - /** - * @brief Construct a new Dip Switch object - * - * @param config Configuration struct for DipSwitch - */ - explicit DipSwitch(const Config& config); - - /** - * @brief Get the state of a switch - * - * @param switch_index Index of the switch - * @return bool True if the switch is on, false otherwise - */ - bool get_switch_state(uint8_t switch_index) const; - - /** - * @brief Get the value of all switches - * - * @return uint8_t Value of all switches - */ - uint8_t get_switches_value() const; - - private: - /** - * @brief Array of GPIOs for the switches - */ - std::array gpio_array; + public: + /** + * @brief Configuration struct for DipSwitch + */ + struct Config { + std::array gpio_array; + }; + + /** + * @brief Construct a new Dip Switch object + * + * @param config Configuration struct for DipSwitch + */ + explicit DipSwitch(const Config& config); + + /** + * @brief Get the state of a switch + * + * @param switch_index Index of the switch + * @return bool True if the switch is on, false otherwise + */ + bool get_switch_state(uint8_t switch_index) const; + + /** + * @brief Get the value of all switches + * + * @return uint8_t Value of all switches + */ + uint8_t get_switches_value() const; + + private: + /** + * @brief Array of GPIOs for the switches + */ + std::array gpio_array; }; } // namespace proxy #include "../src/proxy/dip_switch.cpp" // NOLINT(bugprone-suspicious-include) -#endif // MICRAS_PROXY_DIP_SWITCH_HPP +#endif // MICRAS_PROXY_DIP_SWITCH_HPP diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index 7558fb2..21e012b 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -21,69 +21,69 @@ namespace proxy { */ template class DistanceSensors { - public: - /** - * @brief Configuration structure for distance sensors - */ - struct Config { - hal::AdcDma::Config adc; - hal::Pwm::Config led_pwm; - float max_distance; - }; + public: + /** + * @brief Configuration structure for distance sensors + */ + struct Config { + hal::AdcDma::Config adc; + hal::Pwm::Config led_pwm; + float max_distance; + }; - /** - * @brief Constructor for the DistanceSensors class - * - * @param config Configuration for the distance sensors - */ - explicit DistanceSensors(const Config& config); + /** + * @brief Constructor for the DistanceSensors class + * + * @param config Configuration for the distance sensors + */ + explicit DistanceSensors(const Config& config); - /** - * @brief Set the distance sensors led intensity - * - * @param intensity Intensity percentage of the infrared LED - */ - void set_led_intensity(float intensity); + /** + * @brief Set the distance sensors led intensity + * + * @param intensity Intensity percentage of the infrared LED + */ + void set_led_intensity(float intensity); - /** - * @brief Get the distance from a sensor - * - * @param sensor_index Index of the sensor - * @return float Distance reading from the sensors - */ - float get_distance(uint8_t sensor_index) const; + /** + * @brief Get the distance from a sensor + * + * @param sensor_index Index of the sensor + * @return float Distance reading from the sensors + */ + float get_distance(uint8_t sensor_index) const; - /** - * @brief Get the distance from a sensor - * - * @param sensor_index Index of the sensor - * @return uint32_t Raw reading from the distance sensor - */ - uint32_t get_distance_raw(uint8_t sensor_index) const; + /** + * @brief Get the distance from a sensor + * + * @param sensor_index Index of the sensor + * @return uint32_t Raw reading from the distance sensor + */ + uint32_t get_distance_raw(uint8_t sensor_index) const; - private: - /** - * @brief ADC DMA handle - */ - hal::AdcDma adc; + private: + /** + * @brief ADC DMA handle + */ + hal::AdcDma adc; - /** - * @brief PWM handle for infrared LED - */ - hal::Pwm led_pwm; + /** + * @brief PWM handle for infrared LED + */ + hal::Pwm led_pwm; - /** - * @brief Buffer to store the ADC values - */ - std::array buffer; + /** + * @brief Buffer to store the ADC values + */ + std::array buffer; - /** - * @brief Maximum distance reading in meters - */ - const float max_distance; + /** + * @brief Maximum distance reading in meters + */ + const float max_distance; }; } // namespace proxy #include "../src/proxy/distance_sensors.cpp" // NOLINT(bugprone-suspicious-include) -#endif // MICRAS_PROXY_DISTANCE_SENSORS_HPP +#endif // MICRAS_PROXY_DISTANCE_SENSORS_HPP diff --git a/inc/proxy/fan.hpp b/inc/proxy/fan.hpp index dcbcc2c..9dc6cfe 100644 --- a/inc/proxy/fan.hpp +++ b/inc/proxy/fan.hpp @@ -19,76 +19,76 @@ namespace proxy { * @brief Class for controlling the fan driver */ class Fan { - public: - /** - * @brief Configuration structure for the fan - */ - struct Config { - hal::Pwm::Config pwm; - hal::Gpio::Config direction_gpio; - hal::Gpio::Config enable_gpio; - }; + public: + /** + * @brief Configuration structure for the fan + */ + struct Config { + hal::Pwm::Config pwm; + hal::Gpio::Config direction_gpio; + hal::Gpio::Config enable_gpio; + }; - /** - * @brief Enum for rotation direction - */ - enum RotationDirection { - FORWARD, - BACKWARD - }; + /** + * @brief Enum for rotation direction + */ + enum RotationDirection { + FORWARD, + BACKWARD + }; - /** - * @brief Construct a new fan object - * - * @param config Configuration for the fan driver - */ - explicit Fan(const Config& config); + /** + * @brief Construct a new fan object + * + * @param config Configuration for the fan driver + */ + explicit Fan(const Config& config); - /** - * @brief Enable the fan - */ - void enable(); + /** + * @brief Enable the fan + */ + void enable(); - /** - * @brief Disable the fan - */ - void disable(); + /** + * @brief Disable the fan + */ + void disable(); - /** - * @brief Set the speed of the fans - * - * @param speed Speed percentage of the fan - */ - void set_speed(float speed); + /** + * @brief Set the speed of the fans + * + * @param speed Speed percentage of the fan + */ + void set_speed(float speed); - /** - * @brief Stop the fan - */ - void stop(); + /** + * @brief Stop the fan + */ + void stop(); - private: - /** - * @brief Set the rotation direction of the fan - * - * @param direction Rotation direction - */ - void set_direction(RotationDirection direction); + private: + /** + * @brief Set the rotation direction of the fan + * + * @param direction Rotation direction + */ + void set_direction(RotationDirection direction); - /** - * @brief PWM object for controlling the fan speed - */ - hal::Pwm pwm; + /** + * @brief PWM object for controlling the fan speed + */ + hal::Pwm pwm; - /** - * @brief GPIO object for controlling the fan rotation direction - */ - hal::Gpio direction_gpio; + /** + * @brief GPIO object for controlling the fan rotation direction + */ + hal::Gpio direction_gpio; - /** - * @brief GPIO handle for the fan enable pin - */ - hal::Gpio enable_gpio; + /** + * @brief GPIO handle for the fan enable pin + */ + hal::Gpio enable_gpio; }; } // namespace proxy -#endif // MICRAS_PROXY_FAN_HPP +#endif // MICRAS_PROXY_FAN_HPP diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 51d62df..df65f75 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -21,151 +21,151 @@ namespace proxy { * @brief Class to handle IMU peripheral on STM32 microcontrollers */ class Imu { - public: - /** - * @brief IMU configuration struct - */ - struct Config { - hal::Spi::Config spi; - lsm6dsv_data_rate_t gyroscope_data_rate; - lsm6dsv_data_rate_t accelerometer_data_rate; - lsm6dsv_sflp_data_rate_t orientation_data_rate; - lsm6dsv_gy_full_scale_t gyroscope_scale; - lsm6dsv_xl_full_scale_t accelerometer_scale; - lsm6dsv_filt_gy_lp1_bandwidth_t gyroscope_filter; - lsm6dsv_filt_xl_lp2_bandwidth_t accelerometer_filter; - }; - - /** - * @brief Construct a new Imu object - * - * @param config Configuration for the IMU - */ - explicit Imu(const Config& config); - - enum Axis { - W, - X, - Y, - Z - }; - - /** - * @brief Update the IMU data - */ - void update_data(); - - /** - * @brief Get the IMU orientation over an axis - * @todo implement function using sensior fusion - * - * @param axis Axis to get the orientation from - * - * @return float Orientation over the desired axis using quaternions - */ - float get_orientation(Axis axis) const; - - /** - * @brief Get the IMU angular velocity over an axis - * - * @param axis Axis to get the angular velocity from - * - * @return float Angular velocity over the desired axis in rad/s - */ - float get_angular_velocity(Axis axis) const; - - /** - * @brief Get the IMU linear acceleration over an axis - * - * @param axis Axis to get the linear acceleration from - * - * @return float Linear acceleration over the desired axis in m/sยฒ - */ - float get_linear_acceleration(Axis axis) const; - - private: - /** - * @brief Read data from the IMU - * - * @param handle Pointer to a SPI object - * @param reg Register to read from - * @param bufp Buffer to read - * @param len Length of the buffer - * - * @return int32_t 0 if the operation was successful, -1 otherwise - */ - static int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len); - - /** - * @brief Write data to the IMU - * - * @param handle Pointer to a SPI object - * @param reg Register to write to - * @param bufp Buffer to write - * @param len Length of the buffer - * - * @return int32_t 0 if the operation was successful, -1 otherwise - */ - static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len); - - /** - * @brief Function to convert raw data to orientation - * - * @param quat Quaternion to store the orientation - * @param sflp Raw data from the IMU - */ - static void convert_orientation(std::array& quat, const uint16_t sflp[3]); // NOLINT(*-avoid-c-arrays) - - /** - * @brief Function to convert half precision float to single precision float - * - * @param x Half precision float - * - * @return float Single precision float - */ - static float half_to_float(uint16_t x); // NOLINT(readability-identifier-length) - - /** - * @brief Conversion constants - */ - static constexpr float mdps_to_radps{std::numbers::pi_v / 180000.0F}; - static constexpr float mg_to_mps2{0.00980665F}; - - /** - * @brief SPI for the IMU communication - */ - hal::Spi spi; - - /** - * @brief Device context for the IMU library - */ - stmdev_ctx_t dev_ctx{ }; - - /** - * @brief Current angular velocity on each axis - */ - std::array angular_velocity{ }; - - /** - * @brief Current linear acceleration on each axis - */ - std::array linear_acceleration{ }; - - /** - * @brief Current orientation - */ - std::array orientation{ }; - - /** - * @brief Gyroscope conversion factor - */ - const float gy_factor; - - /** - * @brief Accelerometer conversion factor - */ - const float xl_factor; + public: + /** + * @brief IMU configuration struct + */ + struct Config { + hal::Spi::Config spi; + lsm6dsv_data_rate_t gyroscope_data_rate; + lsm6dsv_data_rate_t accelerometer_data_rate; + lsm6dsv_sflp_data_rate_t orientation_data_rate; + lsm6dsv_gy_full_scale_t gyroscope_scale; + lsm6dsv_xl_full_scale_t accelerometer_scale; + lsm6dsv_filt_gy_lp1_bandwidth_t gyroscope_filter; + lsm6dsv_filt_xl_lp2_bandwidth_t accelerometer_filter; + }; + + /** + * @brief Construct a new Imu object + * + * @param config Configuration for the IMU + */ + explicit Imu(const Config& config); + + enum Axis { + W, + X, + Y, + Z + }; + + /** + * @brief Update the IMU data + */ + void update_data(); + + /** + * @brief Get the IMU orientation over an axis + * @todo implement function using sensior fusion + * + * @param axis Axis to get the orientation from + * + * @return float Orientation over the desired axis using quaternions + */ + float get_orientation(Axis axis) const; + + /** + * @brief Get the IMU angular velocity over an axis + * + * @param axis Axis to get the angular velocity from + * + * @return float Angular velocity over the desired axis in rad/s + */ + float get_angular_velocity(Axis axis) const; + + /** + * @brief Get the IMU linear acceleration over an axis + * + * @param axis Axis to get the linear acceleration from + * + * @return float Linear acceleration over the desired axis in m/sยฒ + */ + float get_linear_acceleration(Axis axis) const; + + private: + /** + * @brief Read data from the IMU + * + * @param handle Pointer to a SPI object + * @param reg Register to read from + * @param bufp Buffer to read + * @param len Length of the buffer + * + * @return int32_t 0 if the operation was successful, -1 otherwise + */ + static int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len); + + /** + * @brief Write data to the IMU + * + * @param handle Pointer to a SPI object + * @param reg Register to write to + * @param bufp Buffer to write + * @param len Length of the buffer + * + * @return int32_t 0 if the operation was successful, -1 otherwise + */ + static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len); + + /** + * @brief Function to convert raw data to orientation + * + * @param quat Quaternion to store the orientation + * @param sflp Raw data from the IMU + */ + static void convert_orientation(std::array& quat, const uint16_t sflp[3]); // NOLINT(*-avoid-c-arrays) + + /** + * @brief Function to convert half precision float to single precision float + * + * @param x Half precision float + * + * @return float Single precision float + */ + static float half_to_float(uint16_t x); // NOLINT(readability-identifier-length) + + /** + * @brief Conversion constants + */ + static constexpr float mdps_to_radps{std::numbers::pi_v / 180000.0F}; + static constexpr float mg_to_mps2{0.00980665F}; + + /** + * @brief SPI for the IMU communication + */ + hal::Spi spi; + + /** + * @brief Device context for the IMU library + */ + stmdev_ctx_t dev_ctx{}; + + /** + * @brief Current angular velocity on each axis + */ + std::array angular_velocity{}; + + /** + * @brief Current linear acceleration on each axis + */ + std::array linear_acceleration{}; + + /** + * @brief Current orientation + */ + std::array orientation{}; + + /** + * @brief Gyroscope conversion factor + */ + const float gy_factor; + + /** + * @brief Accelerometer conversion factor + */ + const float xl_factor; }; } // namespace proxy -#endif // MICRAS_PROXY_IMU_HPP +#endif // MICRAS_PROXY_IMU_HPP diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp index da059ed..937fd5c 100644 --- a/inc/proxy/led.hpp +++ b/inc/proxy/led.hpp @@ -16,42 +16,42 @@ namespace proxy { * @brief Class for controlling an LED */ class Led { - public: - /** - * @brief Configuration structure for LED - */ - struct Config { - hal::Gpio::Config gpio; - }; - - /** - * @brief Constructor for the Led class - * - * @param config Configuration for the LED - */ - explicit Led(const Config& config); - - /** - * @brief Turn the LED on - */ - void turn_on(); - - /** - * @brief Turn the LED off - */ - void turn_off(); - - /** - * @brief Toggle the LED - */ - void toggle(); - - private: - /** - * @brief Gpio object for the LED - */ - hal::Gpio gpio; + public: + /** + * @brief Configuration structure for LED + */ + struct Config { + hal::Gpio::Config gpio; + }; + + /** + * @brief Constructor for the Led class + * + * @param config Configuration for the LED + */ + explicit Led(const Config& config); + + /** + * @brief Turn the LED on + */ + void turn_on(); + + /** + * @brief Turn the LED off + */ + void turn_off(); + + /** + * @brief Toggle the LED + */ + void toggle(); + + private: + /** + * @brief Gpio object for the LED + */ + hal::Gpio gpio; }; } // namespace proxy -#endif // MICRAS_PROXY_LED_HPP +#endif // MICRAS_PROXY_LED_HPP diff --git a/inc/proxy/locomotion.hpp b/inc/proxy/locomotion.hpp index 2cd0e09..c682345 100644 --- a/inc/proxy/locomotion.hpp +++ b/inc/proxy/locomotion.hpp @@ -19,92 +19,92 @@ namespace proxy { * @brief Class for controlling the locomotion driver */ class Locomotion { - public: - /** - * @brief Configuration structure for the locomotion - */ - struct Config { - hal::Pwm::Config pwm_left_fwd; - hal::Pwm::Config pwm_left_bwd; - hal::Pwm::Config pwm_right_fwd; - hal::Pwm::Config pwm_right_bwd; - hal::Gpio::Config enable_gpio; - }; - - /** - * @brief Construct a new locomotion object - * - * @param config Configuration for the locomotion driver - */ - explicit Locomotion(const Config& config); - - /** - * @brief Enable the locomotion driver - */ - void enable(); - - /** - * @brief Disable the locomotion driver - */ - void disable(); - - /** - * @brief Set the speed of the wheels - * - * @param left_speed Speed of the left wheels - * @param right_speed Speed of the right wheels - */ - void set_wheel_speed(float left_speed, float right_speed); - - /** - * @brief Set the linear and angular speeds of the robot - * - * @param linear Linear speed of the robot - * @param angular Angular speed of the robot - */ - void set_speed(float linear, float angular); - - /** - * @brief Stop the motors - */ - void stop(); - - /** - * @brief Stop the left motor - */ - void stop_left(); - - /** - * @brief Stop the right motor - */ - void stop_right(); - - private: - /** - * @brief PWM handle for the left motor forward - */ - hal::Pwm pwm_left_fwd; - - /** - * @brief PWM handle for the left motor backward - */ - hal::Pwm pwm_left_bwd; - - /** - * @brief PWM handle for the right motor forward - */ - hal::Pwm pwm_right_fwd; - - /** - * @brief PWM handle for the right motor backward - */ - hal::Pwm pwm_right_bwd; - - /** - * @brief GPIO handle for the motor driver enable pin - */ - hal::Gpio enable_gpio; + public: + /** + * @brief Configuration structure for the locomotion + */ + struct Config { + hal::Pwm::Config pwm_left_fwd; + hal::Pwm::Config pwm_left_bwd; + hal::Pwm::Config pwm_right_fwd; + hal::Pwm::Config pwm_right_bwd; + hal::Gpio::Config enable_gpio; + }; + + /** + * @brief Construct a new locomotion object + * + * @param config Configuration for the locomotion driver + */ + explicit Locomotion(const Config& config); + + /** + * @brief Enable the locomotion driver + */ + void enable(); + + /** + * @brief Disable the locomotion driver + */ + void disable(); + + /** + * @brief Set the speed of the wheels + * + * @param left_speed Speed of the left wheels + * @param right_speed Speed of the right wheels + */ + void set_wheel_speed(float left_speed, float right_speed); + + /** + * @brief Set the linear and angular speeds of the robot + * + * @param linear Linear speed of the robot + * @param angular Angular speed of the robot + */ + void set_speed(float linear, float angular); + + /** + * @brief Stop the motors + */ + void stop(); + + /** + * @brief Stop the left motor + */ + void stop_left(); + + /** + * @brief Stop the right motor + */ + void stop_right(); + + private: + /** + * @brief PWM handle for the left motor forward + */ + hal::Pwm pwm_left_fwd; + + /** + * @brief PWM handle for the left motor backward + */ + hal::Pwm pwm_left_bwd; + + /** + * @brief PWM handle for the right motor forward + */ + hal::Pwm pwm_right_fwd; + + /** + * @brief PWM handle for the right motor backward + */ + hal::Pwm pwm_right_bwd; + + /** + * @brief GPIO handle for the motor driver enable pin + */ + hal::Gpio enable_gpio; }; } // namespace proxy -#endif // MICRAS_PROXY_LOCOMOTION_HPP +#endif // MICRAS_PROXY_LOCOMOTION_HPP diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index 1e04b22..7e60ec2 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -20,87 +20,87 @@ namespace proxy { * @brief Class to handle rotary sensor peripheral on STM32 microcontrollers */ class RotarySensor { - public: - #include "proxy/rotary_sensor_reg.hpp" - - /** - * @brief Rotary sensor configuration struct - */ - struct Config { - hal::Spi::Config spi; - hal::Encoder::Config encoder; - hal::Crc::Config crc; - uint32_t resolution; - Registers registers; + public: +#include "proxy/rotary_sensor_reg.hpp" + + /** + * @brief Rotary sensor configuration struct + */ + struct Config { + hal::Spi::Config spi; + hal::Encoder::Config encoder; + hal::Crc::Config crc; + uint32_t resolution; + Registers registers; + }; + + /** + * @brief Construct a new RotarySensor object + * + * @param config Configuration for the rotary sensor + */ + explicit RotarySensor(const Config& config); + + /** + * @brief Get the rotary sensor position over an axis + * + * @return float Current angular position of the sensor in radians + */ + float get_position() const; + + private: + union CommandFrame { + struct Fields { + uint8_t do_not_care : 1; + uint8_t rw : 1; + uint16_t address : 14; + uint8_t crc : 8; }; - /** - * @brief Construct a new RotarySensor object - * - * @param config Configuration for the rotary sensor - */ - explicit RotarySensor(const Config& config); - - /** - * @brief Get the rotary sensor position over an axis - * - * @return float Current angular position of the sensor in radians - */ - float get_position() const; - - private: - union CommandFrame { - struct Fields { - uint8_t do_not_care : 1; - uint8_t rw : 1; - uint16_t address : 14; - uint8_t crc : 8; - }; - - Fields fields; - uint32_t raw; - }; - - union DataFrame { - struct Fields { - uint8_t warning : 1; - uint8_t error : 1; - uint16_t data : 14; - uint8_t crc : 8; - }; + Fields fields; + uint32_t raw; + }; - Fields fields; - uint32_t raw; + union DataFrame { + struct Fields { + uint8_t warning : 1; + uint8_t error : 1; + uint16_t data : 14; + uint8_t crc : 8; }; - /** - * @brief Write a register to the rotary sensor - * - * @param command_frame Command frame to send trough SPI - * @param data_frame Data frame to send trough SPI - */ - void write_register(CommandFrame& command_frame, DataFrame& data_frame); - - /** - * @brief SPI for the rotary sensor configuration - */ - hal::Spi spi; - - /** - * @brief Encoder for getting the rotary sensor data - */ - hal::Encoder encoder; - - /** - * @brief CRC for the rotary sensor configuration - */ - hal::Crc crc; - - /** - * @brief Resolution of the rotary sensor - */ - uint32_t resolution; + Fields fields; + uint32_t raw; + }; + + /** + * @brief Write a register to the rotary sensor + * + * @param command_frame Command frame to send trough SPI + * @param data_frame Data frame to send trough SPI + */ + void write_register(CommandFrame& command_frame, DataFrame& data_frame); + + /** + * @brief SPI for the rotary sensor configuration + */ + hal::Spi spi; + + /** + * @brief Encoder for getting the rotary sensor data + */ + hal::Encoder encoder; + + /** + * @brief CRC for the rotary sensor configuration + */ + hal::Crc crc; + + /** + * @brief Resolution of the rotary sensor + */ + uint32_t resolution; }; } // namespace proxy -#endif // MICRAS_PROXY_ROTARY_SENSOR_HPP +#endif // MICRAS_PROXY_ROTARY_SENSOR_HPP diff --git a/inc/proxy/rotary_sensor_reg.hpp b/inc/proxy/rotary_sensor_reg.hpp index 0426148..a6e750f 100644 --- a/inc/proxy/rotary_sensor_reg.hpp +++ b/inc/proxy/rotary_sensor_reg.hpp @@ -21,9 +21,9 @@ struct Registers { */ union Disable { struct Fields { - uint8_t UVW_off : 1; - uint8_t ABI_off : 1; - uint8_t na : 4; + uint8_t UVW_off : 1; + uint8_t ABI_off : 1; + uint8_t na : 4; uint8_t FILTER_disable : 1; }; @@ -42,7 +42,7 @@ struct Registers { union Zposl { struct Fields { - uint8_t ZPOSL : 6; + uint8_t ZPOSL : 6; uint8_t Dia1_en : 1; uint8_t Dia2_en : 1; }; @@ -53,8 +53,8 @@ struct Registers { union Settings1 { struct Fields { - uint8_t K_max : 3; - uint8_t K_min : 3; + uint8_t K_max : 3; + uint8_t K_min : 3; uint8_t Dia3_en : 1; uint8_t Dia4_en : 1; }; @@ -65,14 +65,14 @@ struct Registers { union Settings2 { struct Fields { - uint8_t IWIDTH : 1; - uint8_t NOISESET : 1; - uint8_t DIR : 1; - uint8_t UVW_ABI : 1; - uint8_t DAECDIS : 1; - uint8_t ABI_DEC : 1; + uint8_t IWIDTH : 1; + uint8_t NOISESET : 1; + uint8_t DIR : 1; + uint8_t UVW_ABI : 1; + uint8_t DAECDIS : 1; + uint8_t ABI_DEC : 1; uint8_t Data_select : 1; - uint8_t PWMon : 1; + uint8_t PWMon : 1; }; Fields fields; @@ -81,8 +81,8 @@ struct Registers { union Settings3 { struct Fields { - uint8_t UVWPP : 3; - uint8_t HYS : 2; + uint8_t UVWPP : 3; + uint8_t HYS : 2; uint8_t ABIRES : 3; }; @@ -93,7 +93,7 @@ struct Registers { union Ecc { struct Fields { uint8_t ECC_chsum : 7; - uint8_t ECC_en : 1; + uint8_t ECC_en : 1; }; Fields fields; @@ -114,13 +114,13 @@ struct Registers { /** * @brief Member variables to be configured in the rotary sensor */ - Disable disable; - Zposm zposm; - Zposl zposl; - Settings1 settings1; - Settings2 settings2; - Settings3 settings3; - Ecc ecc; + Disable disable; + Zposm zposm; + Zposl zposl; + Settings1 settings1; + Settings2 settings2; + Settings3 settings3; + Ecc ecc; }; -#endif // MICRAS_PROXY_ROTARY_SENSOR_REG_HPP +#endif // MICRAS_PROXY_ROTARY_SENSOR_REG_HPP diff --git a/inc/proxy/serializable_interface.hpp b/inc/proxy/serializable_interface.hpp index 4aec07d..dc147e9 100644 --- a/inc/proxy/serializable_interface.hpp +++ b/inc/proxy/serializable_interface.hpp @@ -16,38 +16,38 @@ * @brief Interface class for serializable classes */ class ISerializable { - public: - /** - * @brief Virtual destructor for the ISerializable class - */ - virtual ~ISerializable() = default; - - /** - * @brief Serialize the class instance - * - * @return std::vector Serialized data - */ - virtual std::vector serialize() const = 0; - - /** - * @brief Deserialize the class instance - * - * @param serial_data Serialized data - * @param size Size of the serialized data - */ - virtual void deserialize(uint8_t* serial_data, uint16_t size) = 0; - - protected: - /** - * @brief Special member functions declared as default - */ - ISerializable(const ISerializable&) = default; - - ISerializable(ISerializable&&) = default; - - ISerializable& operator =(const ISerializable&) = default; - - ISerializable& operator =(ISerializable&&) = default; + public: + /** + * @brief Virtual destructor for the ISerializable class + */ + virtual ~ISerializable() = default; + + /** + * @brief Serialize the class instance + * + * @return std::vector Serialized data + */ + virtual std::vector serialize() const = 0; + + /** + * @brief Deserialize the class instance + * + * @param serial_data Serialized data + * @param size Size of the serialized data + */ + virtual void deserialize(uint8_t* serial_data, uint16_t size) = 0; + + protected: + /** + * @brief Special member functions declared as default + */ + ISerializable(const ISerializable&) = default; + + ISerializable(ISerializable&&) = default; + + ISerializable& operator=(const ISerializable&) = default; + + ISerializable& operator=(ISerializable&&) = default; }; -#endif // MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP +#endif // MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp index 4ec8875..0a50cef 100644 --- a/inc/proxy/storage.hpp +++ b/inc/proxy/storage.hpp @@ -25,130 +25,130 @@ concept Fundamental = std::is_fundamental::value; * @brief Class for controlling the storage */ class Storage { - public: - /** - * @brief Configuration structure for the storage - */ - struct Config { - uint16_t start_page; - uint16_t number_of_pages; - }; - - /** - * @brief Constructor for the Storage class - * - * @param config Configuration for the storage - */ - explicit Storage(const Config& config); - - /** - * @brief Create a new primitive variable in the storage - * - * @tparam T Type of the variable - * @param name Name of the variable - * @param data Reference to the variable - */ - template - void create(const std::string& name, const T& data); - - /** - * @brief Create a new serializable variable in the storage - * - * @param name Name of the variable - * @param data Reference to the variable - */ - void create(const std::string& name, const ISerializable& data); - - /** - * @brief Sync a primitive variable with the storage - * - * @tparam T Type of the variable - * @param name Name of the variable - * @param data Reference to the variable - */ - template - void sync(const std::string& name, T& data); - - /** - * @brief Sync a serializable variable with the storage - * - * @param name Name of the variable - * @param data Reference to the variable - */ - void sync(const std::string& name, ISerializable& data); - - /** - * @brief Save the storage to the flash - */ - void save(); - - private: - /** - * @brief Structure for primitive variables - */ - struct PrimitiveVariable { - const void* ram_pointer{nullptr}; - uint16_t buffer_address{ }; - uint16_t size{ }; - }; - - /** - * @brief Structure for serializable variables - */ - struct SerializableVariable { - const ISerializable* ram_pointer{nullptr}; - uint16_t buffer_address{ }; - uint16_t size{ }; - }; - - /** - * @brief Serialize a map of variables - * - * @tparam T Type of the variables - * @param variables Map of variables - * - * @return std::vector Serialized buffer - */ - template - static std::vector serialize_var_map(const std::unordered_map& variables); - - /** - * @brief Deserialize a map of variables - * - * @tparam T Type of the variables - * @param buffer Serialized buffer - * @param num_vars Number of variables - * - * @return std::unordered_map Deserialized map of variables - */ - template - static std::unordered_map deserialize_var_map(std::vector& buffer, uint16_t num_vars); - - /** - * @brief Map of primitive variables - */ - std::unordered_map primitives; - - /** - * @brief Map of serializable variables - */ - std::unordered_map serializables; - - /** - * @brief Serialized buffer for the storage - */ - std::vector buffer; - - /** - * @brief Start page of the storage in the flash memory - */ + public: + /** + * @brief Configuration structure for the storage + */ + struct Config { uint16_t start_page; - - /** - * @brief Maximum number of pages used by the storage in the flash memory - */ uint16_t number_of_pages; + }; + + /** + * @brief Constructor for the Storage class + * + * @param config Configuration for the storage + */ + explicit Storage(const Config& config); + + /** + * @brief Create a new primitive variable in the storage + * + * @tparam T Type of the variable + * @param name Name of the variable + * @param data Reference to the variable + */ + template + void create(const std::string& name, const T& data); + + /** + * @brief Create a new serializable variable in the storage + * + * @param name Name of the variable + * @param data Reference to the variable + */ + void create(const std::string& name, const ISerializable& data); + + /** + * @brief Sync a primitive variable with the storage + * + * @tparam T Type of the variable + * @param name Name of the variable + * @param data Reference to the variable + */ + template + void sync(const std::string& name, T& data); + + /** + * @brief Sync a serializable variable with the storage + * + * @param name Name of the variable + * @param data Reference to the variable + */ + void sync(const std::string& name, ISerializable& data); + + /** + * @brief Save the storage to the flash + */ + void save(); + + private: + /** + * @brief Structure for primitive variables + */ + struct PrimitiveVariable { + const void* ram_pointer{nullptr}; + uint16_t buffer_address{}; + uint16_t size{}; + }; + + /** + * @brief Structure for serializable variables + */ + struct SerializableVariable { + const ISerializable* ram_pointer{nullptr}; + uint16_t buffer_address{}; + uint16_t size{}; + }; + + /** + * @brief Serialize a map of variables + * + * @tparam T Type of the variables + * @param variables Map of variables + * + * @return std::vector Serialized buffer + */ + template + static std::vector serialize_var_map(const std::unordered_map& variables); + + /** + * @brief Deserialize a map of variables + * + * @tparam T Type of the variables + * @param buffer Serialized buffer + * @param num_vars Number of variables + * + * @return std::unordered_map Deserialized map of variables + */ + template + static std::unordered_map deserialize_var_map(std::vector& buffer, uint16_t num_vars); + + /** + * @brief Map of primitive variables + */ + std::unordered_map primitives; + + /** + * @brief Map of serializable variables + */ + std::unordered_map serializables; + + /** + * @brief Serialized buffer for the storage + */ + std::vector buffer; + + /** + * @brief Start page of the storage in the flash memory + */ + uint16_t start_page; + + /** + * @brief Maximum number of pages used by the storage in the flash memory + */ + uint16_t number_of_pages; }; } // namespace proxy -#endif // MICRAS_PROXY_STORAGE_HPP +#endif // MICRAS_PROXY_STORAGE_HPP diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index 282f1e7..1084c7d 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -19,51 +19,51 @@ namespace proxy { */ template class TorqueSensors { - public: - /** - * @brief Configuration structure for torque sensors - */ - struct Config { - typename CurrentSensors::Config current_sensors; - float max_torque; - }; + public: + /** + * @brief Configuration structure for torque sensors + */ + struct Config { + typename CurrentSensors::Config current_sensors; + float max_torque; + }; - /** - * @brief Constructor for the TorqueSensors class - * - * @param config Configuration for the torque sensors - */ - explicit TorqueSensors(const Config& config); + /** + * @brief Constructor for the TorqueSensors class + * + * @param config Configuration for the torque sensors + */ + explicit TorqueSensors(const Config& config); - /** - * @brief Get the torque from the sensor - * - * @param sensor_index Index of the sensor - * @return float Torque reading from the sensor in N*m - */ - float get_torque(uint8_t sensor_index) const; + /** + * @brief Get the torque from the sensor + * + * @param sensor_index Index of the sensor + * @return float Torque reading from the sensor in N*m + */ + float get_torque(uint8_t sensor_index) const; - /** - * @brief Get the current from the sensor - * - * @param sensor_index Index of the sensor - * @return float Current reading from the sensor in amps - */ - float get_current(uint8_t sensor_index) const; + /** + * @brief Get the current from the sensor + * + * @param sensor_index Index of the sensor + * @return float Current reading from the sensor in amps + */ + float get_current(uint8_t sensor_index) const; - private: - /** - * @brief Current sensors object - */ - CurrentSensors current_sensors; + private: + /** + * @brief Current sensors object + */ + CurrentSensors current_sensors; - /** - * @brief Maximum torque that can be measured by the sensor - */ - const float max_torque; + /** + * @brief Maximum torque that can be measured by the sensor + */ + const float max_torque; }; } // namespace proxy #include "../src/proxy/torque_sensors.cpp" // NOLINT(bugprone-suspicious-include) -#endif // MICRAS_PROXY_TORQUE_SENSORS_HPP +#endif // MICRAS_PROXY_TORQUE_SENSORS_HPP diff --git a/src/hal/crc.cpp b/src/hal/crc.cpp index 353a009..ba97330 100644 --- a/src/hal/crc.cpp +++ b/src/hal/crc.cpp @@ -9,8 +9,7 @@ #include "hal/crc.hpp" namespace hal { -Crc::Crc(const Config& config) : handle{config.handle} { -} +Crc::Crc(const Config& config) : handle{config.handle} { } // NOLINTNEXTLINE(*-avoid-c-arrays) uint32_t Crc::calculate(uint32_t data[], uint32_t size) { diff --git a/src/hal/flash.cpp b/src/hal/flash.cpp index 1c5eead..b02f150 100644 --- a/src/hal/flash.cpp +++ b/src/hal/flash.cpp @@ -38,7 +38,9 @@ void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { while (address <= end) { if (size >= double_words_per_row) { - HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, reinterpret_cast(data)); // NOLINT(clang-diagnostic-pointer-to-int-cast) + HAL_FLASH_Program( + FLASH_TYPEPROGRAM_FAST, address, reinterpret_cast(data) + ); // NOLINT(clang-diagnostic-pointer-to-int-cast) address += bytes_per_row; data += double_words_per_row; size -= double_words_per_row; @@ -66,7 +68,7 @@ void Flash::erase_pages(uint16_t page, uint16_t number_of_pages) { .NbPages = number_of_pages, }; - uint32_t page_error{ }; + uint32_t page_error{}; HAL_FLASHEx_Erase(&erase_struct, &page_error); } diff --git a/src/hal/gpio.cpp b/src/hal/gpio.cpp index 4117ae6..1cc60f8 100644 --- a/src/hal/gpio.cpp +++ b/src/hal/gpio.cpp @@ -9,8 +9,7 @@ #include "hal/gpio.hpp" namespace hal { -Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { -} +Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { } bool Gpio::read() const { return HAL_GPIO_ReadPin(this->port, this->pin) == GPIO_PIN_SET; diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp index bd7febe..0e480eb 100644 --- a/src/hal/timer.cpp +++ b/src/hal/timer.cpp @@ -39,15 +39,13 @@ uint32_t Timer::elapsed_time_us() const { void Timer::sleep_ms(uint32_t time) { uint32_t start = HAL_GetTick(); - while (HAL_GetTick() - start < time) { - } + while (HAL_GetTick() - start < time) { } } void Timer::sleep_us(uint32_t time) const { uint32_t start = this->get_counter_us(); - while (this->get_counter_us() - start < time) { - } + while (this->get_counter_us() - start < time) { } } uint32_t Timer::get_counter_ms() { diff --git a/src/proxy/argb.cpp b/src/proxy/argb.cpp index d004eb4..7e73043 100644 --- a/src/proxy/argb.cpp +++ b/src/proxy/argb.cpp @@ -13,9 +13,8 @@ namespace proxy { template -Argb::Argb(const Config& config) : pwm{config.pwm}, - low_bit{pwm.get_compare(low_duty_cycle)}, high_bit{pwm.get_compare(high_duty_cycle)} { -} +Argb::Argb(const Config& config) : + pwm{config.pwm}, low_bit{pwm.get_compare(low_duty_cycle)}, high_bit{pwm.get_compare(high_duty_cycle)} { } template void Argb::set_color(const Color& color, uint8_t index) { @@ -57,4 +56,4 @@ void Argb::encode_color(const Color& color, uint8_t index) { } } // namespace proxy -#endif // MICRAS_PROXY_ARGB_CPP +#endif // MICRAS_PROXY_ARGB_CPP diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp index 63278b9..c64b2ac 100644 --- a/src/proxy/button.cpp +++ b/src/proxy/button.cpp @@ -14,8 +14,7 @@ Button::Button(const Config& config) : long_press_delay{config.long_press_delay}, extra_long_press_delay{config.extra_long_press_delay}, gpio{config.gpio}, - pull_resistor{config.pull_resistor} { -} + pull_resistor{config.pull_resistor} { } bool Button::is_pressed() { this->update_state(); diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp index 490784d..b151b6f 100644 --- a/src/proxy/current_sensors.cpp +++ b/src/proxy/current_sensors.cpp @@ -20,8 +20,7 @@ CurrentSensors::CurrentSensors(const Config& config) : template float CurrentSensors::get_current(uint8_t sensor_index) const { - return this->adc.reference_voltage * this->buffer.at(sensor_index) / - (this->adc.max_reading * this->shunt_resistor); + return this->adc.reference_voltage * this->buffer.at(sensor_index) / (this->adc.max_reading * this->shunt_resistor); } template @@ -30,4 +29,4 @@ uint32_t CurrentSensors::get_current_raw(uint8_t sensor_index) c } } // namespace proxy -#endif // MICRAS_PROXY_CURRENT_SENSORS_CPP +#endif // MICRAS_PROXY_CURRENT_SENSORS_CPP diff --git a/src/proxy/dip_switch.cpp b/src/proxy/dip_switch.cpp index 47f41b3..94040c5 100644 --- a/src/proxy/dip_switch.cpp +++ b/src/proxy/dip_switch.cpp @@ -15,10 +15,9 @@ namespace proxy { template DipSwitch::DipSwitch(const Config& config) : - gpio_array([&] (std::index_sequence) -> std::array { + gpio_array([&](std::index_sequence)->std::array { return {hal::Gpio{config.gpio_array[I]}...}; - }(std::make_index_sequence())) { -} + }(std::make_index_sequence())) { } template bool DipSwitch::get_switch_state(uint8_t switch_index) const { @@ -37,4 +36,4 @@ uint8_t DipSwitch::get_switches_value() const { } } // namespace proxy -#endif // MICRAS_PROXY_DIP_SWITCH_CPP +#endif // MICRAS_PROXY_DIP_SWITCH_CPP diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index 702602f..c3f6a79 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -35,4 +35,4 @@ uint32_t DistanceSensors::get_distance_raw(uint8_t sensor_index) } } // namespace proxy -#endif // MICRAS_PROXY_DISTANCE_SENSORS_CPP +#endif // MICRAS_PROXY_DISTANCE_SENSORS_CPP diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp index 8714353..524877f 100644 --- a/src/proxy/imu.cpp +++ b/src/proxy/imu.cpp @@ -12,9 +12,11 @@ #include "proxy/imu.hpp" namespace proxy { -Imu::Imu(const Config& config) : spi{config.spi}, - gy_factor{mdps_to_radps * 4.375F * - (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale)))}, +Imu::Imu(const Config& config) : + spi{config.spi}, + gy_factor{ + mdps_to_radps * 4.375F * + (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale)))}, xl_factor{mg_to_mps2 * (0.61F * (1 << static_cast(config.accelerometer_scale)))} { this->dev_ctx.read_reg = platform_read; this->dev_ctx.write_reg = platform_write; @@ -24,7 +26,7 @@ Imu::Imu(const Config& config) : spi{config.spi}, lsm6dsv_reset_set(&(this->dev_ctx), LSM6DSV_RESTORE_CTRL_REGS); - lsm6dsv_reset_t rst{ }; + lsm6dsv_reset_t rst{}; do { lsm6dsv_reset_get(&(this->dev_ctx), &rst); @@ -148,8 +150,7 @@ float Imu::get_linear_acceleration(Axis axis) const { int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) { auto* spi = static_cast(handle); - while (not spi->select_device()) { - } + while (not spi->select_device()) { } reg |= 0x80; spi->transmit(®, 1); @@ -162,8 +163,7 @@ int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t le int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len) { auto* spi = static_cast(handle); - while (not spi->select_device()) { - } + while (not spi->select_device()) { } spi->transmit(®, 1); spi->transmit(const_cast(bufp), len); // NOLINT(cppcoreguidelines-pro-type-const-cast) @@ -197,24 +197,22 @@ void Imu::convert_orientation(std::array& quat, const uint16_t sflp[3] // NOLINTBEGIN(readability-identifier-length, readability-implicit-bool-conversion) float Imu::half_to_float(uint16_t x) { - static constexpr auto as_float = - [](uint32_t x) -> float { - void* aux = &x; - return *reinterpret_cast(aux); - }; - static constexpr auto as_uint = - [](float x) -> uint32_t { - void* aux = &x; - return *reinterpret_cast(aux); - }; + static constexpr auto as_float = [](uint32_t x) -> float { + void* aux = &x; + return *reinterpret_cast(aux); + }; + static constexpr auto as_uint = [](float x) -> uint32_t { + void* aux = &x; + return *reinterpret_cast(aux); + }; const uint32_t e = (x & 0x7C00) >> 10; const uint32_t m = (x & 0x03FF) << 13; const uint32_t v = as_uint(static_cast(m)) >> 23; return as_float( - (x & 0x8000) << 16 | - (e != 0) * ((e + 112) << 23 | m) | - ((e == 0) & (m != 0)) * ((v - 37) << 23 | ((m << (150 - v)) & 0x007FE000))); + (x & 0x8000) << 16 | (e != 0) * ((e + 112) << 23 | m) | + ((e == 0) & (m != 0)) * ((v - 37) << 23 | ((m << (150 - v)) & 0x007FE000)) + ); } // NOLINTEND(readability-identifier-length, readability-implicit-bool-conversion) diff --git a/src/proxy/led.cpp b/src/proxy/led.cpp index 56e3db5..6e73259 100644 --- a/src/proxy/led.cpp +++ b/src/proxy/led.cpp @@ -9,8 +9,7 @@ #include "proxy/led.hpp" namespace proxy { -Led::Led(const Config& config) : gpio{config.gpio} { -} +Led::Led(const Config& config) : gpio{config.gpio} { } void Led::turn_on() { this->gpio.write(true); diff --git a/src/proxy/locomotion.cpp b/src/proxy/locomotion.cpp index f7afc81..3b26eb3 100644 --- a/src/proxy/locomotion.cpp +++ b/src/proxy/locomotion.cpp @@ -12,8 +12,10 @@ namespace proxy { Locomotion::Locomotion(const Config& config) : - pwm_left_fwd{config.pwm_left_fwd}, pwm_left_bwd{config.pwm_left_bwd}, - pwm_right_fwd{config.pwm_right_fwd}, pwm_right_bwd{config.pwm_right_bwd}, + pwm_left_fwd{config.pwm_left_fwd}, + pwm_left_bwd{config.pwm_left_bwd}, + pwm_right_fwd{config.pwm_right_fwd}, + pwm_right_bwd{config.pwm_right_bwd}, enable_gpio{config.enable_gpio} { this->stop(); this->enable(); diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp index d62493a..96ae50e 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/src/proxy/rotary_sensor.cpp @@ -13,8 +13,8 @@ namespace proxy { RotarySensor::RotarySensor(const Config& config) : spi{config.spi}, encoder{config.encoder}, crc{config.crc}, resolution{config.resolution} { - CommandFrame command_frame{ }; - DataFrame data_frame{ }; + CommandFrame command_frame{}; + DataFrame data_frame{}; command_frame.fields.address = Registers::disable_addr; data_frame.fields.data = config.registers.disable.raw; @@ -50,8 +50,7 @@ float RotarySensor::get_position() const { } void RotarySensor::write_register(CommandFrame& command_frame, DataFrame& data_frame) { - while (not this->spi.select_device()) { - } + while (not this->spi.select_device()) { } command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2); data_frame.fields.crc = this->crc.calculate(&data_frame.raw, 2); diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index afd0ab1..60258c7 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -11,7 +11,7 @@ namespace proxy { Storage::Storage(const Config& config) : start_page{config.start_page}, number_of_pages{config.number_of_pages} { - uint64_t sizes{ }; + uint64_t sizes{}; hal::Flash::read(this->start_page, 0, &sizes); uint32_t total_size = sizes >> 32; @@ -57,7 +57,7 @@ void Storage::save() { this->buffer.clear(); hal::Flash::erase_pages(this->start_page, this->number_of_pages); - for (auto& [name, variable]: this->primitives) { + for (auto& [name, variable] : this->primitives) { if (variable.ram_pointer == nullptr) { this->primitives.erase(name); continue; @@ -68,7 +68,7 @@ void Storage::save() { this->buffer.insert(this->buffer.end(), aux, aux + variable.size); } - for (auto& [name, variable]: this->serializables) { + for (auto& [name, variable] : this->serializables) { if (variable.ram_pointer == nullptr) { this->serializables.erase(name); continue; @@ -106,7 +106,7 @@ template std::vector Storage::serialize_var_map(const std::unordered_map& variables) { std::vector buffer; - for (auto [name, variable]: variables) { + for (auto [name, variable] : variables) { buffer.emplace_back(name.size()); buffer.insert(buffer.end(), name.begin(), name.end()); @@ -123,10 +123,12 @@ std::vector Storage::serialize_var_map(const std::unordered_map std::unordered_map Storage::deserialize_var_map(std::vector& buffer, uint16_t num_vars) { std::unordered_map variables; + uint16_t current_addr = 0; for (uint16_t decoded_vars = 0; decoded_vars < num_vars; decoded_vars++) { uint8_t var_name_len = buffer.at(current_addr); + std::string var_name(buffer.begin() + current_addr + 1, buffer.begin() + current_addr + 1 + var_name_len); current_addr += var_name_len + 1; diff --git a/src/proxy/torque_sensors.cpp b/src/proxy/torque_sensors.cpp index d721b2a..ca0d8ee 100644 --- a/src/proxy/torque_sensors.cpp +++ b/src/proxy/torque_sensors.cpp @@ -14,8 +14,7 @@ namespace proxy { template TorqueSensors::TorqueSensors(const Config& config) : - current_sensors{config.current_sensors}, max_torque{config.max_torque} { -} + current_sensors{config.current_sensors}, max_torque{config.max_torque} { } template float TorqueSensors::get_torque(uint8_t sensor_index) const { @@ -28,4 +27,4 @@ float TorqueSensors::get_current(uint8_t sensor_index) const { } } // namespace proxy -#endif // MICRAS_PROXY_TORQUE_SENSORS_CPP +#endif // MICRAS_PROXY_TORQUE_SENSORS_CPP From 48d7e79a467129db6b68f8780e3b9c1d287e1b5d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 4 Apr 2024 15:06:30 -0300 Subject: [PATCH 129/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Add=20type=20to=20?= =?UTF-8?q?enums?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/button.hpp | 4 ++-- inc/proxy/fan.hpp | 2 +- inc/proxy/imu.hpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index 5fcd73d..6b95fd9 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -23,7 +23,7 @@ class Button { /** * @brief Enum for button status */ - enum Status { + enum Status : uint8_t { NO_PRESS, SHORT_PRESS, LONG_PRESS, @@ -33,7 +33,7 @@ class Button { /** * @brief Enum for button pull resistor */ - enum PullResistor { + enum PullResistor : uint8_t { PULL_UP, PULL_DOWN, }; diff --git a/inc/proxy/fan.hpp b/inc/proxy/fan.hpp index 9dc6cfe..bb8e395 100644 --- a/inc/proxy/fan.hpp +++ b/inc/proxy/fan.hpp @@ -32,7 +32,7 @@ class Fan { /** * @brief Enum for rotation direction */ - enum RotationDirection { + enum RotationDirection : uint8_t { FORWARD, BACKWARD }; diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index df65f75..2fb76ef 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -43,7 +43,7 @@ class Imu { */ explicit Imu(const Config& config); - enum Axis { + enum Axis : uint8_t { W, X, Y, From f34c25992e6069fc4595d7fa28d18595530e90e1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 4 Apr 2024 15:06:46 -0300 Subject: [PATCH 130/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warning?= =?UTF-8?q?=20on=20flash.cpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/hal/flash.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/hal/flash.cpp b/src/hal/flash.cpp index b02f150..61504a0 100644 --- a/src/hal/flash.cpp +++ b/src/hal/flash.cpp @@ -38,9 +38,8 @@ void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { while (address <= end) { if (size >= double_words_per_row) { - HAL_FLASH_Program( - FLASH_TYPEPROGRAM_FAST, address, reinterpret_cast(data) - ); // NOLINT(clang-diagnostic-pointer-to-int-cast) + // NOLINTNEXTLINE(clang-diagnostic-pointer-to-int-cast) + HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, reinterpret_cast(data)); address += bytes_per_row; data += double_words_per_row; size -= double_words_per_row; From cd9cecbe5d5bd48037a224cde3948344e6bb9988 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 5 Apr 2024 12:19:59 -0300 Subject: [PATCH 131/561] =?UTF-8?q?=F0=9F=8E=A8=20Remove=20indentation=20o?= =?UTF-8?q?f=20access=20specifiers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-format | 2 +- inc/hal/adc_dma.hpp | 4 ++-- inc/hal/crc.hpp | 4 ++-- inc/hal/encoder.hpp | 4 ++-- inc/hal/flash.hpp | 4 ++-- inc/hal/gpio.hpp | 4 ++-- inc/hal/mcu.hpp | 2 +- inc/hal/pwm.hpp | 4 ++-- inc/hal/pwm_dma.hpp | 4 ++-- inc/hal/spi.hpp | 4 ++-- inc/hal/timer.hpp | 4 ++-- inc/proxy/argb.hpp | 4 ++-- inc/proxy/battery.hpp | 4 ++-- inc/proxy/button.hpp | 4 ++-- inc/proxy/buzzer.hpp | 4 ++-- inc/proxy/current_sensors.hpp | 4 ++-- inc/proxy/dip_switch.hpp | 4 ++-- inc/proxy/distance_sensors.hpp | 4 ++-- inc/proxy/fan.hpp | 4 ++-- inc/proxy/imu.hpp | 4 ++-- inc/proxy/led.hpp | 4 ++-- inc/proxy/locomotion.hpp | 4 ++-- inc/proxy/rotary_sensor.hpp | 4 ++-- inc/proxy/serializable_interface.hpp | 4 ++-- inc/proxy/storage.hpp | 4 ++-- inc/proxy/torque_sensors.hpp | 4 ++-- 26 files changed, 50 insertions(+), 50 deletions(-) diff --git a/.clang-format b/.clang-format index ef22191..da34eff 100644 --- a/.clang-format +++ b/.clang-format @@ -1,7 +1,7 @@ --- Language: Cpp # BasedOnStyle: LLVM -AccessModifierOffset: -2 +AccessModifierOffset: -4 AlignAfterOpenBracket: BlockIndent AlignArrayOfStructures: None AlignConsecutiveMacros: None diff --git a/inc/hal/adc_dma.hpp b/inc/hal/adc_dma.hpp index 6542a9d..814c9a6 100644 --- a/inc/hal/adc_dma.hpp +++ b/inc/hal/adc_dma.hpp @@ -17,7 +17,7 @@ namespace hal { * @brief Class to handle ADC peripheral on STM32 microcontrollers using DMA */ class AdcDma { - public: +public: /** * @brief Configuration structure for ADC DMA */ @@ -58,7 +58,7 @@ class AdcDma { */ const float reference_voltage; // NOLINT(*-non-private-member-variables-in-classes) - private: +private: /** * @brief ADC handle */ diff --git a/inc/hal/crc.hpp b/inc/hal/crc.hpp index ede41fd..9a666b6 100644 --- a/inc/hal/crc.hpp +++ b/inc/hal/crc.hpp @@ -16,7 +16,7 @@ namespace hal { * @brief Class to handle the cyclic redundancy check peripheral on STM32 microcontrollers */ class Crc { - public: +public: /** * @brief CRC configuration struct */ @@ -41,7 +41,7 @@ class Crc { */ uint32_t calculate(uint32_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) - private: +private: /** * @brief CRC handle */ diff --git a/inc/hal/encoder.hpp b/inc/hal/encoder.hpp index 2f14350..684b83e 100644 --- a/inc/hal/encoder.hpp +++ b/inc/hal/encoder.hpp @@ -16,7 +16,7 @@ namespace hal { * @brief Class to handle encoder peripheral on STM32 microcontrollers */ class Encoder { - public: +public: /** * @brief Encoder configuration struct */ @@ -40,7 +40,7 @@ class Encoder { */ int32_t get_counter() const; - private: +private: /** * @brief Timer handle */ diff --git a/inc/hal/flash.hpp b/inc/hal/flash.hpp index 78ce641..eda7e59 100644 --- a/inc/hal/flash.hpp +++ b/inc/hal/flash.hpp @@ -17,7 +17,7 @@ namespace hal { * @brief Class to handle flash memory on STM32 microcontrollers */ class Flash { - public: +public: /** * @brief Deleted constructor for static class */ @@ -73,7 +73,7 @@ class Flash { */ static void erase_pages(uint16_t page, uint16_t number_of_pages = 1); - private: +private: /** * @brief Number of double words per row */ diff --git a/inc/hal/gpio.hpp b/inc/hal/gpio.hpp index 428787d..633601a 100644 --- a/inc/hal/gpio.hpp +++ b/inc/hal/gpio.hpp @@ -17,7 +17,7 @@ namespace hal { * @brief Class for controlling GPIO pins on STM32 microcontrollers */ class Gpio { - public: +public: /** * @brief Configuration structure for GPIO pin */ @@ -52,7 +52,7 @@ class Gpio { */ void toggle(); - private: +private: /** * @brief The port of the GPIO */ diff --git a/inc/hal/mcu.hpp b/inc/hal/mcu.hpp index 701bb4e..b947ff1 100644 --- a/inc/hal/mcu.hpp +++ b/inc/hal/mcu.hpp @@ -16,7 +16,7 @@ namespace hal { * @brief Microcontroller unit class */ class Mcu { - public: +public: /** * @brief Deleted constructor for static class */ diff --git a/inc/hal/pwm.hpp b/inc/hal/pwm.hpp index 977305a..fe933bb 100644 --- a/inc/hal/pwm.hpp +++ b/inc/hal/pwm.hpp @@ -17,7 +17,7 @@ namespace hal { * @brief Class to handle PWM peripheral on STM32 microcontrollers */ class Pwm { - public: +public: /** * @brief PWM configuration struct */ @@ -48,7 +48,7 @@ class Pwm { */ void set_frequency(uint32_t frequency); - private: +private: /** * @brief Timer handle */ diff --git a/inc/hal/pwm_dma.hpp b/inc/hal/pwm_dma.hpp index 1785a44..3241b60 100644 --- a/inc/hal/pwm_dma.hpp +++ b/inc/hal/pwm_dma.hpp @@ -17,7 +17,7 @@ namespace hal { * @brief Class to handle PWM peripheral on STM32 microcontrollers using DMA */ class PwmDma { - public: +public: /** * @brief PWM configuration struct */ @@ -55,7 +55,7 @@ class PwmDma { */ uint32_t get_compare(float duty_cycle) const; - private: +private: /** * @brief Timer handle */ diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index 1f4d1bc..8389af5 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -19,7 +19,7 @@ namespace hal { * @brief Class to handle SPI peripheral on STM32 microcontrollers */ class Spi { - public: +public: /** * @brief SPI configuration struct */ @@ -65,7 +65,7 @@ class Spi { */ void receive(uint8_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) - private: +private: /** * @brief Handle for the SPI */ diff --git a/inc/hal/timer.hpp b/inc/hal/timer.hpp index 4439010..88fbce3 100644 --- a/inc/hal/timer.hpp +++ b/inc/hal/timer.hpp @@ -17,7 +17,7 @@ namespace hal { * @brief Class to handle timer peripheral on STM32 microcontrollers */ class Timer { - public: +public: /** * @brief Timer configuration struct */ @@ -76,7 +76,7 @@ class Timer { */ void sleep_us(uint32_t time) const; - private: +private: /** * @brief Get the current timer counter * diff --git a/inc/proxy/argb.hpp b/inc/proxy/argb.hpp index 9529d2f..0e88ceb 100644 --- a/inc/proxy/argb.hpp +++ b/inc/proxy/argb.hpp @@ -20,7 +20,7 @@ namespace proxy { */ template class Argb { - public: +public: /** * @brief Configuration structure for the addressable RGB LED */ @@ -71,7 +71,7 @@ class Argb { */ void turn_off(); - private: +private: /** * @brief Encode a color into the data buffer * diff --git a/inc/proxy/battery.hpp b/inc/proxy/battery.hpp index e8ec70c..6e8bfb8 100644 --- a/inc/proxy/battery.hpp +++ b/inc/proxy/battery.hpp @@ -18,7 +18,7 @@ namespace proxy { * @brief Class for getting the battery voltage */ class Battery { - public: +public: /** * @brief Configuration structure for the battery */ @@ -48,7 +48,7 @@ class Battery { */ uint32_t get_voltage_raw() const; - private: +private: /** * @brief Adc object */ diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index 6b95fd9..86f4376 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -19,7 +19,7 @@ namespace proxy { * @brief Class for controlling a button */ class Button { - public: +public: /** * @brief Enum for button status */ @@ -70,7 +70,7 @@ class Button { */ Status get_status(); - private: +private: /** * @brief Get raw button reading * diff --git a/inc/proxy/buzzer.hpp b/inc/proxy/buzzer.hpp index 0f5a6a6..5949068 100644 --- a/inc/proxy/buzzer.hpp +++ b/inc/proxy/buzzer.hpp @@ -19,7 +19,7 @@ namespace proxy { * @brief Class for controlling a buzzer */ class Buzzer { - public: +public: /** * @brief Configuration structure for the buzzer */ @@ -52,7 +52,7 @@ class Buzzer { */ void stop(); - private: +private: /** * @brief PWM object */ diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp index dd8061c..a3a4e2d 100644 --- a/inc/proxy/current_sensors.hpp +++ b/inc/proxy/current_sensors.hpp @@ -20,7 +20,7 @@ namespace proxy { */ template class CurrentSensors { - public: +public: /** * @brief Configuration structure for current sensors */ @@ -52,7 +52,7 @@ class CurrentSensors { */ uint32_t get_current_raw(uint8_t sensor_index) const; - private: +private: /** * @brief ADC DMA handle */ diff --git a/inc/proxy/dip_switch.hpp b/inc/proxy/dip_switch.hpp index 3768cb7..1764072 100644 --- a/inc/proxy/dip_switch.hpp +++ b/inc/proxy/dip_switch.hpp @@ -20,7 +20,7 @@ namespace proxy { */ template class DipSwitch { - public: +public: /** * @brief Configuration struct for DipSwitch */ @@ -50,7 +50,7 @@ class DipSwitch { */ uint8_t get_switches_value() const; - private: +private: /** * @brief Array of GPIOs for the switches */ diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index 21e012b..d46d945 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -21,7 +21,7 @@ namespace proxy { */ template class DistanceSensors { - public: +public: /** * @brief Configuration structure for distance sensors */ @@ -61,7 +61,7 @@ class DistanceSensors { */ uint32_t get_distance_raw(uint8_t sensor_index) const; - private: +private: /** * @brief ADC DMA handle */ diff --git a/inc/proxy/fan.hpp b/inc/proxy/fan.hpp index bb8e395..1897bf2 100644 --- a/inc/proxy/fan.hpp +++ b/inc/proxy/fan.hpp @@ -19,7 +19,7 @@ namespace proxy { * @brief Class for controlling the fan driver */ class Fan { - public: +public: /** * @brief Configuration structure for the fan */ @@ -66,7 +66,7 @@ class Fan { */ void stop(); - private: +private: /** * @brief Set the rotation direction of the fan * diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index 2fb76ef..a7da8bc 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -21,7 +21,7 @@ namespace proxy { * @brief Class to handle IMU peripheral on STM32 microcontrollers */ class Imu { - public: +public: /** * @brief IMU configuration struct */ @@ -83,7 +83,7 @@ class Imu { */ float get_linear_acceleration(Axis axis) const; - private: +private: /** * @brief Read data from the IMU * diff --git a/inc/proxy/led.hpp b/inc/proxy/led.hpp index 937fd5c..092bd0c 100644 --- a/inc/proxy/led.hpp +++ b/inc/proxy/led.hpp @@ -16,7 +16,7 @@ namespace proxy { * @brief Class for controlling an LED */ class Led { - public: +public: /** * @brief Configuration structure for LED */ @@ -46,7 +46,7 @@ class Led { */ void toggle(); - private: +private: /** * @brief Gpio object for the LED */ diff --git a/inc/proxy/locomotion.hpp b/inc/proxy/locomotion.hpp index c682345..3eb8ba9 100644 --- a/inc/proxy/locomotion.hpp +++ b/inc/proxy/locomotion.hpp @@ -19,7 +19,7 @@ namespace proxy { * @brief Class for controlling the locomotion driver */ class Locomotion { - public: +public: /** * @brief Configuration structure for the locomotion */ @@ -79,7 +79,7 @@ class Locomotion { */ void stop_right(); - private: +private: /** * @brief PWM handle for the left motor forward */ diff --git a/inc/proxy/rotary_sensor.hpp b/inc/proxy/rotary_sensor.hpp index 7e60ec2..992629c 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/inc/proxy/rotary_sensor.hpp @@ -20,7 +20,7 @@ namespace proxy { * @brief Class to handle rotary sensor peripheral on STM32 microcontrollers */ class RotarySensor { - public: +public: #include "proxy/rotary_sensor_reg.hpp" /** @@ -48,7 +48,7 @@ class RotarySensor { */ float get_position() const; - private: +private: union CommandFrame { struct Fields { uint8_t do_not_care : 1; diff --git a/inc/proxy/serializable_interface.hpp b/inc/proxy/serializable_interface.hpp index dc147e9..209ebbf 100644 --- a/inc/proxy/serializable_interface.hpp +++ b/inc/proxy/serializable_interface.hpp @@ -16,7 +16,7 @@ * @brief Interface class for serializable classes */ class ISerializable { - public: +public: /** * @brief Virtual destructor for the ISerializable class */ @@ -37,7 +37,7 @@ class ISerializable { */ virtual void deserialize(uint8_t* serial_data, uint16_t size) = 0; - protected: +protected: /** * @brief Special member functions declared as default */ diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp index 0a50cef..7faa45a 100644 --- a/inc/proxy/storage.hpp +++ b/inc/proxy/storage.hpp @@ -25,7 +25,7 @@ concept Fundamental = std::is_fundamental::value; * @brief Class for controlling the storage */ class Storage { - public: +public: /** * @brief Configuration structure for the storage */ @@ -82,7 +82,7 @@ class Storage { */ void save(); - private: +private: /** * @brief Structure for primitive variables */ diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index 1084c7d..8b93a95 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -19,7 +19,7 @@ namespace proxy { */ template class TorqueSensors { - public: +public: /** * @brief Configuration structure for torque sensors */ @@ -51,7 +51,7 @@ class TorqueSensors { */ float get_current(uint8_t sensor_index) const; - private: +private: /** * @brief Current sensors object */ From c664e3981583bf537585b1bb4d6bedae9c7ffe48 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 5 Apr 2024 12:59:21 -0300 Subject: [PATCH 132/561] =?UTF-8?q?=F0=9F=94=A7=20Update=20clang=20format?= =?UTF-8?q?=20base=20on=20style?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-format | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.clang-format b/.clang-format index da34eff..4c488a9 100644 --- a/.clang-format +++ b/.clang-format @@ -1,6 +1,6 @@ --- Language: Cpp -# BasedOnStyle: LLVM +BasedOnStyle: LLVM AccessModifierOffset: -4 AlignAfterOpenBracket: BlockIndent AlignArrayOfStructures: None @@ -71,7 +71,6 @@ EmptyLineAfterAccessModifier: Never EmptyLineBeforeAccessModifier: Always ExperimentalAutoDetectBinPacking: false PackConstructorInitializers: NextLine -BasedOnStyle: '' ConstructorInitializerAllOnOneLineOrOnePerLine: false AllowAllConstructorInitializersOnNextLine: true FixNamespaceComments: true From 819b3169d021915dd8a3877b4b252e0eeb37bd3c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 5 Apr 2024 13:32:33 -0300 Subject: [PATCH 133/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20old=20todo=20in?= =?UTF-8?q?=20imu=20sensor=20fusion?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/imu.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/inc/proxy/imu.hpp b/inc/proxy/imu.hpp index a7da8bc..ff03fe6 100644 --- a/inc/proxy/imu.hpp +++ b/inc/proxy/imu.hpp @@ -57,7 +57,6 @@ class Imu { /** * @brief Get the IMU orientation over an axis - * @todo implement function using sensior fusion * * @param axis Axis to get the orientation from * From 85da6b12cc90ff2a272d343a0c8b8cfea545f420 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 5 Apr 2024 13:33:52 -0300 Subject: [PATCH 134/561] =?UTF-8?q?=E2=9C=A8=20Add=20start=20symbol=20to?= =?UTF-8?q?=20storage?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/storage.hpp | 5 +++++ src/proxy/storage.cpp | 16 ++++++++++------ 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/inc/proxy/storage.hpp b/inc/proxy/storage.hpp index 7faa45a..23208af 100644 --- a/inc/proxy/storage.hpp +++ b/inc/proxy/storage.hpp @@ -124,6 +124,11 @@ class Storage { template static std::unordered_map deserialize_var_map(std::vector& buffer, uint16_t num_vars); + /** + * @brief Start symbol to avoid reading garbage from flash + */ + static constexpr uint16_t start_symbol = 0xABAB; + /** * @brief Map of primitive variables */ diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index 60258c7..4952e7f 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -14,12 +14,16 @@ Storage::Storage(const Config& config) : start_page{config.start_page}, number_o uint64_t sizes{}; hal::Flash::read(this->start_page, 0, &sizes); - uint32_t total_size = sizes >> 32; + if (sizes >> 32 != 0xABAB) { + return; + } + + uint16_t total_size = sizes >> 32; uint16_t num_primitives = sizes >> 16; uint16_t num_serializables = sizes; - this->buffer.resize(total_size); - hal::Flash::read(this->start_page, 1, reinterpret_cast(this->buffer.data()), total_size / 8); + this->buffer.resize(8 * total_size); + hal::Flash::read(this->start_page, 1, reinterpret_cast(this->buffer.data()), total_size); this->primitives = deserialize_var_map(this->buffer, num_primitives); this->serializables = deserialize_var_map(this->buffer, num_serializables); @@ -87,7 +91,7 @@ void Storage::save() { this->buffer.insert(this->buffer.begin(), serialized_primitives.begin(), serialized_primitives.end()); this->buffer.insert(this->buffer.end(), (8 - (this->buffer.size() % 8)) % 8, 0); - uint32_t total_size = this->buffer.size(); + uint16_t total_size = this->buffer.size() / 8; this->buffer.emplace_back(this->serializables.size()); this->buffer.emplace_back(this->serializables.size() >> 8); @@ -96,8 +100,8 @@ void Storage::save() { this->buffer.emplace_back(total_size); this->buffer.emplace_back(total_size >> 8); - this->buffer.emplace_back(total_size >> 16); - this->buffer.emplace_back(total_size >> 24); + this->buffer.emplace_back(start_symbol); + this->buffer.emplace_back(start_symbol >> 8); hal::Flash::write(this->start_page, 0, reinterpret_cast(buffer.data()), buffer.size() / 8); } From adef411323e7e6a34151564962e161062156b212 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 5 Apr 2024 13:36:47 -0300 Subject: [PATCH 135/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20start=20symbol=20c?= =?UTF-8?q?heck=20on=20storage=20header?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/storage.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index 4952e7f..c5a8688 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -11,16 +11,16 @@ namespace proxy { Storage::Storage(const Config& config) : start_page{config.start_page}, number_of_pages{config.number_of_pages} { - uint64_t sizes{}; - hal::Flash::read(this->start_page, 0, &sizes); + uint64_t header{}; + hal::Flash::read(this->start_page, 0, &header); - if (sizes >> 32 != 0xABAB) { + if (header >> 48 != 0xABAB) { return; } - uint16_t total_size = sizes >> 32; - uint16_t num_primitives = sizes >> 16; - uint16_t num_serializables = sizes; + uint16_t total_size = header >> 32; + uint16_t num_primitives = header >> 16; + uint16_t num_serializables = header; this->buffer.resize(8 * total_size); hal::Flash::read(this->start_page, 1, reinterpret_cast(this->buffer.data()), total_size); From 5ce7e7c6a1168055b646ec7a7a0c4bf747930aaf Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 5 Apr 2024 14:26:57 -0300 Subject: [PATCH 136/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20current=5Fsenso?= =?UTF-8?q?rs=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/current_sensors.hpp | 75 ----------------------------------- inc/proxy/torque_sensors.hpp | 22 +++++++--- src/proxy/current_sensors.cpp | 32 --------------- src/proxy/torque_sensors.cpp | 8 ++-- 4 files changed, 22 insertions(+), 115 deletions(-) delete mode 100644 inc/proxy/current_sensors.hpp delete mode 100644 src/proxy/current_sensors.cpp diff --git a/inc/proxy/current_sensors.hpp b/inc/proxy/current_sensors.hpp deleted file mode 100644 index a3a4e2d..0000000 --- a/inc/proxy/current_sensors.hpp +++ /dev/null @@ -1,75 +0,0 @@ -/** - * @file current_sensors.hpp - * - * @brief Proxy CurrentSensors class header - * - * @date 03/2024 - */ - -#ifndef MICRAS_PROXY_CURRENT_SENSORS_HPP -#define MICRAS_PROXY_CURRENT_SENSORS_HPP - -#include -#include - -#include "hal/adc_dma.hpp" - -namespace proxy { -/** - * @brief Class for controlling CurrentSensors - */ -template -class CurrentSensors { -public: - /** - * @brief Configuration structure for current sensors - */ - struct Config { - hal::AdcDma::Config adc; - float shunt_resistor; - }; - - /** - * @brief Constructor for the CurrentSensors class - * - * @param config Configuration for the current sensors - */ - explicit CurrentSensors(const Config& config); - - /** - * @brief Get the current from the sensor - * - * @param sensor_index Index of the sensor - * @return float Current reading from the sensor in amps - */ - float get_current(uint8_t sensor_index) const; - - /** - * @brief Get the raw reading from the current sensor - * - * @param sensor_index Index of the sensor - * @return uint16_t Current reading from the sensor - */ - uint32_t get_current_raw(uint8_t sensor_index) const; - -private: - /** - * @brief ADC DMA handle - */ - hal::AdcDma adc; - - /** - * @brief Buffer to store the ADC values - */ - std::array buffer; - - /** - * @brief Value of the shunt resistor in ohms - */ - const float shunt_resistor; -}; -} // namespace proxy - -#include "../src/proxy/current_sensors.cpp" // NOLINT(bugprone-suspicious-include) - -#endif // MICRAS_PROXY_CURRENT_SENSORS_HPP diff --git a/inc/proxy/torque_sensors.hpp b/inc/proxy/torque_sensors.hpp index 8b93a95..2241f48 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/inc/proxy/torque_sensors.hpp @@ -9,9 +9,10 @@ #ifndef MICRAS_PROXY_TORQUE_SENSORS_HPP #define MICRAS_PROXY_TORQUE_SENSORS_HPP +#include #include -#include "proxy/current_sensors.hpp" +#include "hal/adc_dma.hpp" namespace proxy { /** @@ -24,8 +25,9 @@ class TorqueSensors { * @brief Configuration structure for torque sensors */ struct Config { - typename CurrentSensors::Config current_sensors; - float max_torque; + hal::AdcDma::Config adc; + float shunt_resistor; + float max_torque; }; /** @@ -53,9 +55,19 @@ class TorqueSensors { private: /** - * @brief Current sensors object + * @brief ADC DMA handle */ - CurrentSensors current_sensors; + hal::AdcDma adc; + + /** + * @brief Buffer to store the ADC values + */ + std::array buffer; + + /** + * @brief Value of the shunt resistor in ohms + */ + const float shunt_resistor; /** * @brief Maximum torque that can be measured by the sensor diff --git a/src/proxy/current_sensors.cpp b/src/proxy/current_sensors.cpp deleted file mode 100644 index b151b6f..0000000 --- a/src/proxy/current_sensors.cpp +++ /dev/null @@ -1,32 +0,0 @@ -/** - * @file current_sensors.cpp - * - * @brief Proxy CurrentSensors class implementation - * - * @date 03/2024 - */ - -#ifndef MICRAS_PROXY_CURRENT_SENSORS_CPP -#define MICRAS_PROXY_CURRENT_SENSORS_CPP - -#include "proxy/current_sensors.hpp" - -namespace proxy { -template -CurrentSensors::CurrentSensors(const Config& config) : - adc{config.adc}, shunt_resistor{config.shunt_resistor} { - this->adc.start_dma(this->buffer.data(), num_of_sensors); -} - -template -float CurrentSensors::get_current(uint8_t sensor_index) const { - return this->adc.reference_voltage * this->buffer.at(sensor_index) / (this->adc.max_reading * this->shunt_resistor); -} - -template -uint32_t CurrentSensors::get_current_raw(uint8_t sensor_index) const { - return this->buffer.at(sensor_index); -} -} // namespace proxy - -#endif // MICRAS_PROXY_CURRENT_SENSORS_CPP diff --git a/src/proxy/torque_sensors.cpp b/src/proxy/torque_sensors.cpp index ca0d8ee..2343caa 100644 --- a/src/proxy/torque_sensors.cpp +++ b/src/proxy/torque_sensors.cpp @@ -14,16 +14,18 @@ namespace proxy { template TorqueSensors::TorqueSensors(const Config& config) : - current_sensors{config.current_sensors}, max_torque{config.max_torque} { } + adc{config.adc}, shunt_resistor{config.shunt_resistor}, max_torque{config.max_torque} { + this->adc.start_dma(this->buffer.data(), num_of_sensors); +} template float TorqueSensors::get_torque(uint8_t sensor_index) const { - return this->current_sensors.get_current_raw(sensor_index) * TorqueSensors::max_torque / this->adc.max_reading; + return this->buffer.at(sensor_index) * TorqueSensors::max_torque / this->adc.max_reading; } template float TorqueSensors::get_current(uint8_t sensor_index) const { - return this->current_sensors.get_current(sensor_index); + return this->adc.reference_voltage * this->buffer.at(sensor_index) / (this->adc.max_reading * this->shunt_resistor); } } // namespace proxy From 789a9d67b1e7c2ef5d5a07e4b4be4047035e5775 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 5 Apr 2024 16:26:26 -0300 Subject: [PATCH 137/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20warning=20on=20arg?= =?UTF-8?q?b=20encode=20color?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/argb.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/proxy/argb.cpp b/src/proxy/argb.cpp index 7e73043..1c0de27 100644 --- a/src/proxy/argb.cpp +++ b/src/proxy/argb.cpp @@ -49,7 +49,7 @@ template void Argb::encode_color(const Color& color, uint8_t index) { uint32_t data = color.green << (2 * bits_per_color) | color.red << bits_per_color | color.blue; - for (uint32_t i = bits_per_led * index, j = 0; i < bits_per_led * (index + 1); i++) { + for (uint32_t i = bits_per_led * index, j = 0; i < bits_per_led * (index + 1U); i++) { this->buffer.at(i) = ((data >> j) & 1) == 1 ? this->high_bit : this->low_bit; j++; } From cb962e26ee42f811b10d901d5b2d4b65334c6853 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 6 Apr 2024 03:00:35 -0300 Subject: [PATCH 138/561] =?UTF-8?q?=F0=9F=9A=B8=20Use=20generic=20include?= =?UTF-8?q?=20on=20flash=20hal?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/flash.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/inc/hal/flash.hpp b/inc/hal/flash.hpp index eda7e59..55b1a92 100644 --- a/inc/hal/flash.hpp +++ b/inc/hal/flash.hpp @@ -10,7 +10,7 @@ #define MICRAS_HAL_FLASH_HPP #include -#include +#include namespace hal { /** From 02eb9a5394be8893340680591e7f01b0cc694181 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 6 Apr 2024 04:04:49 -0300 Subject: [PATCH 139/561] =?UTF-8?q?=F0=9F=9A=B8=20Add=20names=20from=20mai?= =?UTF-8?q?n.h=20to=20target.cpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/hal/spi.hpp | 4 ++-- inc/proxy/distance_sensors.hpp | 12 +++++++++--- src/hal/spi.cpp | 6 +++--- src/proxy/distance_sensors.cpp | 8 +++++--- 4 files changed, 19 insertions(+), 11 deletions(-) diff --git a/inc/hal/spi.hpp b/inc/hal/spi.hpp index 8389af5..b7c6d74 100644 --- a/inc/hal/spi.hpp +++ b/inc/hal/spi.hpp @@ -26,7 +26,7 @@ class Spi { struct Config { void (*init_function)(); SPI_HandleTypeDef* handle; - hal::Gpio::Config gpio; + hal::Gpio::Config cs_gpio; uint32_t timeout; }; @@ -74,7 +74,7 @@ class Spi { /** * @brief GPIO for the chip select pin */ - hal::Gpio gpio; + hal::Gpio cs_gpio; /** * @brief Timeout for the SPI operations in ms diff --git a/inc/proxy/distance_sensors.hpp b/inc/proxy/distance_sensors.hpp index d46d945..28d2369 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/inc/proxy/distance_sensors.hpp @@ -27,7 +27,8 @@ class DistanceSensors { */ struct Config { hal::AdcDma::Config adc; - hal::Pwm::Config led_pwm; + hal::Pwm::Config led0_pwm; + hal::Pwm::Config led1_pwm; float max_distance; }; @@ -68,9 +69,14 @@ class DistanceSensors { hal::AdcDma adc; /** - * @brief PWM handle for infrared LED + * @brief PWM handle for the even infrared LEDs */ - hal::Pwm led_pwm; + hal::Pwm led0_pwm; + + /** + * @brief PWM handle for the odd infrared LEDs + */ + hal::Pwm led1_pwm; /** * @brief Buffer to store the ADC values diff --git a/src/hal/spi.cpp b/src/hal/spi.cpp index 0850c8f..e323bfc 100644 --- a/src/hal/spi.cpp +++ b/src/hal/spi.cpp @@ -9,7 +9,7 @@ #include "hal/spi.hpp" namespace hal { -Spi::Spi(const Config& config) : handle{config.handle}, gpio{config.gpio}, timeout{config.timeout} { +Spi::Spi(const Config& config) : handle{config.handle}, cs_gpio{config.cs_gpio}, timeout{config.timeout} { config.init_function(); } @@ -18,12 +18,12 @@ bool Spi::select_device() { return false; } - this->gpio.write(false); + this->cs_gpio.write(false); return true; } void Spi::unselect_device() { - this->gpio.write(true); + this->cs_gpio.write(true); } // NOLINTNEXTLINE(*-avoid-c-arrays) diff --git a/src/proxy/distance_sensors.cpp b/src/proxy/distance_sensors.cpp index c3f6a79..fc41b90 100644 --- a/src/proxy/distance_sensors.cpp +++ b/src/proxy/distance_sensors.cpp @@ -14,14 +14,16 @@ namespace proxy { template DistanceSensors::DistanceSensors(const Config& config) : - adc{config.adc}, led_pwm{config.led_pwm}, max_distance{config.max_distance} { + adc{config.adc}, led0_pwm{config.led0_pwm}, led1_pwm{config.led1_pwm}, max_distance{config.max_distance} { this->adc.start_dma(this->buffer.data(), num_of_sensors); - this->led_pwm.set_duty_cycle(100.0F); + this->led0_pwm.set_duty_cycle(50.0F); + this->led1_pwm.set_duty_cycle(50.0F); } template void DistanceSensors::set_led_intensity(float intensity) { - this->led_pwm.set_duty_cycle(intensity); + this->led0_pwm.set_duty_cycle(intensity); + this->led1_pwm.set_duty_cycle(intensity); } template From ee794a7f6bda5e80602c83468d6aed2fc18ae1ff Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 6 Apr 2024 04:08:10 -0300 Subject: [PATCH 140/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20implicit=20widenin?= =?UTF-8?q?g=20linter=20warning=20on=20storage=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/proxy/storage.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp index c5a8688..80ef85c 100644 --- a/src/proxy/storage.cpp +++ b/src/proxy/storage.cpp @@ -22,7 +22,7 @@ Storage::Storage(const Config& config) : start_page{config.start_page}, number_o uint16_t num_primitives = header >> 16; uint16_t num_serializables = header; - this->buffer.resize(8 * total_size); + this->buffer.resize(8L * total_size); hal::Flash::read(this->start_page, 1, reinterpret_cast(this->buffer.data()), total_size); this->primitives = deserialize_var_map(this->buffer, num_primitives); From 9a179b47f8364f9e1a62ea121d9df77192ae8da4 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 6 Apr 2024 04:56:51 -0300 Subject: [PATCH 141/561] =?UTF-8?q?=E2=9C=85=20Fix=20led=20test?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/proxy/button.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/inc/proxy/button.hpp b/inc/proxy/button.hpp index 86f4376..6b4c8ae 100644 --- a/inc/proxy/button.hpp +++ b/inc/proxy/button.hpp @@ -44,9 +44,9 @@ class Button { struct Config { hal::Gpio::Config gpio{}; PullResistor pull_resistor{}; - uint16_t debounce_delay = 10; - uint16_t long_press_delay = 1000; - uint16_t extra_long_press_delay = 5000; + uint16_t debounce_delay{10}; + uint16_t long_press_delay{500}; + uint16_t extra_long_press_delay{2000}; }; /** From 25fc6204b40752339cc9735957c22ba0c10f67d7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 7 Apr 2024 23:42:11 -0300 Subject: [PATCH 142/561] =?UTF-8?q?=F0=9F=9A=9A=20=F0=9F=8F=97=EF=B8=8F=20?= =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20the=20project?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/CMakeLists.txt | 47 +++++++++++++++++++ .../include/micras}/hal/adc_dma.hpp | 4 +- .../include/micras}/hal/crc.hpp | 4 +- .../include/micras}/hal/encoder.hpp | 4 +- .../include/micras}/hal/flash.hpp | 4 +- .../include/micras}/hal/gpio.hpp | 4 +- .../include/micras}/hal/mcu.hpp | 4 +- .../include/micras}/hal/pwm.hpp | 4 +- .../include/micras}/hal/pwm_dma.hpp | 4 +- .../include/micras}/hal/spi.hpp | 6 +-- .../include/micras}/hal/timer.hpp | 4 +- {src/hal => micras_hal/stm32/src}/adc_dma.cpp | 6 +-- {src/hal => micras_hal/stm32/src}/crc.cpp | 6 +-- {src/hal => micras_hal/stm32/src}/encoder.cpp | 6 +-- {src/hal => micras_hal/stm32/src}/flash.cpp | 6 +-- {src/hal => micras_hal/stm32/src}/gpio.cpp | 6 +-- {src/hal => micras_hal/stm32/src}/mcu.cpp | 6 +-- {src/hal => micras_hal/stm32/src}/pwm.cpp | 6 +-- {src/hal => micras_hal/stm32/src}/pwm_dma.cpp | 6 +-- {src/hal => micras_hal/stm32/src}/spi.cpp | 6 +-- {src/hal => micras_hal/stm32/src}/timer.cpp | 6 +-- micras_proxy/CMakeLists.txt | 28 +++++++++++ .../include/micras}/proxy/argb.hpp | 8 ++-- .../include/micras}/proxy/battery.hpp | 6 +-- .../include/micras}/proxy/button.hpp | 8 ++-- .../include/micras}/proxy/buzzer.hpp | 8 ++-- .../include/micras}/proxy/dip_switch.hpp | 8 ++-- .../micras}/proxy/distance_sensors.hpp | 10 ++-- .../include/micras}/proxy/fan.hpp | 8 ++-- .../include/micras}/proxy/imu.hpp | 6 +-- .../include/micras}/proxy/led.hpp | 6 +-- .../include/micras}/proxy/locomotion.hpp | 8 ++-- .../include/micras}/proxy/rotary_sensor.hpp | 12 ++--- .../micras}/proxy/rotary_sensor_reg.hpp | 0 .../micras}/proxy/serializable_interface.hpp | 2 + .../include/micras}/proxy/storage.hpp | 4 +- .../include/micras}/proxy/torque_sensors.hpp | 8 ++-- {src/proxy => micras_proxy/src}/argb.cpp | 6 +-- {src/proxy => micras_proxy/src}/battery.cpp | 6 +-- {src/proxy => micras_proxy/src}/button.cpp | 6 +-- {src/proxy => micras_proxy/src}/buzzer.cpp | 6 +-- .../proxy => micras_proxy/src}/dip_switch.cpp | 8 ++-- .../src}/distance_sensors.cpp | 6 +-- {src/proxy => micras_proxy/src}/fan.cpp | 6 +-- {src/proxy => micras_proxy/src}/imu.cpp | 8 ++-- {src/proxy => micras_proxy/src}/led.cpp | 6 +-- .../proxy => micras_proxy/src}/locomotion.cpp | 6 +-- .../src}/rotary_sensor.cpp | 6 +-- {src/proxy => micras_proxy/src}/storage.cpp | 8 ++-- .../src}/torque_sensors.cpp | 6 +-- 50 files changed, 220 insertions(+), 143 deletions(-) create mode 100644 micras_hal/CMakeLists.txt rename {inc => micras_hal/include/micras}/hal/adc_dma.hpp (96%) rename {inc => micras_hal/include/micras}/hal/crc.hpp (94%) rename {inc => micras_hal/include/micras}/hal/encoder.hpp (94%) rename {inc => micras_hal/include/micras}/hal/flash.hpp (98%) rename {inc => micras_hal/include/micras}/hal/gpio.hpp (95%) rename {inc => micras_hal/include/micras}/hal/mcu.hpp (89%) rename {inc => micras_hal/include/micras}/hal/pwm.hpp (95%) rename {inc => micras_hal/include/micras}/hal/pwm_dma.hpp (96%) rename {inc => micras_hal/include/micras}/hal/spi.hpp (95%) rename {inc => micras_hal/include/micras}/hal/timer.hpp (97%) rename {src/hal => micras_hal/stm32/src}/adc_dma.cpp (84%) rename {src/hal => micras_hal/stm32/src}/crc.cpp (78%) rename {src/hal => micras_hal/stm32/src}/encoder.cpp (80%) rename {src/hal => micras_hal/stm32/src}/flash.cpp (96%) rename {src/hal => micras_hal/stm32/src}/gpio.cpp (84%) rename {src/hal => micras_hal/stm32/src}/mcu.cpp (80%) rename {src/hal => micras_hal/stm32/src}/pwm.cpp (91%) rename {src/hal => micras_hal/stm32/src}/pwm_dma.cpp (90%) rename {src/hal => micras_hal/stm32/src}/spi.cpp (90%) rename {src/hal => micras_hal/stm32/src}/timer.cpp (93%) create mode 100644 micras_proxy/CMakeLists.txt rename {inc => micras_proxy/include/micras}/proxy/argb.hpp (94%) rename {inc => micras_proxy/include/micras}/proxy/battery.hpp (93%) rename {inc => micras_proxy/include/micras}/proxy/button.hpp (95%) rename {inc => micras_proxy/include/micras}/proxy/buzzer.hpp (91%) rename {inc => micras_proxy/include/micras}/proxy/dip_switch.hpp (88%) rename {inc => micras_proxy/include/micras}/proxy/distance_sensors.hpp (90%) rename {inc => micras_proxy/include/micras}/proxy/fan.hpp (93%) rename {inc => micras_proxy/include/micras}/proxy/imu.hpp (98%) rename {inc => micras_proxy/include/micras}/proxy/led.hpp (90%) rename {inc => micras_proxy/include/micras}/proxy/locomotion.hpp (94%) rename {inc => micras_proxy/include/micras}/proxy/rotary_sensor.hpp (91%) rename {inc => micras_proxy/include/micras}/proxy/rotary_sensor_reg.hpp (100%) rename {inc => micras_proxy/include/micras}/proxy/serializable_interface.hpp (95%) rename {inc => micras_proxy/include/micras}/proxy/storage.hpp (98%) rename {inc => micras_proxy/include/micras}/proxy/torque_sensors.hpp (90%) rename {src/proxy => micras_proxy/src}/argb.cpp (94%) rename {src/proxy => micras_proxy/src}/battery.cpp (83%) rename {src/proxy => micras_proxy/src}/button.cpp (95%) rename {src/proxy => micras_proxy/src}/buzzer.cpp (88%) rename {src/proxy => micras_proxy/src}/dip_switch.cpp (88%) rename {src/proxy => micras_proxy/src}/distance_sensors.cpp (92%) rename {src/proxy => micras_proxy/src}/fan.cpp (90%) rename {src/proxy => micras_proxy/src}/imu.cpp (98%) rename {src/proxy => micras_proxy/src}/led.cpp (77%) rename {src/proxy => micras_proxy/src}/locomotion.cpp (95%) rename {src/proxy => micras_proxy/src}/rotary_sensor.cpp (95%) rename {src/proxy => micras_proxy/src}/storage.cpp (97%) rename {src/proxy => micras_proxy/src}/torque_sensors.cpp (90%) diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt new file mode 100644 index 0000000..6042a58 --- /dev/null +++ b/micras_hal/CMakeLists.txt @@ -0,0 +1,47 @@ +project(micras_hal) + +get_target_property(CUBE_INCLUDE_DIRECTORIES stm32cubemx INTERFACE_INCLUDE_DIRECTORIES) +get_target_property(CUBE_SOURCES stm32cubemx INTERFACE_SOURCES) +get_target_property(CUBE_COMPILE_DEFINITIONS stm32cubemx INTERFACE_COMPILE_DEFINITIONS) + +# Remove warnings from Cube headers +target_include_directories(stm32cubemx SYSTEM INTERFACE + ${CUBE_INCLUDE_DIRECTORIES} +) + +# Remove warnings from Cube sources +set_source_files_properties( + ${CUBE_SOURCES} + PROPERTIES + COMPILE_FLAGS "-w" +) + +file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") + +add_library(${PROJECT_NAME} STATIC) + +target_include_directories(${PROJECT_NAME} PUBLIC + include +) + +target_sources(${PROJECT_NAME} PRIVATE + ${PROJECT_SOURCES} +) + +target_link_libraries(${PROJECT_NAME} PRIVATE + stm32cubemx +) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + ${CUBE_INCLUDE_DIRECTORIES} +) + +target_compile_definitions(${PROJECT_NAME} PUBLIC + ${CUBE_COMPILE_DEFINITIONS} +) + +add_library(micras_hal_whole_archive INTERFACE) +add_library(micras::hal ALIAS micras_hal_whole_archive) +target_link_libraries(micras_hal_whole_archive INTERFACE + "-Wl,--whole-archive" ${PROJECT_NAME} "-Wl,--no-whole-archive" +) diff --git a/inc/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp similarity index 96% rename from inc/hal/adc_dma.hpp rename to micras_hal/include/micras/hal/adc_dma.hpp index 814c9a6..4e481bc 100644 --- a/inc/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -12,7 +12,7 @@ #include #include -namespace hal { +namespace micras::hal { /** * @brief Class to handle ADC peripheral on STM32 microcontrollers using DMA */ @@ -64,6 +64,6 @@ class AdcDma { */ ADC_HandleTypeDef* handle; }; -} // namespace hal +} // namespace micras::hal #endif // MICRAS_HAL_ADC_DMA_HPP diff --git a/inc/hal/crc.hpp b/micras_hal/include/micras/hal/crc.hpp similarity index 94% rename from inc/hal/crc.hpp rename to micras_hal/include/micras/hal/crc.hpp index 9a666b6..bc1e6cd 100644 --- a/inc/hal/crc.hpp +++ b/micras_hal/include/micras/hal/crc.hpp @@ -11,7 +11,7 @@ #include -namespace hal { +namespace micras::hal { /** * @brief Class to handle the cyclic redundancy check peripheral on STM32 microcontrollers */ @@ -47,6 +47,6 @@ class Crc { */ CRC_HandleTypeDef* handle; }; -} // namespace hal +} // namespace micras::hal #endif // MICRAS_HAL_CRC_HPP diff --git a/inc/hal/encoder.hpp b/micras_hal/include/micras/hal/encoder.hpp similarity index 94% rename from inc/hal/encoder.hpp rename to micras_hal/include/micras/hal/encoder.hpp index 684b83e..8664b43 100644 --- a/inc/hal/encoder.hpp +++ b/micras_hal/include/micras/hal/encoder.hpp @@ -11,7 +11,7 @@ #include -namespace hal { +namespace micras::hal { /** * @brief Class to handle encoder peripheral on STM32 microcontrollers */ @@ -46,6 +46,6 @@ class Encoder { */ TIM_HandleTypeDef* handle; }; -} // namespace hal +} // namespace micras::hal #endif // MICRAS_HAL_ENCODER_HPP diff --git a/inc/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp similarity index 98% rename from inc/hal/flash.hpp rename to micras_hal/include/micras/hal/flash.hpp index 55b1a92..9fac40f 100644 --- a/inc/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -12,7 +12,7 @@ #include #include -namespace hal { +namespace micras::hal { /** * @brief Class to handle flash memory on STM32 microcontrollers */ @@ -89,6 +89,6 @@ class Flash { */ static constexpr uint32_t double_words_per_page{FLASH_PAGE_SIZE / 8}; }; -} // namespace hal +} // namespace micras::hal #endif // MICRAS_HAL_FLASH_HPP diff --git a/inc/hal/gpio.hpp b/micras_hal/include/micras/hal/gpio.hpp similarity index 95% rename from inc/hal/gpio.hpp rename to micras_hal/include/micras/hal/gpio.hpp index 633601a..bd350a8 100644 --- a/inc/hal/gpio.hpp +++ b/micras_hal/include/micras/hal/gpio.hpp @@ -12,7 +12,7 @@ #include #include -namespace hal { +namespace micras::hal { /** * @brief Class for controlling GPIO pins on STM32 microcontrollers */ @@ -63,6 +63,6 @@ class Gpio { */ uint16_t pin; }; -} // namespace hal +} // namespace micras::hal #endif // MICRAS_HAL_GPIO_HPP diff --git a/inc/hal/mcu.hpp b/micras_hal/include/micras/hal/mcu.hpp similarity index 89% rename from inc/hal/mcu.hpp rename to micras_hal/include/micras/hal/mcu.hpp index b947ff1..38e6e1f 100644 --- a/inc/hal/mcu.hpp +++ b/micras_hal/include/micras/hal/mcu.hpp @@ -11,7 +11,7 @@ #include -namespace hal { +namespace micras::hal { /** * @brief Microcontroller unit class */ @@ -27,6 +27,6 @@ class Mcu { */ static void init(); }; -} // namespace hal +} // namespace micras::hal #endif // MICRAS_HAL_MCU_HPP diff --git a/inc/hal/pwm.hpp b/micras_hal/include/micras/hal/pwm.hpp similarity index 95% rename from inc/hal/pwm.hpp rename to micras_hal/include/micras/hal/pwm.hpp index fe933bb..fed77a2 100644 --- a/inc/hal/pwm.hpp +++ b/micras_hal/include/micras/hal/pwm.hpp @@ -12,7 +12,7 @@ #include #include -namespace hal { +namespace micras::hal { /** * @brief Class to handle PWM peripheral on STM32 microcontrollers */ @@ -59,6 +59,6 @@ class Pwm { */ uint32_t channel; }; -} // namespace hal +} // namespace micras::hal #endif // MICRAS_HAL_PWM_HPP diff --git a/inc/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp similarity index 96% rename from inc/hal/pwm_dma.hpp rename to micras_hal/include/micras/hal/pwm_dma.hpp index 3241b60..8c55fab 100644 --- a/inc/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -12,7 +12,7 @@ #include #include -namespace hal { +namespace micras::hal { /** * @brief Class to handle PWM peripheral on STM32 microcontrollers using DMA */ @@ -66,6 +66,6 @@ class PwmDma { */ uint32_t channel; }; -} // namespace hal +} // namespace micras::hal #endif // MICRAS_HAL_PWM_DMA_HPP diff --git a/inc/hal/spi.hpp b/micras_hal/include/micras/hal/spi.hpp similarity index 95% rename from inc/hal/spi.hpp rename to micras_hal/include/micras/hal/spi.hpp index b7c6d74..4524044 100644 --- a/inc/hal/spi.hpp +++ b/micras_hal/include/micras/hal/spi.hpp @@ -12,9 +12,9 @@ #include #include -#include "hal/gpio.hpp" +#include "micras/hal/gpio.hpp" -namespace hal { +namespace micras::hal { /** * @brief Class to handle SPI peripheral on STM32 microcontrollers */ @@ -81,6 +81,6 @@ class Spi { */ uint32_t timeout; }; -} // namespace hal +} // namespace micras::hal #endif // MICRAS_HAL_SPI_HPP diff --git a/inc/hal/timer.hpp b/micras_hal/include/micras/hal/timer.hpp similarity index 97% rename from inc/hal/timer.hpp rename to micras_hal/include/micras/hal/timer.hpp index 88fbce3..1ab6929 100644 --- a/inc/hal/timer.hpp +++ b/micras_hal/include/micras/hal/timer.hpp @@ -12,7 +12,7 @@ #include #include -namespace hal { +namespace micras::hal { /** * @brief Class to handle timer peripheral on STM32 microcontrollers */ @@ -106,6 +106,6 @@ class Timer { */ bool enable_microseconds{false}; }; -} // namespace hal +} // namespace micras::hal #endif // MICRAS_HAL_TIMER_HPP diff --git a/src/hal/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp similarity index 84% rename from src/hal/adc_dma.cpp rename to micras_hal/stm32/src/adc_dma.cpp index 7d063c4..8472e0f 100644 --- a/src/hal/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "hal/adc_dma.hpp" +#include "micras/hal/adc_dma.hpp" -namespace hal { +namespace micras::hal { AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { config.init_function(); @@ -22,4 +22,4 @@ void AdcDma::start_dma(uint32_t buffer[], uint32_t size) { void AdcDma::stop_dma() { HAL_ADC_Stop_DMA(this->handle); } -} // namespace hal +} // namespace micras::hal diff --git a/src/hal/crc.cpp b/micras_hal/stm32/src/crc.cpp similarity index 78% rename from src/hal/crc.cpp rename to micras_hal/stm32/src/crc.cpp index ba97330..0d32810 100644 --- a/src/hal/crc.cpp +++ b/micras_hal/stm32/src/crc.cpp @@ -6,13 +6,13 @@ * @date 03/2024 */ -#include "hal/crc.hpp" +#include "micras/hal/crc.hpp" -namespace hal { +namespace micras::hal { Crc::Crc(const Config& config) : handle{config.handle} { } // NOLINTNEXTLINE(*-avoid-c-arrays) uint32_t Crc::calculate(uint32_t data[], uint32_t size) { return HAL_CRC_Calculate(this->handle, data, size); } -} // namespace hal +} // namespace micras::hal diff --git a/src/hal/encoder.cpp b/micras_hal/stm32/src/encoder.cpp similarity index 80% rename from src/hal/encoder.cpp rename to micras_hal/stm32/src/encoder.cpp index a2d627c..dd1d0c8 100644 --- a/src/hal/encoder.cpp +++ b/micras_hal/stm32/src/encoder.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "hal/encoder.hpp" +#include "micras/hal/encoder.hpp" -namespace hal { +namespace micras::hal { Encoder::Encoder(const Config& config) : handle{config.handle} { config.init_function(); HAL_TIM_Encoder_Start(this->handle, config.timer_channel); @@ -17,4 +17,4 @@ Encoder::Encoder(const Config& config) : handle{config.handle} { int32_t Encoder::get_counter() const { return static_cast(__HAL_TIM_GET_COUNTER(this->handle)); } -} // namespace hal +} // namespace micras::hal diff --git a/src/hal/flash.cpp b/micras_hal/stm32/src/flash.cpp similarity index 96% rename from src/hal/flash.cpp rename to micras_hal/stm32/src/flash.cpp index 61504a0..3a1785a 100644 --- a/src/hal/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "hal/flash.hpp" +#include "micras/hal/flash.hpp" -namespace hal { +namespace micras::hal { /** * @brief Last address of the flash memory * needs to be here because it is not defined at compile time @@ -71,4 +71,4 @@ void Flash::erase_pages(uint16_t page, uint16_t number_of_pages) { HAL_FLASHEx_Erase(&erase_struct, &page_error); } -} // namespace hal +} // namespace micras::hal diff --git a/src/hal/gpio.cpp b/micras_hal/stm32/src/gpio.cpp similarity index 84% rename from src/hal/gpio.cpp rename to micras_hal/stm32/src/gpio.cpp index 1cc60f8..900e924 100644 --- a/src/hal/gpio.cpp +++ b/micras_hal/stm32/src/gpio.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "hal/gpio.hpp" +#include "micras/hal/gpio.hpp" -namespace hal { +namespace micras::hal { Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { } bool Gpio::read() const { @@ -22,4 +22,4 @@ void Gpio::write(bool state) { void Gpio::toggle() { HAL_GPIO_TogglePin(this->port, this->pin); } -} // namespace hal +} // namespace micras::hal diff --git a/src/hal/mcu.cpp b/micras_hal/stm32/src/mcu.cpp similarity index 80% rename from src/hal/mcu.cpp rename to micras_hal/stm32/src/mcu.cpp index f0731f9..90751f3 100644 --- a/src/hal/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -8,7 +8,7 @@ #include #include -#include "hal/mcu.hpp" +#include "micras/hal/mcu.hpp" extern "C" { /** @@ -18,7 +18,7 @@ extern "C" { void SystemClock_Config(); } -namespace hal { +namespace micras::hal { void Mcu::init() { HAL_Init(); @@ -28,4 +28,4 @@ void Mcu::init() { MX_DMA_Init(); MX_CRC_Init(); } -} // namespace hal +} // namespace micras::hal diff --git a/src/hal/pwm.cpp b/micras_hal/stm32/src/pwm.cpp similarity index 91% rename from src/hal/pwm.cpp rename to micras_hal/stm32/src/pwm.cpp index dda3da7..3fd27ee 100644 --- a/src/hal/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -8,9 +8,9 @@ #include -#include "hal/pwm.hpp" +#include "micras/hal/pwm.hpp" -namespace hal { +namespace micras::hal { Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel} { config.init_function(); HAL_TIM_PWM_Start(this->handle, this->channel); @@ -29,4 +29,4 @@ void Pwm::set_frequency(uint32_t frequency) { uint32_t autoreload = base_freq / ((prescaler + 1) * frequency) - 1; __HAL_TIM_SET_AUTORELOAD(this->handle, autoreload); } -} // namespace hal +} // namespace micras::hal diff --git a/src/hal/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp similarity index 90% rename from src/hal/pwm_dma.cpp rename to micras_hal/stm32/src/pwm_dma.cpp index ef84d46..d32436a 100644 --- a/src/hal/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -8,9 +8,9 @@ #include -#include "hal/pwm_dma.hpp" +#include "micras/hal/pwm_dma.hpp" -namespace hal { +namespace micras::hal { PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.timer_channel} { config.init_function(); } @@ -31,4 +31,4 @@ void PwmDma::stop_dma() { uint32_t PwmDma::get_compare(float duty_cycle) const { return std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); } -} // namespace hal +} // namespace micras::hal diff --git a/src/hal/spi.cpp b/micras_hal/stm32/src/spi.cpp similarity index 90% rename from src/hal/spi.cpp rename to micras_hal/stm32/src/spi.cpp index e323bfc..0077087 100644 --- a/src/hal/spi.cpp +++ b/micras_hal/stm32/src/spi.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "hal/spi.hpp" +#include "micras/hal/spi.hpp" -namespace hal { +namespace micras::hal { Spi::Spi(const Config& config) : handle{config.handle}, cs_gpio{config.cs_gpio}, timeout{config.timeout} { config.init_function(); } @@ -35,4 +35,4 @@ void Spi::transmit(uint8_t data[], uint32_t size) { void Spi::receive(uint8_t data[], uint32_t size) { HAL_SPI_Receive(this->handle, data, size, this->timeout); } -} // namespace hal +} // namespace micras::hal diff --git a/src/hal/timer.cpp b/micras_hal/stm32/src/timer.cpp similarity index 93% rename from src/hal/timer.cpp rename to micras_hal/stm32/src/timer.cpp index 0e480eb..0a24623 100644 --- a/src/hal/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "hal/timer.hpp" +#include "micras/hal/timer.hpp" -namespace hal { +namespace micras::hal { Timer::Timer(const Config& config) : handle{config.handle} { config.init_function(); @@ -59,4 +59,4 @@ uint32_t Timer::get_counter_us() const { return 1000 * HAL_GetTick(); } -} // namespace hal +} // namespace micras::hal diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt new file mode 100644 index 0000000..5b1455e --- /dev/null +++ b/micras_proxy/CMakeLists.txt @@ -0,0 +1,28 @@ +project(micras_proxy) + +file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") +file(GLOB_RECURSE PROJECT_LIB_SOURCES CONFIGURE_DEPENDS "lib/*.c*") + +# Remove warnings from lib sources +set_source_files_properties( + ${PROJECT_LIB_SOURCES} + PROPERTIES + COMPILE_FLAGS "-w" +) + +add_library(${PROJECT_NAME} STATIC) +add_library(micras::proxy ALIAS ${PROJECT_NAME}) + +target_include_directories(${PROJECT_NAME} PUBLIC + include + lib/lsm6dsv-pid +) + +target_sources(${PROJECT_NAME} PRIVATE + ${PROJECT_SOURCES} + ${PROJECT_LIB_SOURCES} +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + micras::hal +) diff --git a/inc/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp similarity index 94% rename from inc/proxy/argb.hpp rename to micras_proxy/include/micras/proxy/argb.hpp index 0e88ceb..68317f0 100644 --- a/inc/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -12,9 +12,9 @@ #include #include -#include "hal/pwm_dma.hpp" +#include "micras/hal/pwm_dma.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class for controlling an addressable RGB LED */ @@ -110,8 +110,8 @@ class Argb { */ std::array buffer; }; -} // namespace proxy +} // namespace micras::proxy -#include "../src/proxy/argb.cpp" // NOLINT(bugprone-suspicious-include) +#include "../src/argb.cpp" // NOLINT(bugprone-suspicious-include) #endif // MICRAS_PROXY_ARGB_HPP diff --git a/inc/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp similarity index 93% rename from inc/proxy/battery.hpp rename to micras_proxy/include/micras/proxy/battery.hpp index 6e8bfb8..f4ddc3f 100644 --- a/inc/proxy/battery.hpp +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -11,9 +11,9 @@ #include -#include "hal/adc_dma.hpp" +#include "micras/hal/adc_dma.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class for getting the battery voltage */ @@ -64,6 +64,6 @@ class Battery { */ const float voltage_divider; }; -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_BATTERY_HPP diff --git a/inc/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp similarity index 95% rename from inc/proxy/button.hpp rename to micras_proxy/include/micras/proxy/button.hpp index 6b4c8ae..9337a64 100644 --- a/inc/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -11,10 +11,10 @@ #include -#include "hal/gpio.hpp" -#include "hal/timer.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/timer.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class for controlling a button */ @@ -139,6 +139,6 @@ class Button { */ bool current_state = false; }; -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_BUTTON_HPP diff --git a/inc/proxy/buzzer.hpp b/micras_proxy/include/micras/proxy/buzzer.hpp similarity index 91% rename from inc/proxy/buzzer.hpp rename to micras_proxy/include/micras/proxy/buzzer.hpp index 5949068..76cca8c 100644 --- a/inc/proxy/buzzer.hpp +++ b/micras_proxy/include/micras/proxy/buzzer.hpp @@ -11,10 +11,10 @@ #include -#include "hal/pwm.hpp" -#include "hal/timer.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/timer.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class for controlling a buzzer */ @@ -73,6 +73,6 @@ class Buzzer { */ uint32_t duration{0}; }; -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_BUZZER_HPP diff --git a/inc/proxy/dip_switch.hpp b/micras_proxy/include/micras/proxy/dip_switch.hpp similarity index 88% rename from inc/proxy/dip_switch.hpp rename to micras_proxy/include/micras/proxy/dip_switch.hpp index 1764072..c22f93f 100644 --- a/inc/proxy/dip_switch.hpp +++ b/micras_proxy/include/micras/proxy/dip_switch.hpp @@ -12,9 +12,9 @@ #include #include -#include "hal/gpio.hpp" +#include "micras/hal/gpio.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class for controlling a dip switch */ @@ -56,8 +56,8 @@ class DipSwitch { */ std::array gpio_array; }; -} // namespace proxy +} // namespace micras::proxy -#include "../src/proxy/dip_switch.cpp" // NOLINT(bugprone-suspicious-include) +#include "../src/dip_switch.cpp" // NOLINT(bugprone-suspicious-include) #endif // MICRAS_PROXY_DIP_SWITCH_HPP diff --git a/inc/proxy/distance_sensors.hpp b/micras_proxy/include/micras/proxy/distance_sensors.hpp similarity index 90% rename from inc/proxy/distance_sensors.hpp rename to micras_proxy/include/micras/proxy/distance_sensors.hpp index 28d2369..c77127e 100644 --- a/inc/proxy/distance_sensors.hpp +++ b/micras_proxy/include/micras/proxy/distance_sensors.hpp @@ -12,10 +12,10 @@ #include #include -#include "hal/adc_dma.hpp" -#include "hal/pwm.hpp" +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/pwm.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class for controlling DistanceSensors */ @@ -88,8 +88,8 @@ class DistanceSensors { */ const float max_distance; }; -} // namespace proxy +} // namespace micras::proxy -#include "../src/proxy/distance_sensors.cpp" // NOLINT(bugprone-suspicious-include) +#include "../src/distance_sensors.cpp" // NOLINT(bugprone-suspicious-include) #endif // MICRAS_PROXY_DISTANCE_SENSORS_HPP diff --git a/inc/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp similarity index 93% rename from inc/proxy/fan.hpp rename to micras_proxy/include/micras/proxy/fan.hpp index 1897bf2..884b46e 100644 --- a/inc/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -11,10 +11,10 @@ #include -#include "hal/gpio.hpp" -#include "hal/pwm.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/pwm.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class for controlling the fan driver */ @@ -89,6 +89,6 @@ class Fan { */ hal::Gpio enable_gpio; }; -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_FAN_HPP diff --git a/inc/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp similarity index 98% rename from inc/proxy/imu.hpp rename to micras_proxy/include/micras/proxy/imu.hpp index ff03fe6..96047e5 100644 --- a/inc/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -14,9 +14,9 @@ #include #include -#include "hal/spi.hpp" +#include "micras/hal/spi.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class to handle IMU peripheral on STM32 microcontrollers */ @@ -165,6 +165,6 @@ class Imu { */ const float xl_factor; }; -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_IMU_HPP diff --git a/inc/proxy/led.hpp b/micras_proxy/include/micras/proxy/led.hpp similarity index 90% rename from inc/proxy/led.hpp rename to micras_proxy/include/micras/proxy/led.hpp index 092bd0c..03c3664 100644 --- a/inc/proxy/led.hpp +++ b/micras_proxy/include/micras/proxy/led.hpp @@ -9,9 +9,9 @@ #ifndef MICRAS_PROXY_LED_HPP #define MICRAS_PROXY_LED_HPP -#include "hal/gpio.hpp" +#include "micras/hal/gpio.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class for controlling an LED */ @@ -52,6 +52,6 @@ class Led { */ hal::Gpio gpio; }; -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_LED_HPP diff --git a/inc/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp similarity index 94% rename from inc/proxy/locomotion.hpp rename to micras_proxy/include/micras/proxy/locomotion.hpp index 3eb8ba9..777f202 100644 --- a/inc/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -11,10 +11,10 @@ #include -#include "hal/gpio.hpp" -#include "hal/pwm.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/pwm.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class for controlling the locomotion driver */ @@ -105,6 +105,6 @@ class Locomotion { */ hal::Gpio enable_gpio; }; -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_LOCOMOTION_HPP diff --git a/inc/proxy/rotary_sensor.hpp b/micras_proxy/include/micras/proxy/rotary_sensor.hpp similarity index 91% rename from inc/proxy/rotary_sensor.hpp rename to micras_proxy/include/micras/proxy/rotary_sensor.hpp index 992629c..4b56aa7 100644 --- a/inc/proxy/rotary_sensor.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor.hpp @@ -11,17 +11,17 @@ #include -#include "hal/crc.hpp" -#include "hal/encoder.hpp" -#include "hal/spi.hpp" +#include "micras/hal/crc.hpp" +#include "micras/hal/encoder.hpp" +#include "micras/hal/spi.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class to handle rotary sensor peripheral on STM32 microcontrollers */ class RotarySensor { public: -#include "proxy/rotary_sensor_reg.hpp" +#include "micras/proxy/rotary_sensor_reg.hpp" /** * @brief Rotary sensor configuration struct @@ -101,6 +101,6 @@ class RotarySensor { */ uint32_t resolution; }; -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_ROTARY_SENSOR_HPP diff --git a/inc/proxy/rotary_sensor_reg.hpp b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp similarity index 100% rename from inc/proxy/rotary_sensor_reg.hpp rename to micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp diff --git a/inc/proxy/serializable_interface.hpp b/micras_proxy/include/micras/proxy/serializable_interface.hpp similarity index 95% rename from inc/proxy/serializable_interface.hpp rename to micras_proxy/include/micras/proxy/serializable_interface.hpp index 209ebbf..ddc5152 100644 --- a/inc/proxy/serializable_interface.hpp +++ b/micras_proxy/include/micras/proxy/serializable_interface.hpp @@ -12,6 +12,7 @@ #include #include +namespace micras::proxy { /** * @brief Interface class for serializable classes */ @@ -49,5 +50,6 @@ class ISerializable { ISerializable& operator=(ISerializable&&) = default; }; +} // namespace micras::proxy #endif // MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP diff --git a/inc/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp similarity index 98% rename from inc/proxy/storage.hpp rename to micras_proxy/include/micras/proxy/storage.hpp index 23208af..10e17c6 100644 --- a/inc/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -17,7 +17,7 @@ #include "serializable_interface.hpp" -namespace proxy { +namespace micras::proxy { template concept Fundamental = std::is_fundamental::value; @@ -154,6 +154,6 @@ class Storage { */ uint16_t number_of_pages; }; -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_STORAGE_HPP diff --git a/inc/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp similarity index 90% rename from inc/proxy/torque_sensors.hpp rename to micras_proxy/include/micras/proxy/torque_sensors.hpp index 2241f48..90e2082 100644 --- a/inc/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -12,9 +12,9 @@ #include #include -#include "hal/adc_dma.hpp" +#include "micras/hal/adc_dma.hpp" -namespace proxy { +namespace micras::proxy { /** * @brief Class for controlling TorqueSensors */ @@ -74,8 +74,8 @@ class TorqueSensors { */ const float max_torque; }; -} // namespace proxy +} // namespace micras::proxy -#include "../src/proxy/torque_sensors.cpp" // NOLINT(bugprone-suspicious-include) +#include "../src/torque_sensors.cpp" // NOLINT(bugprone-suspicious-include) #endif // MICRAS_PROXY_TORQUE_SENSORS_HPP diff --git a/src/proxy/argb.cpp b/micras_proxy/src/argb.cpp similarity index 94% rename from src/proxy/argb.cpp rename to micras_proxy/src/argb.cpp index 1c0de27..a7cd3e2 100644 --- a/src/proxy/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -9,9 +9,9 @@ #ifndef MICRAS_PROXY_ARGB_CPP #define MICRAS_PROXY_ARGB_CPP -#include "proxy/argb.hpp" +#include "micras/proxy/argb.hpp" -namespace proxy { +namespace micras::proxy { template Argb::Argb(const Config& config) : pwm{config.pwm}, low_bit{pwm.get_compare(low_duty_cycle)}, high_bit{pwm.get_compare(high_duty_cycle)} { } @@ -54,6 +54,6 @@ void Argb::encode_color(const Color& color, uint8_t index) { j++; } } -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_ARGB_CPP diff --git a/src/proxy/battery.cpp b/micras_proxy/src/battery.cpp similarity index 83% rename from src/proxy/battery.cpp rename to micras_proxy/src/battery.cpp index 3eac595..06baca2 100644 --- a/src/proxy/battery.cpp +++ b/micras_proxy/src/battery.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "proxy/battery.hpp" +#include "micras/proxy/battery.hpp" -namespace proxy { +namespace micras::proxy { Battery::Battery(const Config& config) : adc{config.adc}, voltage_divider{config.voltage_divider} { this->adc.start_dma(&(this->raw_reading), 1); } @@ -20,4 +20,4 @@ float Battery::get_voltage() const { uint32_t Battery::get_voltage_raw() const { return this->raw_reading; } -} // namespace proxy +} // namespace micras::proxy diff --git a/src/proxy/button.cpp b/micras_proxy/src/button.cpp similarity index 95% rename from src/proxy/button.cpp rename to micras_proxy/src/button.cpp index c64b2ac..4638f5c 100644 --- a/src/proxy/button.cpp +++ b/micras_proxy/src/button.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "proxy/button.hpp" +#include "micras/proxy/button.hpp" -namespace proxy { +namespace micras::proxy { Button::Button(const Config& config) : debounce_delay{config.debounce_delay}, long_press_delay{config.long_press_delay}, @@ -69,4 +69,4 @@ bool Button::is_rising_edge() const { bool Button::is_falling_edge() const { return not this->current_state and this->previous_state; } -} // namespace proxy +} // namespace micras::proxy diff --git a/src/proxy/buzzer.cpp b/micras_proxy/src/buzzer.cpp similarity index 88% rename from src/proxy/buzzer.cpp rename to micras_proxy/src/buzzer.cpp index eb968e4..190cf67 100644 --- a/src/proxy/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "proxy/buzzer.hpp" +#include "micras/proxy/buzzer.hpp" -namespace proxy { +namespace micras::proxy { Buzzer::Buzzer(const Config& config) : pwm{config.pwm} { this->stop(); } @@ -34,4 +34,4 @@ void Buzzer::stop() { this->is_playing = false; this->pwm.set_duty_cycle(0.0F); } -} // namespace proxy +} // namespace micras::proxy diff --git a/src/proxy/dip_switch.cpp b/micras_proxy/src/dip_switch.cpp similarity index 88% rename from src/proxy/dip_switch.cpp rename to micras_proxy/src/dip_switch.cpp index 94040c5..55679fb 100644 --- a/src/proxy/dip_switch.cpp +++ b/micras_proxy/src/dip_switch.cpp @@ -9,10 +9,10 @@ #ifndef MICRAS_PROXY_DIP_SWITCH_CPP #define MICRAS_PROXY_DIP_SWITCH_CPP -#include "hal/mcu.hpp" -#include "proxy/dip_switch.hpp" +#include "micras/hal/mcu.hpp" +#include "micras/proxy/dip_switch.hpp" -namespace proxy { +namespace micras::proxy { template DipSwitch::DipSwitch(const Config& config) : gpio_array([&](std::index_sequence)->std::array { @@ -34,6 +34,6 @@ uint8_t DipSwitch::get_switches_value() const { return switches_value; } -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_DIP_SWITCH_CPP diff --git a/src/proxy/distance_sensors.cpp b/micras_proxy/src/distance_sensors.cpp similarity index 92% rename from src/proxy/distance_sensors.cpp rename to micras_proxy/src/distance_sensors.cpp index fc41b90..3b799ba 100644 --- a/src/proxy/distance_sensors.cpp +++ b/micras_proxy/src/distance_sensors.cpp @@ -9,9 +9,9 @@ #ifndef MICRAS_PROXY_DISTANCE_SENSORS_CPP #define MICRAS_PROXY_DISTANCE_SENSORS_CPP -#include "proxy/distance_sensors.hpp" +#include "micras/proxy/distance_sensors.hpp" -namespace proxy { +namespace micras::proxy { template DistanceSensors::DistanceSensors(const Config& config) : adc{config.adc}, led0_pwm{config.led0_pwm}, led1_pwm{config.led1_pwm}, max_distance{config.max_distance} { @@ -35,6 +35,6 @@ template uint32_t DistanceSensors::get_distance_raw(uint8_t sensor_index) const { return this->buffer.at(sensor_index); } -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_DISTANCE_SENSORS_CPP diff --git a/src/proxy/fan.cpp b/micras_proxy/src/fan.cpp similarity index 90% rename from src/proxy/fan.cpp rename to micras_proxy/src/fan.cpp index 9f00b8c..b8a044c 100644 --- a/src/proxy/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "proxy/fan.hpp" +#include "micras/proxy/fan.hpp" -namespace proxy { +namespace micras::proxy { Fan::Fan(const Config& config) : pwm{config.pwm}, direction_gpio{config.direction_gpio}, enable_gpio{config.enable_gpio} { this->stop(); @@ -42,4 +42,4 @@ void Fan::stop() { void Fan::set_direction(RotationDirection direction) { this->direction_gpio.write(static_cast(direction)); } -} // namespace proxy +} // namespace micras::proxy diff --git a/src/proxy/imu.cpp b/micras_proxy/src/imu.cpp similarity index 98% rename from src/proxy/imu.cpp rename to micras_proxy/src/imu.cpp index 524877f..4b467d9 100644 --- a/src/proxy/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -8,10 +8,10 @@ #include -#include "hal/timer.hpp" -#include "proxy/imu.hpp" +#include "micras/hal/timer.hpp" +#include "micras/proxy/imu.hpp" -namespace proxy { +namespace micras::proxy { Imu::Imu(const Config& config) : spi{config.spi}, gy_factor{ @@ -216,4 +216,4 @@ float Imu::half_to_float(uint16_t x) { } // NOLINTEND(readability-identifier-length, readability-implicit-bool-conversion) -} // namespace proxy +} // namespace micras::proxy diff --git a/src/proxy/led.cpp b/micras_proxy/src/led.cpp similarity index 77% rename from src/proxy/led.cpp rename to micras_proxy/src/led.cpp index 6e73259..a40d2ae 100644 --- a/src/proxy/led.cpp +++ b/micras_proxy/src/led.cpp @@ -6,9 +6,9 @@ * @date 03/2024 */ -#include "proxy/led.hpp" +#include "micras/proxy/led.hpp" -namespace proxy { +namespace micras::proxy { Led::Led(const Config& config) : gpio{config.gpio} { } void Led::turn_on() { @@ -22,4 +22,4 @@ void Led::turn_off() { void Led::toggle() { this->gpio.toggle(); } -} // namespace proxy +} // namespace micras::proxy diff --git a/src/proxy/locomotion.cpp b/micras_proxy/src/locomotion.cpp similarity index 95% rename from src/proxy/locomotion.cpp rename to micras_proxy/src/locomotion.cpp index 3b26eb3..8700255 100644 --- a/src/proxy/locomotion.cpp +++ b/micras_proxy/src/locomotion.cpp @@ -8,9 +8,9 @@ #include -#include "proxy/locomotion.hpp" +#include "micras/proxy/locomotion.hpp" -namespace proxy { +namespace micras::proxy { Locomotion::Locomotion(const Config& config) : pwm_left_fwd{config.pwm_left_fwd}, pwm_left_bwd{config.pwm_left_bwd}, @@ -82,4 +82,4 @@ void Locomotion::stop_right() { this->pwm_right_fwd.set_duty_cycle(0.0F); this->pwm_right_bwd.set_duty_cycle(0.0F); } -} // namespace proxy +} // namespace micras::proxy diff --git a/src/proxy/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp similarity index 95% rename from src/proxy/rotary_sensor.cpp rename to micras_proxy/src/rotary_sensor.cpp index 96ae50e..dc1390c 100644 --- a/src/proxy/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -8,9 +8,9 @@ #include -#include "proxy/rotary_sensor.hpp" +#include "micras/proxy/rotary_sensor.hpp" -namespace proxy { +namespace micras::proxy { RotarySensor::RotarySensor(const Config& config) : spi{config.spi}, encoder{config.encoder}, crc{config.crc}, resolution{config.resolution} { CommandFrame command_frame{}; @@ -59,4 +59,4 @@ void RotarySensor::write_register(CommandFrame& command_frame, DataFrame& data_f this->spi.transmit(reinterpret_cast(&data_frame.raw), 3); this->spi.unselect_device(); } -} // namespace proxy +} // namespace micras::proxy diff --git a/src/proxy/storage.cpp b/micras_proxy/src/storage.cpp similarity index 97% rename from src/proxy/storage.cpp rename to micras_proxy/src/storage.cpp index 80ef85c..61d565c 100644 --- a/src/proxy/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -6,10 +6,10 @@ * @date 03/2024 */ -#include "hal/flash.hpp" -#include "proxy/storage.hpp" +#include "micras/hal/flash.hpp" +#include "micras/proxy/storage.hpp" -namespace proxy { +namespace micras::proxy { Storage::Storage(const Config& config) : start_page{config.start_page}, number_of_pages{config.number_of_pages} { uint64_t header{}; hal::Flash::read(this->start_page, 0, &header); @@ -146,4 +146,4 @@ std::unordered_map Storage::deserialize_var_map(std::vector TorqueSensors::TorqueSensors(const Config& config) : adc{config.adc}, shunt_resistor{config.shunt_resistor}, max_torque{config.max_torque} { @@ -27,6 +27,6 @@ template float TorqueSensors::get_current(uint8_t sensor_index) const { return this->adc.reference_voltage * this->buffer.at(sensor_index) / (this->adc.max_reading * this->shunt_resistor); } -} // namespace proxy +} // namespace micras::proxy #endif // MICRAS_PROXY_TORQUE_SENSORS_CPP From 95b5f05498b90d424ba0fc89a71927d3840d1694 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Mon, 8 Apr 2024 00:05:37 -0300 Subject: [PATCH 143/561] =?UTF-8?q?=E2=9E=95=20Add=20submodule=20for=20lsm?= =?UTF-8?q?6dsv-pid=20library?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/lib/lsm6dsv-pid | 1 + 1 file changed, 1 insertion(+) create mode 160000 micras_proxy/lib/lsm6dsv-pid diff --git a/micras_proxy/lib/lsm6dsv-pid b/micras_proxy/lib/lsm6dsv-pid new file mode 160000 index 0000000..1208320 --- /dev/null +++ b/micras_proxy/lib/lsm6dsv-pid @@ -0,0 +1 @@ +Subproject commit 120832044d340c5a2f59e05861375f8aea4d8804 From 686c3646b145070f447e08e11c973ffd1d98355b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 8 Apr 2024 00:20:55 -0300 Subject: [PATCH 144/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20target=20source?= =?UTF-8?q?s=20call=20from=20CMake?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/CMakeLists.txt | 13 +++---------- micras_proxy/CMakeLists.txt | 11 +++++------ 2 files changed, 8 insertions(+), 16 deletions(-) diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt index 6042a58..aead3da 100644 --- a/micras_hal/CMakeLists.txt +++ b/micras_hal/CMakeLists.txt @@ -4,11 +4,6 @@ get_target_property(CUBE_INCLUDE_DIRECTORIES stm32cubemx INTERFACE_INCLUDE_DIREC get_target_property(CUBE_SOURCES stm32cubemx INTERFACE_SOURCES) get_target_property(CUBE_COMPILE_DEFINITIONS stm32cubemx INTERFACE_COMPILE_DEFINITIONS) -# Remove warnings from Cube headers -target_include_directories(stm32cubemx SYSTEM INTERFACE - ${CUBE_INCLUDE_DIRECTORIES} -) - # Remove warnings from Cube sources set_source_files_properties( ${CUBE_SOURCES} @@ -18,16 +13,14 @@ set_source_files_properties( file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") -add_library(${PROJECT_NAME} STATIC) +add_library(${PROJECT_NAME} STATIC + ${PROJECT_SOURCES} +) target_include_directories(${PROJECT_NAME} PUBLIC include ) -target_sources(${PROJECT_NAME} PRIVATE - ${PROJECT_SOURCES} -) - target_link_libraries(${PROJECT_NAME} PRIVATE stm32cubemx ) diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt index 5b1455e..13aa38e 100644 --- a/micras_proxy/CMakeLists.txt +++ b/micras_proxy/CMakeLists.txt @@ -10,7 +10,11 @@ set_source_files_properties( COMPILE_FLAGS "-w" ) -add_library(${PROJECT_NAME} STATIC) +add_library(${PROJECT_NAME} STATIC + ${PROJECT_SOURCES} + ${PROJECT_LIB_SOURCES} +) + add_library(micras::proxy ALIAS ${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} PUBLIC @@ -18,11 +22,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC lib/lsm6dsv-pid ) -target_sources(${PROJECT_NAME} PRIVATE - ${PROJECT_SOURCES} - ${PROJECT_LIB_SOURCES} -) - target_link_libraries(${PROJECT_NAME} PUBLIC micras::hal ) From 6ca533018ce3d243a9089ddcbfd2bd6a7da280ae Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 8 Apr 2024 20:54:35 -0300 Subject: [PATCH 145/561] =?UTF-8?q?=F0=9F=90=9B=20Add=20start=20count=20to?= =?UTF-8?q?=20encoder?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/encoder.hpp | 5 +++++ micras_hal/stm32/src/encoder.cpp | 4 +++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/micras_hal/include/micras/hal/encoder.hpp b/micras_hal/include/micras/hal/encoder.hpp index 8664b43..8e01ec4 100644 --- a/micras_hal/include/micras/hal/encoder.hpp +++ b/micras_hal/include/micras/hal/encoder.hpp @@ -45,6 +45,11 @@ class Encoder { * @brief Timer handle */ TIM_HandleTypeDef* handle; + + /** + * @brief Start value of the timer counter + */ + uint32_t start_count; }; } // namespace micras::hal diff --git a/micras_hal/stm32/src/encoder.cpp b/micras_hal/stm32/src/encoder.cpp index dd1d0c8..d4ebd5a 100644 --- a/micras_hal/stm32/src/encoder.cpp +++ b/micras_hal/stm32/src/encoder.cpp @@ -12,9 +12,11 @@ namespace micras::hal { Encoder::Encoder(const Config& config) : handle{config.handle} { config.init_function(); HAL_TIM_Encoder_Start(this->handle, config.timer_channel); + this->start_count = __HAL_TIM_GET_AUTORELOAD(this->handle) / 2; + __HAL_TIM_SET_COUNTER(this->handle, this->start_count); } int32_t Encoder::get_counter() const { - return static_cast(__HAL_TIM_GET_COUNTER(this->handle)); + return static_cast(__HAL_TIM_GET_COUNTER(this->handle)) - this->start_count; } } // namespace micras::hal From 9c15933ba32a20a7128d0439cec2af26d074639e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 9 Apr 2024 00:27:49 -0300 Subject: [PATCH 146/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20ARGB=20duty=20cycl?= =?UTF-8?q?e?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/argb.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index 68317f0..fcab999 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -86,8 +86,8 @@ class Argb { static constexpr uint8_t bits_per_color{8}; static constexpr uint8_t colors_per_led{3}; static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; - static constexpr float low_duty_cycle{0.32F}; // NOLINT(bugprone-dynamic-static-initializers) - static constexpr float high_duty_cycle{0.64F}; // NOLINT(bugprone-dynamic-static-initializers) + static constexpr float low_duty_cycle{32.0F}; // NOLINT(bugprone-dynamic-static-initializers) + static constexpr float high_duty_cycle{64.0F}; // NOLINT(bugprone-dynamic-static-initializers) static constexpr uint8_t reset_length{40}; /** From 1af624a7288de6491632c1cce721a1837d5646db Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 9 Apr 2024 03:06:56 -0300 Subject: [PATCH 147/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20addressable=20RGB?= =?UTF-8?q?=20flickering?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/pwm_dma.hpp | 7 +++++++ micras_hal/stm32/src/pwm_dma.cpp | 20 ++++++++++++++++++++ micras_proxy/include/micras/proxy/argb.hpp | 2 +- micras_proxy/src/argb.cpp | 9 ++++----- 4 files changed, 32 insertions(+), 6 deletions(-) diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp index 8c55fab..8f98e95 100644 --- a/micras_hal/include/micras/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -56,6 +56,13 @@ class PwmDma { uint32_t get_compare(float duty_cycle) const; private: + /** + * @brief Callback for stopping the dma when the pulse has finished + * + * @param htim Timer handle + */ + static void pulse_finished_callback(TIM_HandleTypeDef* htim); + /** * @brief Timer handle */ diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index d32436a..3075a10 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -13,6 +13,7 @@ namespace micras::hal { PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.timer_channel} { config.init_function(); + HAL_TIM_RegisterCallback(this->handle, HAL_TIM_PWM_PULSE_FINISHED_CB_ID, pulse_finished_callback); } // NOLINTNEXTLINE(*-avoid-c-arrays) @@ -31,4 +32,23 @@ void PwmDma::stop_dma() { uint32_t PwmDma::get_compare(float duty_cycle) const { return std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); } + +void PwmDma::pulse_finished_callback(TIM_HandleTypeDef* htim) { + switch (HAL_TIM_GetActiveChannel(htim)) { + case HAL_TIM_ACTIVE_CHANNEL_1: + HAL_TIM_PWM_Stop_DMA(htim, TIM_CHANNEL_1); + break; + case HAL_TIM_ACTIVE_CHANNEL_2: + HAL_TIM_PWM_Stop_DMA(htim, TIM_CHANNEL_2); + break; + case HAL_TIM_ACTIVE_CHANNEL_3: + HAL_TIM_PWM_Stop_DMA(htim, TIM_CHANNEL_3); + break; + case HAL_TIM_ACTIVE_CHANNEL_4: + HAL_TIM_PWM_Stop_DMA(htim, TIM_CHANNEL_4); + break; + default: + break; + } +} } // namespace micras::hal diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index fcab999..1ea99a2 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -108,7 +108,7 @@ class Argb { /** * @brief Data buffer to send to the addressable RGB LED */ - std::array buffer; + std::array buffer{}; }; } // namespace micras::proxy diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index a7cd3e2..beaa07c 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -23,7 +23,7 @@ void Argb::set_color(const Color& color, uint8_t index) { } this->encode_color(color, index); - this->pwm.start_dma(this->buffer.data(), this->buffer.size()); + this->pwm.start_dma(reinterpret_cast(this->buffer.data()), this->buffer.size()); } template @@ -32,7 +32,7 @@ void Argb::set_color(const Color& color) { this->encode_color(color, i); } - this->pwm.start_dma(this->buffer.data(), this->buffer.size()); + this->pwm.start_dma(reinterpret_cast(this->buffer.data()), this->buffer.size()); } template @@ -47,11 +47,10 @@ void Argb::turn_off() { template void Argb::encode_color(const Color& color, uint8_t index) { - uint32_t data = color.green << (2 * bits_per_color) | color.red << bits_per_color | color.blue; + uint32_t data = (color.green << (2 * bits_per_color)) | (color.red << bits_per_color) | color.blue; - for (uint32_t i = bits_per_led * index, j = 0; i < bits_per_led * (index + 1U); i++) { + for (uint32_t i = bits_per_led * index, j = bits_per_led - 1; i < bits_per_led * (index + 1U); i++, j--) { this->buffer.at(i) = ((data >> j) & 1) == 1 ? this->high_bit : this->low_bit; - j++; } } } // namespace micras::proxy From 79e6f76a579dbd5ee959b76a47daf103d340d8e8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 9 Apr 2024 15:24:56 -0300 Subject: [PATCH 148/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20register=20call?= =?UTF-8?q?back=20from=20pwm=20dma?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/pwm_dma.hpp | 7 ------- micras_hal/stm32/src/encoder.cpp | 2 ++ micras_hal/stm32/src/pwm_dma.cpp | 20 -------------------- 3 files changed, 2 insertions(+), 27 deletions(-) diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp index 8f98e95..8c55fab 100644 --- a/micras_hal/include/micras/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -56,13 +56,6 @@ class PwmDma { uint32_t get_compare(float duty_cycle) const; private: - /** - * @brief Callback for stopping the dma when the pulse has finished - * - * @param htim Timer handle - */ - static void pulse_finished_callback(TIM_HandleTypeDef* htim); - /** * @brief Timer handle */ diff --git a/micras_hal/stm32/src/encoder.cpp b/micras_hal/stm32/src/encoder.cpp index d4ebd5a..813de58 100644 --- a/micras_hal/stm32/src/encoder.cpp +++ b/micras_hal/stm32/src/encoder.cpp @@ -12,6 +12,8 @@ namespace micras::hal { Encoder::Encoder(const Config& config) : handle{config.handle} { config.init_function(); HAL_TIM_Encoder_Start(this->handle, config.timer_channel); + + // NOLINTNEXTLINE(cppcoreguidelines-prefer-member-initializer) this->start_count = __HAL_TIM_GET_AUTORELOAD(this->handle) / 2; __HAL_TIM_SET_COUNTER(this->handle, this->start_count); } diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index 3075a10..d32436a 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -13,7 +13,6 @@ namespace micras::hal { PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.timer_channel} { config.init_function(); - HAL_TIM_RegisterCallback(this->handle, HAL_TIM_PWM_PULSE_FINISHED_CB_ID, pulse_finished_callback); } // NOLINTNEXTLINE(*-avoid-c-arrays) @@ -32,23 +31,4 @@ void PwmDma::stop_dma() { uint32_t PwmDma::get_compare(float duty_cycle) const { return std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); } - -void PwmDma::pulse_finished_callback(TIM_HandleTypeDef* htim) { - switch (HAL_TIM_GetActiveChannel(htim)) { - case HAL_TIM_ACTIVE_CHANNEL_1: - HAL_TIM_PWM_Stop_DMA(htim, TIM_CHANNEL_1); - break; - case HAL_TIM_ACTIVE_CHANNEL_2: - HAL_TIM_PWM_Stop_DMA(htim, TIM_CHANNEL_2); - break; - case HAL_TIM_ACTIVE_CHANNEL_3: - HAL_TIM_PWM_Stop_DMA(htim, TIM_CHANNEL_3); - break; - case HAL_TIM_ACTIVE_CHANNEL_4: - HAL_TIM_PWM_Stop_DMA(htim, TIM_CHANNEL_4); - break; - default: - break; - } -} } // namespace micras::hal From 172dbdee01866e83fbac0b3eb207fe86eb1e88aa Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 9 Apr 2024 16:18:52 -0300 Subject: [PATCH 149/561] =?UTF-8?q?=E2=9C=A8=20Add=20ADC=20calibration=20t?= =?UTF-8?q?o=20AdcDma=20constructor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/adc_dma.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 8472e0f..d11b9a6 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -12,6 +12,7 @@ namespace micras::hal { AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { config.init_function(); + HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); } // NOLINTNEXTLINE(*-avoid-c-arrays) From 85108b549d43482ea22629492acded4f5dac5fa5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 9 Apr 2024 22:19:55 -0300 Subject: [PATCH 150/561] =?UTF-8?q?=F0=9F=90=9B=20Remove=20target=20from?= =?UTF-8?q?=20precompile=20headers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.clang-tidy b/.clang-tidy index ce2c114..ea5d4ca 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -24,7 +24,7 @@ Checks: -readability-duplicate-include, -readability-magic-numbers, WarningsAsErrors: '' -HeaderFilterRegex: 'inc/.*.hpp' +HeaderFilterRegex: '(include|config)/.*.hpp' AnalyzeTemporaryDtors: false FormatStyle: none ... From d820cd921fbdb20867e2c362d7c78d2b99ff2caf Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 9 Apr 2024 22:21:19 -0300 Subject: [PATCH 151/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20adc=20and=20dma=20?= =?UTF-8?q?buffer=20sizes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/adc_dma.hpp | 8 ++++++++ micras_hal/include/micras/hal/pwm_dma.hpp | 8 ++++++++ micras_hal/stm32/src/adc_dma.cpp | 5 +++++ micras_hal/stm32/src/pwm_dma.cpp | 5 +++++ micras_proxy/include/micras/proxy/battery.hpp | 2 +- micras_proxy/include/micras/proxy/distance_sensors.hpp | 2 +- micras_proxy/include/micras/proxy/torque_sensors.hpp | 2 +- micras_proxy/src/argb.cpp | 4 ++-- 8 files changed, 31 insertions(+), 5 deletions(-) diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 4e481bc..d3e19ea 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -43,6 +43,14 @@ class AdcDma { */ void start_dma(uint32_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) + /** + * @brief Enable ADC, start conversion of regular group and transfer result through DMA + * + * @param buffer Destination Buffer address + * @param size Number of data to be transferred from ADC DMA peripheral to memory + */ + void start_dma(uint16_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) + /** * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode) */ diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp index 8c55fab..ae4bac2 100644 --- a/micras_hal/include/micras/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -42,6 +42,14 @@ class PwmDma { */ void start_dma(uint32_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) + /** + * @brief Start PWM and DMA transfer + * + * @param buffer Buffer to transfer + * @param size Size of the buffer + */ + void start_dma(uint16_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) + /** * @brief Stop PWM and DMA transfer */ diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index d11b9a6..762011e 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -20,6 +20,11 @@ void AdcDma::start_dma(uint32_t buffer[], uint32_t size) { HAL_ADC_Start_DMA(this->handle, buffer, size); } +// NOLINTNEXTLINE(*-avoid-c-arrays) +void AdcDma::start_dma(uint16_t buffer[], uint32_t size) { + this->start_dma(reinterpret_cast(buffer), size); +} + void AdcDma::stop_dma() { HAL_ADC_Stop_DMA(this->handle); } diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index d32436a..24ae303 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -24,6 +24,11 @@ void PwmDma::start_dma(uint32_t buffer[], uint32_t size) { HAL_TIM_PWM_Start_DMA(this->handle, this->channel, buffer, size); } +// NOLINTNEXTLINE(*-avoid-c-arrays) +void PwmDma::start_dma(uint16_t buffer[], uint32_t size) { + start_dma(reinterpret_cast(buffer), size); +} + void PwmDma::stop_dma() { HAL_TIM_PWM_Stop_DMA(this->handle, this->channel); } diff --git a/micras_proxy/include/micras/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp index f4ddc3f..c2c235d 100644 --- a/micras_proxy/include/micras/proxy/battery.hpp +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -57,7 +57,7 @@ class Battery { /** * @brief Raw reading from the battery */ - uint32_t raw_reading{}; + uint16_t raw_reading{}; /** * @brief Voltage divider ratio diff --git a/micras_proxy/include/micras/proxy/distance_sensors.hpp b/micras_proxy/include/micras/proxy/distance_sensors.hpp index c77127e..92a3711 100644 --- a/micras_proxy/include/micras/proxy/distance_sensors.hpp +++ b/micras_proxy/include/micras/proxy/distance_sensors.hpp @@ -81,7 +81,7 @@ class DistanceSensors { /** * @brief Buffer to store the ADC values */ - std::array buffer; + std::array buffer; /** * @brief Maximum distance reading in meters diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 90e2082..33d9290 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -62,7 +62,7 @@ class TorqueSensors { /** * @brief Buffer to store the ADC values */ - std::array buffer; + std::array buffer; /** * @brief Value of the shunt resistor in ohms diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index beaa07c..b5847b3 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -23,7 +23,7 @@ void Argb::set_color(const Color& color, uint8_t index) { } this->encode_color(color, index); - this->pwm.start_dma(reinterpret_cast(this->buffer.data()), this->buffer.size()); + this->pwm.start_dma(this->buffer.data(), this->buffer.size()); } template @@ -32,7 +32,7 @@ void Argb::set_color(const Color& color) { this->encode_color(color, i); } - this->pwm.start_dma(reinterpret_cast(this->buffer.data()), this->buffer.size()); + this->pwm.start_dma(this->buffer.data(), this->buffer.size()); } template From 1ac9582f7cdf3d767e1a04fb5c43cce700a85989 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 9 Apr 2024 22:54:53 -0300 Subject: [PATCH 152/561] =?UTF-8?q?=E2=9C=A8=20Add=20wait=20function=20to?= =?UTF-8?q?=20buzzer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/buzzer.hpp | 7 +++++++ micras_proxy/src/buzzer.cpp | 9 +++++++++ 2 files changed, 16 insertions(+) diff --git a/micras_proxy/include/micras/proxy/buzzer.hpp b/micras_proxy/include/micras/proxy/buzzer.hpp index 76cca8c..03068da 100644 --- a/micras_proxy/include/micras/proxy/buzzer.hpp +++ b/micras_proxy/include/micras/proxy/buzzer.hpp @@ -52,6 +52,13 @@ class Buzzer { */ void stop(); + /** + * @brief Wait for a duration updating the buzzer + * + * @param duration Duration to wait in ms + */ + void wait(uint32_t duration); + private: /** * @brief PWM object diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp index 190cf67..88ab23f 100644 --- a/micras_proxy/src/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -30,6 +30,15 @@ void Buzzer::update() { } } +void Buzzer::wait(uint32_t duration) { + hal::Timer wait_timer; + wait_timer.reset_ms(); + + while (wait_timer.elapsed_time_ms() < duration) { + this->update(); + } +} + void Buzzer::stop() { this->is_playing = false; this->pwm.set_duty_cycle(0.0F); From 9dbb246dab1e62180b3fc616a8fd78205104f22f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 9 Apr 2024 23:46:38 -0300 Subject: [PATCH 153/561] =?UTF-8?q?=E2=9C=A8=20Change=20orientation=20unit?= =?UTF-8?q?=20to=20euler=20angles?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/imu.hpp | 4 +--- micras_proxy/src/imu.cpp | 26 +++++++++++++++++------ 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 96047e5..769eef1 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -44,7 +44,6 @@ class Imu { explicit Imu(const Config& config); enum Axis : uint8_t { - W, X, Y, Z @@ -110,10 +109,9 @@ class Imu { /** * @brief Function to convert raw data to orientation * - * @param quat Quaternion to store the orientation * @param sflp Raw data from the IMU */ - static void convert_orientation(std::array& quat, const uint16_t sflp[3]); // NOLINT(*-avoid-c-arrays) + void update_orientation(const uint16_t sflp[3]); // NOLINT(*-avoid-c-arrays) /** * @brief Function to convert half precision float to single precision float diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 4b467d9..7c00bb9 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -87,7 +87,7 @@ void Imu::update_data() { break; case lsm6dsv_fifo_out_raw_t::LSM6DSV_SFLP_GAME_ROTATION_VECTOR_TAG: - convert_orientation(this->orientation, reinterpret_cast(axis)); + this->update_orientation(reinterpret_cast(axis)); break; default: @@ -98,9 +98,6 @@ void Imu::update_data() { float Imu::get_orientation(Axis axis) const { switch (axis) { - case Axis::W: - return this->orientation[3]; - case Axis::X: return this->orientation[0]; @@ -173,13 +170,15 @@ int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint } // NOLINTNEXTLINE(*-avoid-c-arrays) -void Imu::convert_orientation(std::array& quat, const uint16_t sflp[3]) { - float sumsq = 0; +void Imu::update_orientation(const uint16_t sflp[3]) { + std::array quat{}; quat[0] = half_to_float(sflp[0]); quat[1] = half_to_float(sflp[1]); quat[2] = half_to_float(sflp[2]); + float sumsq = 0; + for (uint8_t i = 0; i < 3; i++) { sumsq += quat[i] * quat[i]; // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index) } @@ -193,6 +192,21 @@ void Imu::convert_orientation(std::array& quat, const uint16_t sflp[3] } quat[3] = std::sqrt(1.0F - sumsq); + + // roll (x-axis rotation) + float sinp = 2 * (quat[3] * quat[0] + quat[1] * quat[2]); + float cosp = 1 - 2 * (quat[0] * quat[0] + quat[1] * quat[1]); + this->orientation[0] = std::atan2(sinp, cosp); + + // pitch (y-axis rotation) + sinp = std::sqrt(1 + 2 * (quat[3] * quat[1] - quat[0] * quat[2])); + cosp = std::sqrt(1 - 2 * (quat[3] * quat[1] - quat[0] * quat[2])); + this->orientation[1] = 2 * std::atan2(sinp, cosp) - M_PI / 2; + + // yaw (z-axis rotation) + sinp = 2 * (quat[3] * quat[2] + quat[0] * quat[1]); + cosp = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]); + this->orientation[2] = std::atan2(sinp, cosp); } // NOLINTBEGIN(readability-identifier-length, readability-implicit-bool-conversion) From 7628bfc5037354ebe79653a4ccf6f2dee75e5e0b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 10 Apr 2024 02:14:30 -0300 Subject: [PATCH 154/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20pwm=20compare=20ro?= =?UTF-8?q?unding?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/pwm.cpp | 2 +- micras_hal/stm32/src/pwm_dma.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index 3fd27ee..0445f72 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -18,7 +18,7 @@ Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_cha } void Pwm::set_duty_cycle(float duty_cycle) { - uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); + uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100) - 1); __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); } diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index 24ae303..d875fe9 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -34,6 +34,6 @@ void PwmDma::stop_dma() { } uint32_t PwmDma::get_compare(float duty_cycle) const { - return std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1)) / 100 - 1); + return std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100) - 1); } } // namespace micras::hal From f570c5b28a6209939dae809b695056782c5daa1a Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 10 Apr 2024 19:36:43 -0300 Subject: [PATCH 155/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20button=20is=5Fpres?= =?UTF-8?q?sed=20bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/button.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp index 4638f5c..3c00a0b 100644 --- a/micras_proxy/src/button.cpp +++ b/micras_proxy/src/button.cpp @@ -17,8 +17,7 @@ Button::Button(const Config& config) : pull_resistor{config.pull_resistor} { } bool Button::is_pressed() { - this->update_state(); - return this->current_state; + return this->get_raw_reading(); } Button::Status Button::get_status() { From 6fe5fea86df7d44e070f5a4d1c44221e41d17473 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 10 Apr 2024 22:07:17 -0300 Subject: [PATCH 156/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Add=20debounce=20t?= =?UTF-8?q?o=20button=20is=5Fpressed?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/button.hpp | 4 +++- micras_proxy/src/button.cpp | 13 +++++++------ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index 9337a64..6f9e22e 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -80,8 +80,10 @@ class Button { /** * @brief Update button state + * + * @return Current state of the button */ - void update_state(); + bool update_state(); /** * @brief Check if button was just pressed diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp index 3c00a0b..30d6c46 100644 --- a/micras_proxy/src/button.cpp +++ b/micras_proxy/src/button.cpp @@ -17,11 +17,12 @@ Button::Button(const Config& config) : pull_resistor{config.pull_resistor} { } bool Button::is_pressed() { - return this->get_raw_reading(); + return this->update_state(); } Button::Status Button::get_status() { - this->update_state(); + this->previous_state = this->current_state; + this->current_state = this->update_state(); if (this->is_rising_edge()) { this->status_timer.reset_ms(); @@ -44,7 +45,7 @@ bool Button::get_raw_reading() const { return this->gpio.read() == static_cast(this->pull_resistor); } -void Button::update_state() { +bool Button::update_state() { bool raw_reading = this->get_raw_reading(); if ((raw_reading != this->current_state) and not this->is_debouncing) { @@ -55,10 +56,10 @@ void Button::update_state() { this->is_debouncing = false; } } else { - this->is_debouncing = false; - this->previous_state = this->current_state; - this->current_state = raw_reading; + return raw_reading; } + + return this->current_state; } bool Button::is_rising_edge() const { From f058dc3a07e45704a4695732497ef061a952c1c9 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 11 Apr 2024 15:29:10 -0300 Subject: [PATCH 157/561] =?UTF-8?q?=F0=9F=92=A1=20Improve=20class=20commen?= =?UTF-8?q?ts=20in=20proxies?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/button.hpp | 8 ++++---- micras_proxy/include/micras/proxy/dip_switch.hpp | 2 +- micras_proxy/include/micras/proxy/torque_sensors.hpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index 6f9e22e..6cc6808 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -16,7 +16,7 @@ namespace micras::proxy { /** - * @brief Class for controlling a button + * @brief Class for receiving a button data */ class Button { public: @@ -129,17 +129,17 @@ class Button { /** * @brief Flag to know when button is debouncing */ - bool is_debouncing = false; + bool is_debouncing{false}; /** * @brief Flag to know if button was being pressed */ - bool previous_state = false; + bool previous_state{false}; /** * @brief Flag to know if button is being pressed */ - bool current_state = false; + bool current_state{false}; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/dip_switch.hpp b/micras_proxy/include/micras/proxy/dip_switch.hpp index c22f93f..9164795 100644 --- a/micras_proxy/include/micras/proxy/dip_switch.hpp +++ b/micras_proxy/include/micras/proxy/dip_switch.hpp @@ -16,7 +16,7 @@ namespace micras::proxy { /** - * @brief Class for controlling a dip switch + * @brief Class for receiving a dip switch data */ template class DipSwitch { diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 33d9290..e1fd153 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -16,7 +16,7 @@ namespace micras::proxy { /** - * @brief Class for controlling TorqueSensors + * @brief Class for receiving torque sensors data */ template class TorqueSensors { From 64f76293bb0161d1427d5db5790237a3aaa3a916 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 11 Apr 2024 15:29:51 -0300 Subject: [PATCH 158/561] =?UTF-8?q?=E2=9C=A8=20Reset=20timer=20on=20constr?= =?UTF-8?q?uctor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/timer.hpp | 2 +- micras_hal/stm32/src/timer.cpp | 6 ++++++ micras_proxy/src/buzzer.cpp | 1 - 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/micras_hal/include/micras/hal/timer.hpp b/micras_hal/include/micras/hal/timer.hpp index 1ab6929..ab5b6c0 100644 --- a/micras_hal/include/micras/hal/timer.hpp +++ b/micras_hal/include/micras/hal/timer.hpp @@ -29,7 +29,7 @@ class Timer { /** * @brief Construct a new Timer object */ - Timer() = default; + Timer(); /** * @brief Construct a new Timer object diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index 0a24623..cda058f 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -9,6 +9,10 @@ #include "micras/hal/timer.hpp" namespace micras::hal { +Timer::Timer() { + this->reset_ms(); +} + Timer::Timer(const Config& config) : handle{config.handle} { config.init_function(); @@ -18,6 +22,8 @@ Timer::Timer(const Config& config) : handle{config.handle} { if (base_freq / (prescaler + 1) == 1000000 and __HAL_TIM_GetAutoreload(this->handle) == 999) { this->enable_microseconds = true; } + + this->reset_us(); } void Timer::reset_ms() { diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp index 88ab23f..e4a230f 100644 --- a/micras_proxy/src/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -32,7 +32,6 @@ void Buzzer::update() { void Buzzer::wait(uint32_t duration) { hal::Timer wait_timer; - wait_timer.reset_ms(); while (wait_timer.elapsed_time_ms() < duration) { this->update(); From 49f62b0873950aa439ec38c11b9e785003f66a40 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 11 Apr 2024 15:30:37 -0300 Subject: [PATCH 159/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20stop=5Fleft=20a?= =?UTF-8?q?nd=20stop=5Fright=20functions=20from=20locomotion?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/locomotion.hpp | 10 ---------- micras_proxy/src/locomotion.cpp | 13 ++++--------- 2 files changed, 4 insertions(+), 19 deletions(-) diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index 777f202..1e4dd03 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -69,16 +69,6 @@ class Locomotion { */ void stop(); - /** - * @brief Stop the left motor - */ - void stop_left(); - - /** - * @brief Stop the right motor - */ - void stop_right(); - private: /** * @brief PWM handle for the left motor forward diff --git a/micras_proxy/src/locomotion.cpp b/micras_proxy/src/locomotion.cpp index 8700255..8cf6b9b 100644 --- a/micras_proxy/src/locomotion.cpp +++ b/micras_proxy/src/locomotion.cpp @@ -37,7 +37,8 @@ void Locomotion::set_wheel_speed(float left_speed, float right_speed) { this->pwm_left_fwd.set_duty_cycle(0.0F); this->pwm_left_bwd.set_duty_cycle(-left_speed); } else { - this->stop_left(); + this->pwm_left_fwd.set_duty_cycle(0.0F); + this->pwm_left_bwd.set_duty_cycle(0.0F); } if (right_speed > 0.0F) { @@ -47,7 +48,8 @@ void Locomotion::set_wheel_speed(float left_speed, float right_speed) { this->pwm_right_fwd.set_duty_cycle(0.0F); this->pwm_right_bwd.set_duty_cycle(-right_speed); } else { - this->stop_right(); + this->pwm_right_fwd.set_duty_cycle(0.0F); + this->pwm_right_bwd.set_duty_cycle(0.0F); } } @@ -69,16 +71,9 @@ void Locomotion::set_speed(float linear, float angular) { } void Locomotion::stop() { - this->stop_left(); - this->stop_right(); -} - -void Locomotion::stop_left() { this->pwm_left_fwd.set_duty_cycle(0.0F); this->pwm_left_bwd.set_duty_cycle(0.0F); -} -void Locomotion::stop_right() { this->pwm_right_fwd.set_duty_cycle(0.0F); this->pwm_right_bwd.set_duty_cycle(0.0F); } From 565592059140c94cb20f3b0596b6a8453e89f86b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 11 Apr 2024 15:36:11 -0300 Subject: [PATCH 160/561] =?UTF-8?q?=F0=9F=9A=B8=20Make=20fan=20rotation=20?= =?UTF-8?q?direction=20struct=20private?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/fan.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index 884b46e..1f70366 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -29,14 +29,6 @@ class Fan { hal::Gpio::Config enable_gpio; }; - /** - * @brief Enum for rotation direction - */ - enum RotationDirection : uint8_t { - FORWARD, - BACKWARD - }; - /** * @brief Construct a new fan object * @@ -67,6 +59,14 @@ class Fan { void stop(); private: + /** + * @brief Enum for rotation direction + */ + enum RotationDirection : uint8_t { + FORWARD, + BACKWARD + }; + /** * @brief Set the rotation direction of the fan * From 05397821686482f1cfd933d43d48a1b4982a6766 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 12 Apr 2024 01:05:49 -0300 Subject: [PATCH 161/561] =?UTF-8?q?=F0=9F=9A=9A=20Improve=20locomotion=20p?= =?UTF-8?q?roxy=20config=20variable=20names?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/locomotion.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index 1e4dd03..0a962e5 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -24,10 +24,10 @@ class Locomotion { * @brief Configuration structure for the locomotion */ struct Config { - hal::Pwm::Config pwm_left_fwd; - hal::Pwm::Config pwm_left_bwd; - hal::Pwm::Config pwm_right_fwd; - hal::Pwm::Config pwm_right_bwd; + hal::Pwm::Config pwm_left_forward; + hal::Pwm::Config pwm_left_backwards; + hal::Pwm::Config pwm_right_forward; + hal::Pwm::Config pwm_right_backwards; hal::Gpio::Config enable_gpio; }; From 80ba00a12bbfedb572ad2b0de53fdcdf050e6698 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 12 Apr 2024 01:06:51 -0300 Subject: [PATCH 162/561] =?UTF-8?q?=F0=9F=92=A1=20Change=20receiving=20to?= =?UTF-8?q?=20acquiring=20in=20class=20description=20comments?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi <62271285+PedroDeSanti@users.noreply.github.com> --- micras_proxy/include/micras/proxy/button.hpp | 2 +- micras_proxy/include/micras/proxy/dip_switch.hpp | 2 +- micras_proxy/include/micras/proxy/torque_sensors.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index 6cc6808..c1cad58 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -16,7 +16,7 @@ namespace micras::proxy { /** - * @brief Class for receiving a button data + * @brief Class for acquiring a button data */ class Button { public: diff --git a/micras_proxy/include/micras/proxy/dip_switch.hpp b/micras_proxy/include/micras/proxy/dip_switch.hpp index 9164795..c500ebe 100644 --- a/micras_proxy/include/micras/proxy/dip_switch.hpp +++ b/micras_proxy/include/micras/proxy/dip_switch.hpp @@ -16,7 +16,7 @@ namespace micras::proxy { /** - * @brief Class for receiving a dip switch data + * @brief Class for acquiring a dip switch data */ template class DipSwitch { diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index e1fd153..74ec861 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -16,7 +16,7 @@ namespace micras::proxy { /** - * @brief Class for receiving torque sensors data + * @brief Class for acquiring torque sensors data */ template class TorqueSensors { From e163b915ba8062f4d2a849dfa0a8781dd7993068 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 12 Apr 2024 01:10:52 -0300 Subject: [PATCH 163/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20pwm=20names=20on?= =?UTF-8?q?=20locomotion=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/proxy/locomotion.hpp | 8 ++-- micras_proxy/src/locomotion.cpp | 40 +++++++++---------- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index 0a962e5..16264b7 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -73,22 +73,22 @@ class Locomotion { /** * @brief PWM handle for the left motor forward */ - hal::Pwm pwm_left_fwd; + hal::Pwm pwm_left_forward; /** * @brief PWM handle for the left motor backward */ - hal::Pwm pwm_left_bwd; + hal::Pwm pwm_left_backwards; /** * @brief PWM handle for the right motor forward */ - hal::Pwm pwm_right_fwd; + hal::Pwm pwm_right_forward; /** * @brief PWM handle for the right motor backward */ - hal::Pwm pwm_right_bwd; + hal::Pwm pwm_right_backwards; /** * @brief GPIO handle for the motor driver enable pin diff --git a/micras_proxy/src/locomotion.cpp b/micras_proxy/src/locomotion.cpp index 8cf6b9b..f4b7335 100644 --- a/micras_proxy/src/locomotion.cpp +++ b/micras_proxy/src/locomotion.cpp @@ -12,10 +12,10 @@ namespace micras::proxy { Locomotion::Locomotion(const Config& config) : - pwm_left_fwd{config.pwm_left_fwd}, - pwm_left_bwd{config.pwm_left_bwd}, - pwm_right_fwd{config.pwm_right_fwd}, - pwm_right_bwd{config.pwm_right_bwd}, + pwm_left_forward{config.pwm_left_forward}, + pwm_left_backwards{config.pwm_left_backwards}, + pwm_right_forward{config.pwm_right_forward}, + pwm_right_backwards{config.pwm_right_backwards}, enable_gpio{config.enable_gpio} { this->stop(); this->enable(); @@ -31,25 +31,25 @@ void Locomotion::disable() { void Locomotion::set_wheel_speed(float left_speed, float right_speed) { if (left_speed > 0.0F) { - this->pwm_left_fwd.set_duty_cycle(left_speed); - this->pwm_left_bwd.set_duty_cycle(0.0F); + this->pwm_left_forward.set_duty_cycle(left_speed); + this->pwm_left_backwards.set_duty_cycle(0.0F); } else if (left_speed < 0.0F) { - this->pwm_left_fwd.set_duty_cycle(0.0F); - this->pwm_left_bwd.set_duty_cycle(-left_speed); + this->pwm_left_forward.set_duty_cycle(0.0F); + this->pwm_left_backwards.set_duty_cycle(-left_speed); } else { - this->pwm_left_fwd.set_duty_cycle(0.0F); - this->pwm_left_bwd.set_duty_cycle(0.0F); + this->pwm_left_forward.set_duty_cycle(0.0F); + this->pwm_left_backwards.set_duty_cycle(0.0F); } if (right_speed > 0.0F) { - this->pwm_right_fwd.set_duty_cycle(right_speed); - this->pwm_right_bwd.set_duty_cycle(0.0F); + this->pwm_right_forward.set_duty_cycle(right_speed); + this->pwm_right_backwards.set_duty_cycle(0.0F); } else if (right_speed < 0.0F) { - this->pwm_right_fwd.set_duty_cycle(0.0F); - this->pwm_right_bwd.set_duty_cycle(-right_speed); + this->pwm_right_forward.set_duty_cycle(0.0F); + this->pwm_right_backwards.set_duty_cycle(-right_speed); } else { - this->pwm_right_fwd.set_duty_cycle(0.0F); - this->pwm_right_bwd.set_duty_cycle(0.0F); + this->pwm_right_forward.set_duty_cycle(0.0F); + this->pwm_right_backwards.set_duty_cycle(0.0F); } } @@ -71,10 +71,10 @@ void Locomotion::set_speed(float linear, float angular) { } void Locomotion::stop() { - this->pwm_left_fwd.set_duty_cycle(0.0F); - this->pwm_left_bwd.set_duty_cycle(0.0F); + this->pwm_left_forward.set_duty_cycle(0.0F); + this->pwm_left_backwards.set_duty_cycle(0.0F); - this->pwm_right_fwd.set_duty_cycle(0.0F); - this->pwm_right_bwd.set_duty_cycle(0.0F); + this->pwm_right_forward.set_duty_cycle(0.0F); + this->pwm_right_backwards.set_duty_cycle(0.0F); } } // namespace micras::proxy From 44fe0a4a20a1b462cdbb7080ab3190b590eb4ee1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 12 Apr 2024 01:23:24 -0300 Subject: [PATCH 164/561] =?UTF-8?q?=F0=9F=9A=9A=20Change=20led=20pwm=20nam?= =?UTF-8?q?es=20on=20distance=20sensors=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/distance_sensors.hpp | 8 ++++---- micras_proxy/src/distance_sensors.cpp | 10 +++++----- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/micras_proxy/include/micras/proxy/distance_sensors.hpp b/micras_proxy/include/micras/proxy/distance_sensors.hpp index 92a3711..5b88409 100644 --- a/micras_proxy/include/micras/proxy/distance_sensors.hpp +++ b/micras_proxy/include/micras/proxy/distance_sensors.hpp @@ -27,8 +27,8 @@ class DistanceSensors { */ struct Config { hal::AdcDma::Config adc; - hal::Pwm::Config led0_pwm; - hal::Pwm::Config led1_pwm; + hal::Pwm::Config led_0_pwm; + hal::Pwm::Config led_1_pwm; float max_distance; }; @@ -71,12 +71,12 @@ class DistanceSensors { /** * @brief PWM handle for the even infrared LEDs */ - hal::Pwm led0_pwm; + hal::Pwm led_0_pwm; /** * @brief PWM handle for the odd infrared LEDs */ - hal::Pwm led1_pwm; + hal::Pwm led_1_pwm; /** * @brief Buffer to store the ADC values diff --git a/micras_proxy/src/distance_sensors.cpp b/micras_proxy/src/distance_sensors.cpp index 3b799ba..060c61e 100644 --- a/micras_proxy/src/distance_sensors.cpp +++ b/micras_proxy/src/distance_sensors.cpp @@ -14,16 +14,16 @@ namespace micras::proxy { template DistanceSensors::DistanceSensors(const Config& config) : - adc{config.adc}, led0_pwm{config.led0_pwm}, led1_pwm{config.led1_pwm}, max_distance{config.max_distance} { + adc{config.adc}, led_0_pwm{config.led_0_pwm}, led_1_pwm{config.led_1_pwm}, max_distance{config.max_distance} { this->adc.start_dma(this->buffer.data(), num_of_sensors); - this->led0_pwm.set_duty_cycle(50.0F); - this->led1_pwm.set_duty_cycle(50.0F); + this->led_0_pwm.set_duty_cycle(50.0F); + this->led_1_pwm.set_duty_cycle(50.0F); } template void DistanceSensors::set_led_intensity(float intensity) { - this->led0_pwm.set_duty_cycle(intensity); - this->led1_pwm.set_duty_cycle(intensity); + this->led_0_pwm.set_duty_cycle(intensity); + this->led_1_pwm.set_duty_cycle(intensity); } template From c5abc3fbb9008d9aa19adb393aafc783f84e29dc Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 12 Apr 2024 03:11:51 -0300 Subject: [PATCH 165/561] =?UTF-8?q?=E2=9C=85=20Add=20storage=20proxy=20tes?= =?UTF-8?q?t?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../micras/proxy/serializable_interface.hpp | 4 ++- micras_proxy/include/micras/proxy/storage.hpp | 13 +++++++-- micras_proxy/src/storage.cpp | 28 +++++++++---------- 3 files changed, 27 insertions(+), 18 deletions(-) diff --git a/micras_proxy/include/micras/proxy/serializable_interface.hpp b/micras_proxy/include/micras/proxy/serializable_interface.hpp index ddc5152..889f77f 100644 --- a/micras_proxy/include/micras/proxy/serializable_interface.hpp +++ b/micras_proxy/include/micras/proxy/serializable_interface.hpp @@ -36,12 +36,14 @@ class ISerializable { * @param serial_data Serialized data * @param size Size of the serialized data */ - virtual void deserialize(uint8_t* serial_data, uint16_t size) = 0; + virtual void deserialize(const uint8_t* serial_data, uint16_t size) = 0; protected: /** * @brief Special member functions declared as default */ + ISerializable() = default; + ISerializable(const ISerializable&) = default; ISerializable(ISerializable&&) = default; diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index 10e17c6..7201164 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -49,7 +49,10 @@ class Storage { * @param data Reference to the variable */ template - void create(const std::string& name, const T& data); + void create(const std::string& name, const T& data) { + this->primitives[name].ram_pointer = &data; + this->primitives.at(name).size = sizeof(T); + } /** * @brief Create a new serializable variable in the storage @@ -67,7 +70,13 @@ class Storage { * @param data Reference to the variable */ template - void sync(const std::string& name, T& data); + void sync(const std::string& name, T& data) { + if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr) { + data = reinterpret_cast(this->buffer.at(this->primitives.at(name).buffer_address)); + } + + this->create(name, data); + } /** * @brief Sync a serializable variable with the storage diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 61d565c..4554aef 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -29,25 +29,10 @@ Storage::Storage(const Config& config) : start_page{config.start_page}, number_o this->serializables = deserialize_var_map(this->buffer, num_serializables); } -template -void Storage::create(const std::string& name, const T& data) { - this->primitives[name].ram_pointer = &data; - this->primitives.at(name).size = sizeof(T); -} - void Storage::create(const std::string& name, const ISerializable& data) { this->serializables[name].ram_pointer = &data; } -template -void Storage::sync(const std::string& name, T& data) { - if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr) { - data = reinterpret_cast(this->buffer.at(this->primitives.at(name).buffer_address)); - } - - this->create(name, data); -} - void Storage::sync(const std::string& name, ISerializable& data) { if (this->serializables.contains(name) and this->serializables.at(name).ram_pointer == nullptr) { const auto& serializable = this->serializables.at(name); @@ -146,4 +131,17 @@ std::unordered_map Storage::deserialize_var_map(std::vector + Storage::serialize_var_map(const std::unordered_map& variables); + +template std::unordered_map + Storage::deserialize_var_map(std::vector& buffer, uint16_t num_vars); + +template std::vector + Storage::serialize_var_map(const std::unordered_map& variables); + +template std::unordered_map + Storage::deserialize_var_map(std::vector& buffer, uint16_t num_vars); } // namespace micras::proxy From ade3438eb8a045c1dccce912c9599a63da308008 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 12 Apr 2024 13:04:30 -0300 Subject: [PATCH 166/561] =?UTF-8?q?=F0=9F=90=9B=20Use=20start=20symbol=20o?= =?UTF-8?q?n=20storage=20constructor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/storage.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 4554aef..8e568a4 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -14,7 +14,7 @@ Storage::Storage(const Config& config) : start_page{config.start_page}, number_o uint64_t header{}; hal::Flash::read(this->start_page, 0, &header); - if (header >> 48 != 0xABAB) { + if (header >> 48 != start_symbol) { return; } From e35f08643b95a88317874fb3a6006ab4e0c1c601 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 12 Apr 2024 23:24:27 -0300 Subject: [PATCH 167/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20buzzer=20wait=20fu?= =?UTF-8?q?nction?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/buzzer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp index e4a230f..2388c7c 100644 --- a/micras_proxy/src/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -17,9 +17,9 @@ void Buzzer::play(uint32_t frequency, uint32_t duration) { this->pwm.set_duty_cycle(50.0F); this->pwm.set_frequency(frequency); this->is_playing = true; + this->duration = duration; if (duration > 0) { - this->duration = duration; this->timer.reset_ms(); } } @@ -31,6 +31,10 @@ void Buzzer::update() { } void Buzzer::wait(uint32_t duration) { + while (this->duration > 0 and this->is_playing) { + this->update(); + } + hal::Timer wait_timer; while (wait_timer.elapsed_time_ms() < duration) { From 6b67f7338ecca12ae50a689528c3e06e245c1117 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 13 Apr 2024 12:56:39 -0300 Subject: [PATCH 168/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Change=20ADC=20max?= =?UTF-8?q?=20reading=20type=20to=20uint16=5Ft?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/adc_dma.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index d3e19ea..9121427 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -24,7 +24,7 @@ class AdcDma { struct Config { void (*init_function)(); ADC_HandleTypeDef* handle; - uint32_t max_reading; + uint16_t max_reading; float reference_voltage; }; @@ -59,7 +59,7 @@ class AdcDma { /** * @brief Maximum ADC reading */ - const uint32_t max_reading; // NOLINT(*-non-private-member-variables-in-classes) + const uint16_t max_reading; // NOLINT(*-non-private-member-variables-in-classes) /** * @brief Reference voltage for the ADC measurement From 93780acac0c2131f1ae4a121d7439d6b6f6fdb02 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 15 Apr 2024 18:17:32 -0300 Subject: [PATCH 169/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20bin=20folder?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/CMakeLists.txt | 6 +++--- micras_proxy/lib/lsm6dsv-pid | 1 - 2 files changed, 3 insertions(+), 4 deletions(-) delete mode 160000 micras_proxy/lib/lsm6dsv-pid diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt index 13aa38e..752ba24 100644 --- a/micras_proxy/CMakeLists.txt +++ b/micras_proxy/CMakeLists.txt @@ -1,9 +1,9 @@ project(micras_proxy) file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") -file(GLOB_RECURSE PROJECT_LIB_SOURCES CONFIGURE_DEPENDS "lib/*.c*") +file(GLOB_RECURSE PROJECT_LIB_SOURCES CONFIGURE_DEPENDS "libs/*.c*") -# Remove warnings from lib sources +# Remove warnings from libraries sources set_source_files_properties( ${PROJECT_LIB_SOURCES} PROPERTIES @@ -19,7 +19,7 @@ add_library(micras::proxy ALIAS ${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} PUBLIC include - lib/lsm6dsv-pid + libs/lsm6dsv-pid ) target_link_libraries(${PROJECT_NAME} PUBLIC diff --git a/micras_proxy/lib/lsm6dsv-pid b/micras_proxy/lib/lsm6dsv-pid deleted file mode 160000 index 1208320..0000000 --- a/micras_proxy/lib/lsm6dsv-pid +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 120832044d340c5a2f59e05861375f8aea4d8804 From be5dcf8a1fc46ea90cec81ca0ae6080d233bc3fb Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 15 Apr 2024 18:33:39 -0300 Subject: [PATCH 170/561] =?UTF-8?q?=E2=8F=AA=20Add=20imu=20submodule=20bac?= =?UTF-8?q?k?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/libs/lsm6dsv-pid | 1 + 1 file changed, 1 insertion(+) create mode 160000 micras_proxy/libs/lsm6dsv-pid diff --git a/micras_proxy/libs/lsm6dsv-pid b/micras_proxy/libs/lsm6dsv-pid new file mode 160000 index 0000000..1208320 --- /dev/null +++ b/micras_proxy/libs/lsm6dsv-pid @@ -0,0 +1 @@ +Subproject commit 120832044d340c5a2f59e05861375f8aea4d8804 From 59320a22a40f38105e77364e00decb66f59c87e4 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Mon, 15 Apr 2024 19:40:57 -0300 Subject: [PATCH 171/561] =?UTF-8?q?=F0=9F=94=A7=20Update=20variable=20name?= =?UTF-8?q?s=20in=20CMakeLists.txt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt index 752ba24..dc523d5 100644 --- a/micras_proxy/CMakeLists.txt +++ b/micras_proxy/CMakeLists.txt @@ -1,18 +1,18 @@ project(micras_proxy) file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") -file(GLOB_RECURSE PROJECT_LIB_SOURCES CONFIGURE_DEPENDS "libs/*.c*") +file(GLOB_RECURSE PROJECT_LIBS_SOURCES CONFIGURE_DEPENDS "libs/*.c*") # Remove warnings from libraries sources set_source_files_properties( - ${PROJECT_LIB_SOURCES} + ${PROJECT_LIBS_SOURCES} PROPERTIES COMPILE_FLAGS "-w" ) add_library(${PROJECT_NAME} STATIC ${PROJECT_SOURCES} - ${PROJECT_LIB_SOURCES} + ${PROJECT_LIBS_SOURCES} ) add_library(micras::proxy ALIAS ${PROJECT_NAME}) From d220d17ecad21d825ef5254edcba3aeba850562f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 18 Apr 2024 18:49:30 -0300 Subject: [PATCH 172/561] =?UTF-8?q?=F0=9F=9A=9A=20Rename=20speed=20to=20co?= =?UTF-8?q?mmand=20in=20locomotion?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/proxy/locomotion.hpp | 16 ++++---- micras_proxy/src/locomotion.cpp | 38 +++++++++---------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index 16264b7..9ca9ba5 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -49,20 +49,20 @@ class Locomotion { void disable(); /** - * @brief Set the speed of the wheels + * @brief Set the command of the wheels * - * @param left_speed Speed of the left wheels - * @param right_speed Speed of the right wheels + * @param left_command Command of the left wheels + * @param right_command Command of the right wheels */ - void set_wheel_speed(float left_speed, float right_speed); + void set_wheel_command(float left_command, float right_command); /** - * @brief Set the linear and angular speeds of the robot + * @brief Set the linear and angular commands of the robot * - * @param linear Linear speed of the robot - * @param angular Angular speed of the robot + * @param linear Linear command of the robot + * @param angular Angular command of the robot */ - void set_speed(float linear, float angular); + void set_command(float linear, float angular); /** * @brief Stop the motors diff --git a/micras_proxy/src/locomotion.cpp b/micras_proxy/src/locomotion.cpp index f4b7335..bd85041 100644 --- a/micras_proxy/src/locomotion.cpp +++ b/micras_proxy/src/locomotion.cpp @@ -29,45 +29,45 @@ void Locomotion::disable() { this->enable_gpio.write(false); } -void Locomotion::set_wheel_speed(float left_speed, float right_speed) { - if (left_speed > 0.0F) { - this->pwm_left_forward.set_duty_cycle(left_speed); +void Locomotion::set_wheel_command(float left_command, float right_command) { + if (left_command > 0.0F) { + this->pwm_left_forward.set_duty_cycle(left_command); this->pwm_left_backwards.set_duty_cycle(0.0F); - } else if (left_speed < 0.0F) { + } else if (left_command < 0.0F) { this->pwm_left_forward.set_duty_cycle(0.0F); - this->pwm_left_backwards.set_duty_cycle(-left_speed); + this->pwm_left_backwards.set_duty_cycle(-left_command); } else { this->pwm_left_forward.set_duty_cycle(0.0F); this->pwm_left_backwards.set_duty_cycle(0.0F); } - if (right_speed > 0.0F) { - this->pwm_right_forward.set_duty_cycle(right_speed); + if (right_command > 0.0F) { + this->pwm_right_forward.set_duty_cycle(right_command); this->pwm_right_backwards.set_duty_cycle(0.0F); - } else if (right_speed < 0.0F) { + } else if (right_command < 0.0F) { this->pwm_right_forward.set_duty_cycle(0.0F); - this->pwm_right_backwards.set_duty_cycle(-right_speed); + this->pwm_right_backwards.set_duty_cycle(-right_command); } else { this->pwm_right_forward.set_duty_cycle(0.0F); this->pwm_right_backwards.set_duty_cycle(0.0F); } } -void Locomotion::set_speed(float linear, float angular) { - float left_speed = linear - angular; - float right_speed = linear + angular; +void Locomotion::set_command(float linear, float angular) { + float left_command = linear - angular; + float right_command = linear + angular; - if (std::abs(left_speed) > 100.0F) { - left_speed *= 100.0F / std::abs(left_speed); - right_speed *= 100.0F / std::abs(left_speed); + if (std::abs(left_command) > 100.0F) { + left_command *= 100.0F / std::abs(left_command); + right_command *= 100.0F / std::abs(left_command); } - if (std::abs(right_speed) > 100.0F) { - left_speed *= 100.0F / std::abs(right_speed); - right_speed *= 100.0F / std::abs(right_speed); + if (std::abs(right_command) > 100.0F) { + left_command *= 100.0F / std::abs(right_command); + right_command *= 100.0F / std::abs(right_command); } - this->set_wheel_speed(left_speed, right_speed); + this->set_wheel_command(left_command, right_command); } void Locomotion::stop() { From e43f0e733395738dca8cd0d959787e73f792a166 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 22 Apr 2024 22:32:04 -0300 Subject: [PATCH 173/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20microsecond=20time?= =?UTF-8?q?r=20initialization?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/timer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index cda058f..5126696 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -19,8 +19,9 @@ Timer::Timer(const Config& config) : handle{config.handle} { uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); uint32_t prescaler = this->handle->Instance->PSC; - if (base_freq / (prescaler + 1) == 1000000 and __HAL_TIM_GetAutoreload(this->handle) == 999) { + if (base_freq / (prescaler + 1) == 1000000) { this->enable_microseconds = true; + HAL_TIM_Base_Start(this->handle); } this->reset_us(); @@ -60,7 +61,7 @@ uint32_t Timer::get_counter_ms() { uint32_t Timer::get_counter_us() const { if (this->enable_microseconds) { - return 1000 * HAL_GetTick() + __HAL_TIM_GET_COUNTER(this->handle); + return __HAL_TIM_GET_COUNTER(this->handle); } return 1000 * HAL_GetTick(); From 799358375cf49208fd92cc45fe371d62cbe30cc0 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 22 Apr 2024 22:36:27 -0300 Subject: [PATCH 174/561] =?UTF-8?q?=E2=9C=A8=20Add=20encoder=20odometry?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/CMakeLists.txt | 17 +++ micras_nav/include/micras/nav/odometry.hpp | 121 +++++++++++++++++++++ micras_nav/src/odometry.cpp | 87 +++++++++++++++ 3 files changed, 225 insertions(+) create mode 100644 micras_nav/CMakeLists.txt create mode 100644 micras_nav/include/micras/nav/odometry.hpp create mode 100644 micras_nav/src/odometry.cpp diff --git a/micras_nav/CMakeLists.txt b/micras_nav/CMakeLists.txt new file mode 100644 index 0000000..0ddaacb --- /dev/null +++ b/micras_nav/CMakeLists.txt @@ -0,0 +1,17 @@ +project(micras_nav) + +file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") + +add_library(${PROJECT_NAME} STATIC + ${PROJECT_SOURCES} +) + +add_library(micras::nav ALIAS ${PROJECT_NAME}) + +target_include_directories(${PROJECT_NAME} PUBLIC + include +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + micras::proxy +) diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp new file mode 100644 index 0000000..a1ee035 --- /dev/null +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -0,0 +1,121 @@ +/** + * @file odometry.hpp + * + * @brief Nav Odometry class declaration + * + * @date 04/2024 + */ + +#ifndef MICRAS_NAV_ODOMETRY_HPP +#define MICRAS_NAV_ODOMETRY_HPP + +#include + +#include "micras/hal/timer.hpp" +#include "micras/proxy/rotary_sensor.hpp" + +namespace micras::nav { +/** + * @brief Class for calculating the robot odometry + */ +class Odometry { +public: + /** + * @brief Constructor for the Odometry class + * + * @param config Configuration for the odometry + */ + Odometry( + const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, + hal::Timer::Config timer_config, float wheel_radius, float wheel_separation + ); + + /** + * @brief Update the odometry + */ + void update(); + + /** + * @brief Get the position x + * + * @return float Position x + */ + float get_position_x() const; + + /** + * @brief Get the position y + * + * @return float Position y + */ + float get_position_y() const; + + /** + * @brief Get the orientation + * + * @return float Orientation + */ + float get_orientation() const; + + /** + * @brief Get the linear velocity + * + * @return float Linear velocity + */ + float get_linear_velocity() const; + + /** + * @brief Get the angular velocity + * + * @return float Angular velocity + */ + float get_angular_velocity() const; + +private: + /** + * @brief Left rotary sensor + */ + const proxy::RotarySensor& left_rotary_sensor; + + /** + * @brief Right rotary sensor + */ + const proxy::RotarySensor& right_rotary_sensor; + + /** + * @brief Timer for velocity calculation + */ + hal::Timer timer; + + /** + * @brief Wheel radius + */ + const float wheel_radius; + + /** + * @brief Robot width + */ + const float wheel_separation; + + /** + * @brief Last left rotary sensor position + */ + float left_last_position{}; + + /** + * @brief Last right rotary sensor position + */ + float right_last_position{}; + + /** + * @brief Odometry variables + */ + float position_x{}; + float position_y{}; + float orientation{}; + float linear_velocity{}; + float angular_velocity{}; +}; + +} // namespace micras::nav + +#endif // MICRAS_NAV_ODOMETRY_HPP diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp new file mode 100644 index 0000000..67bfc8e --- /dev/null +++ b/micras_nav/src/odometry.cpp @@ -0,0 +1,87 @@ +/** + * @file odometry.cpp + * + * @brief Nav Odometry class implementation + * + * @date 04/2024 + */ + +#include +#include + +#include "micras/nav/odometry.hpp" + +namespace micras::nav { +Odometry::Odometry( + const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, + hal::Timer::Config timer_config, float wheel_radius, float wheel_separation +) : + + left_rotary_sensor{left_rotary_sensor}, + right_rotary_sensor{right_rotary_sensor}, + timer{timer_config}, + wheel_radius{wheel_radius}, + wheel_separation{wheel_separation} { + this->left_last_position = this->left_rotary_sensor.get_position(); + this->right_last_position = this->right_rotary_sensor.get_position(); +} + +void Odometry::update() { + float left_position = this->left_rotary_sensor.get_position(); + float right_position = this->right_rotary_sensor.get_position(); + + float elapsed_time = this->timer.elapsed_time_us() / 1000000.0F; + this->timer.reset_us(); + + if (elapsed_time == 0) { + return; + } + + float left_distance = this->wheel_radius * (left_position - this->left_last_position); + float right_distance = this->wheel_radius * (right_position - this->right_last_position); + + this->left_last_position = left_position; + this->right_last_position = right_position; + + float linear_distance = (left_distance + right_distance) / 2; + float angular_distance = (right_distance - left_distance) / this->wheel_separation; + float linear_diagonal{}; + + if (angular_distance < 0.05) { + linear_diagonal = linear_distance; + } else { + linear_diagonal = std::abs( + std::sqrt(1 - std::cos(angular_distance)) * std::numbers::sqrt2_v * linear_distance / + angular_distance + ); + } + + this->position_x += linear_diagonal * std::cos(this->orientation + angular_distance / 2); + this->position_y += linear_diagonal * std::sin(this->orientation + angular_distance / 2); + + this->orientation += angular_distance; + + this->linear_velocity = linear_distance / elapsed_time; + this->angular_velocity = angular_distance / elapsed_time; +} + +float Odometry::get_position_x() const { + return this->position_x; +} + +float Odometry::get_position_y() const { + return this->position_y; +} + +float Odometry::get_orientation() const { + return this->orientation; +} + +float Odometry::get_linear_velocity() const { + return this->linear_velocity; +} + +float Odometry::get_angular_velocity() const { + return this->angular_velocity; +} +} // namespace micras::nav From 07a89a01bfd8918fd8cc5141aea162b9a602f72e Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 24 Apr 2024 12:07:30 -0300 Subject: [PATCH 175/561] =?UTF-8?q?=F0=9F=8E=A8=20Update=20odometry=20clas?= =?UTF-8?q?s=20to=20use=20'wheels=5Fdistance'=20instead=20of=20'wheel=5Fse?= =?UTF-8?q?paration'?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/odometry.hpp | 4 ++-- micras_nav/src/odometry.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index a1ee035..f6c2a91 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -27,7 +27,7 @@ class Odometry { */ Odometry( const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - hal::Timer::Config timer_config, float wheel_radius, float wheel_separation + hal::Timer::Config timer_config, float wheel_radius, float wheels_distance ); /** @@ -94,7 +94,7 @@ class Odometry { /** * @brief Robot width */ - const float wheel_separation; + const float wheels_distance; /** * @brief Last left rotary sensor position diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 67bfc8e..5504016 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -14,14 +14,14 @@ namespace micras::nav { Odometry::Odometry( const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - hal::Timer::Config timer_config, float wheel_radius, float wheel_separation + hal::Timer::Config timer_config, float wheel_radius, float wheels_distance ) : left_rotary_sensor{left_rotary_sensor}, right_rotary_sensor{right_rotary_sensor}, timer{timer_config}, wheel_radius{wheel_radius}, - wheel_separation{wheel_separation} { + wheels_distance{wheels_distance} { this->left_last_position = this->left_rotary_sensor.get_position(); this->right_last_position = this->right_rotary_sensor.get_position(); } @@ -44,7 +44,7 @@ void Odometry::update() { this->right_last_position = right_position; float linear_distance = (left_distance + right_distance) / 2; - float angular_distance = (right_distance - left_distance) / this->wheel_separation; + float angular_distance = (right_distance - left_distance) / this->wheels_distance; float linear_diagonal{}; if (angular_distance < 0.05) { From d42055be725e5f6031b4197222802abbf847b02c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 3 May 2024 10:32:08 -0300 Subject: [PATCH 176/561] =?UTF-8?q?=E2=AC=86=EF=B8=8F=20Update=20imu=20sub?= =?UTF-8?q?module?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/libs/lsm6dsv-pid | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_proxy/libs/lsm6dsv-pid b/micras_proxy/libs/lsm6dsv-pid index 1208320..551af49 160000 --- a/micras_proxy/libs/lsm6dsv-pid +++ b/micras_proxy/libs/lsm6dsv-pid @@ -1 +1 @@ -Subproject commit 120832044d340c5a2f59e05861375f8aea4d8804 +Subproject commit 551af49c8f98b3c31e8b505f3dc9bd32b2bc694c From ab7f3baf18cc768c628467246890efb91ca1f65e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 3 May 2024 12:06:49 -0300 Subject: [PATCH 177/561] =?UTF-8?q?=F0=9F=9A=B8=20Add=20max=5Fbrightness?= =?UTF-8?q?=20to=20argb?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/argb.hpp | 16 +++++++++++++++- micras_proxy/src/argb.cpp | 9 ++++++--- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index 1ea99a2..07c1767 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -26,6 +26,7 @@ class Argb { */ struct Config { hal::PwmDma::Config pwm; + float max_brightness; }; /** @@ -35,6 +36,14 @@ class Argb { uint8_t red; uint8_t green; uint8_t blue; + + Color operator*(float brightness) const { + return { + static_cast(this->red * brightness), + static_cast(this->green * brightness), + static_cast(this->blue * brightness), + }; + } }; /** @@ -88,7 +97,7 @@ class Argb { static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; static constexpr float low_duty_cycle{32.0F}; // NOLINT(bugprone-dynamic-static-initializers) static constexpr float high_duty_cycle{64.0F}; // NOLINT(bugprone-dynamic-static-initializers) - static constexpr uint8_t reset_length{40}; + static constexpr uint8_t reset_length{225}; /** * @brief PWM control object @@ -105,6 +114,11 @@ class Argb { */ const uint32_t high_bit; + /** + * @brief Maximum brightness of the addressable RGB LED + */ + const float brightness; + /** * @brief Data buffer to send to the addressable RGB LED */ diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index b5847b3..81494bc 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -14,7 +14,10 @@ namespace micras::proxy { template Argb::Argb(const Config& config) : - pwm{config.pwm}, low_bit{pwm.get_compare(low_duty_cycle)}, high_bit{pwm.get_compare(high_duty_cycle)} { } + pwm{config.pwm}, + low_bit{pwm.get_compare(low_duty_cycle)}, + high_bit{pwm.get_compare(high_duty_cycle)}, + brightness{config.max_brightness / 100.0F} { } template void Argb::set_color(const Color& color, uint8_t index) { @@ -22,14 +25,14 @@ void Argb::set_color(const Color& color, uint8_t index) { return; } - this->encode_color(color, index); + this->encode_color(color * this->brightness, index); this->pwm.start_dma(this->buffer.data(), this->buffer.size()); } template void Argb::set_color(const Color& color) { for (uint8_t i = 0; i < num_of_leds; i++) { - this->encode_color(color, i); + this->encode_color(color * this->brightness, i); } this->pwm.start_dma(this->buffer.data(), this->buffer.size()); From 043802818bbf0428c5f57704f0d7b3607a1cfe75 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 5 May 2024 15:23:26 -0300 Subject: [PATCH 178/561] =?UTF-8?q?=F0=9F=9A=B8=20Disable=20RGB=20at=20sta?= =?UTF-8?q?rtup?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/argb.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index 81494bc..3c202b6 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -17,7 +17,9 @@ Argb::Argb(const Config& config) : pwm{config.pwm}, low_bit{pwm.get_compare(low_duty_cycle)}, high_bit{pwm.get_compare(high_duty_cycle)}, - brightness{config.max_brightness / 100.0F} { } + brightness{config.max_brightness / 100.0F} { + this->set_color({0, 0, 0}); +} template void Argb::set_color(const Color& color, uint8_t index) { From 7287b7d1a9561dc189e9e070d9c643a18143e9fa Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 5 May 2024 15:24:43 -0300 Subject: [PATCH 179/561] =?UTF-8?q?=E2=9C=A8=20Add=20whoami=20check=20to?= =?UTF-8?q?=20imu?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/imu.hpp | 7 +++++++ micras_proxy/src/imu.cpp | 14 +++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 769eef1..597405b 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -49,6 +49,13 @@ class Imu { Z }; + /** + * @brief Check the IMU device + * + * @return bool true if the device is correct, false otherwise + */ + bool check_whoami(); + /** * @brief Update the IMU data */ diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 7c00bb9..aa4ac50 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -22,7 +22,11 @@ Imu::Imu(const Config& config) : this->dev_ctx.write_reg = platform_write; this->dev_ctx.handle = &this->spi; - hal::Timer::sleep_ms(10); + hal::Timer::sleep_ms(100); + + if (not this->check_whoami()) { + return; + } lsm6dsv_reset_set(&(this->dev_ctx), LSM6DSV_RESTORE_CTRL_REGS); @@ -58,6 +62,14 @@ Imu::Imu(const Config& config) : lsm6dsv_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE); } +bool Imu::check_whoami() { + uint8_t whoami; + + lsm6dsv_device_id_get(&(this->dev_ctx), &whoami); + + return whoami == LSM6DSV_ID; +} + void Imu::update_data() { lsm6dsv_fifo_status_t fifo_status; lsm6dsv_fifo_status_get(&(this->dev_ctx), &fifo_status); From 54ca03994ca6016fbd768da04dc23fc35c5de6d4 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Tue, 14 May 2024 00:47:39 -0300 Subject: [PATCH 180/561] =?UTF-8?q?=F0=9F=94=A8=20Add=20torque=20sensor=20?= =?UTF-8?q?calibration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/torque_sensors.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index 6ad55fa..510f5dd 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -9,6 +9,7 @@ #ifndef MICRAS_PROXY_TORQUE_SENSORS_CPP #define MICRAS_PROXY_TORQUE_SENSORS_CPP +#include "micras/hal/timer.hpp" #include "micras/proxy/torque_sensors.hpp" namespace micras::proxy { @@ -16,6 +17,12 @@ template TorqueSensors::TorqueSensors(const Config& config) : adc{config.adc}, shunt_resistor{config.shunt_resistor}, max_torque{config.max_torque} { this->adc.start_dma(this->buffer.data(), num_of_sensors); + + hal::Timer::sleep_ms(2); + + for (uint8_t i = 0; i < num_of_sensors; i++) { + this->base_reading.at(i) = this->base_reading.at(i); + } } template @@ -25,7 +32,8 @@ float TorqueSensors::get_torque(uint8_t sensor_index) const { template float TorqueSensors::get_current(uint8_t sensor_index) const { - return this->adc.reference_voltage * this->buffer.at(sensor_index) / (this->adc.max_reading * this->shunt_resistor); + return this->adc.reference_voltage * (this->buffer.at(sensor_index) / this->adc.max_reading - 0.5F) / + this->shunt_resistor; } } // namespace micras::proxy From 07edd0b61631a47aa4cc0991296837fa143fc4d7 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Tue, 14 May 2024 00:47:47 -0300 Subject: [PATCH 181/561] =?UTF-8?q?=F0=9F=94=A8=20Add=20torque=20sensor=20?= =?UTF-8?q?calibration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/torque_sensors.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 74ec861..5b5d11d 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -64,6 +64,11 @@ class TorqueSensors { */ std::array buffer; + /** + * @brief Reading of each sensor when no current is flowing + */ + std::array base_reading; + /** * @brief Value of the shunt resistor in ohms */ From e1e1f68fe46a43c2de8fb301601f3c6285171255 Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 15 May 2024 21:47:02 -0300 Subject: [PATCH 182/561] =?UTF-8?q?=F0=9F=90=9B=20=20Fix=20issue=20with=20?= =?UTF-8?q?PWM=20DMA=20start?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/pwm_dma.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index d875fe9..d104724 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -18,7 +18,7 @@ PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.tim // NOLINTNEXTLINE(*-avoid-c-arrays) void PwmDma::start_dma(uint32_t buffer[], uint32_t size) { if (TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_CHANNEL_STATE_BUSY) { - HAL_TIM_PWM_Stop_DMA(this->handle, this->channel); + return; } HAL_TIM_PWM_Start_DMA(this->handle, this->channel, buffer, size); From 99e5b90ef484f7039cb15c90cf7fa9967588306a Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Wed, 15 May 2024 21:48:48 -0300 Subject: [PATCH 183/561] =?UTF-8?q?=F0=9F=90=9B=20=20Fix=20issue=20set=20d?= =?UTF-8?q?uty=20cycle=20underflow?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/pwm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index 0445f72..0dd0dd8 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -18,7 +18,7 @@ Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_cha } void Pwm::set_duty_cycle(float duty_cycle) { - uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100) - 1); + uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100)); __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); } From ff00838e1d9a11d11aa982152d47ddc4b3e837ae Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 3 Sep 2024 20:27:12 -0300 Subject: [PATCH 184/561] =?UTF-8?q?=E2=9C=A8=20Add=20read=5Fregister=20fun?= =?UTF-8?q?ction=20to=20rotary=5Fsensors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/proxy/rotary_sensor.hpp | 37 +++++++++++-------- micras_proxy/src/rotary_sensor.cpp | 21 +++++++++-- 2 files changed, 40 insertions(+), 18 deletions(-) diff --git a/micras_proxy/include/micras/proxy/rotary_sensor.hpp b/micras_proxy/include/micras/proxy/rotary_sensor.hpp index 4b56aa7..17d14f1 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor.hpp @@ -34,21 +34,6 @@ class RotarySensor { Registers registers; }; - /** - * @brief Construct a new RotarySensor object - * - * @param config Configuration for the rotary sensor - */ - explicit RotarySensor(const Config& config); - - /** - * @brief Get the rotary sensor position over an axis - * - * @return float Current angular position of the sensor in radians - */ - float get_position() const; - -private: union CommandFrame { struct Fields { uint8_t do_not_care : 1; @@ -73,6 +58,27 @@ class RotarySensor { uint32_t raw; }; + /** + * @brief Construct a new RotarySensor object + * + * @param config Configuration for the rotary sensor + */ + explicit RotarySensor(const Config& config); + + /** + * @brief Get the rotary sensor position over an axis + * + * @return float Current angular position of the sensor in radians + */ + float get_position() const; + + /** + * @brief Read a register to the rotary sensor + * + * @param command_frame Command frame to send trough SPI + */ + uint16_t read_register(uint16_t address); + /** * @brief Write a register to the rotary sensor * @@ -81,6 +87,7 @@ class RotarySensor { */ void write_register(CommandFrame& command_frame, DataFrame& data_frame); +private: /** * @brief SPI for the rotary sensor configuration */ diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index dc1390c..147708d 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -49,11 +49,26 @@ float RotarySensor::get_position() const { return encoder.get_counter() * 2 * std::numbers::pi_v / this->resolution; } -void RotarySensor::write_register(CommandFrame& command_frame, DataFrame& data_frame) { +uint16_t RotarySensor::read_register(uint16_t address) { + CommandFrame command_frame = {{.rw = 1, .address = address}}; + DataFrame data_frame; + + command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2) ^ 0xFF; + while (not this->spi.select_device()) { } - command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2); - data_frame.fields.crc = this->crc.calculate(&data_frame.raw, 2); + this->spi.transmit(reinterpret_cast(&command_frame.raw), 3); + this->spi.receive(reinterpret_cast(&data_frame.raw), 3); + this->spi.unselect_device(); + + return data_frame.fields.data; +} + +void RotarySensor::write_register(CommandFrame& command_frame, DataFrame& data_frame) { + command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2) ^ 0xFF; + data_frame.fields.crc = this->crc.calculate(&data_frame.raw, 2) ^ 0xFF; + + while (not this->spi.select_device()) { } this->spi.transmit(reinterpret_cast(&command_frame.raw), 3); this->spi.transmit(reinterpret_cast(&data_frame.raw), 3); From a9f9b1200cedbee235ad8e9ebc1c741000a4a257 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 3 Sep 2024 20:27:39 -0300 Subject: [PATCH 185/561] =?UTF-8?q?=E2=9C=A8=20Add=20base=5Freading=20to?= =?UTF-8?q?=20torque=5Fsensors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/torque_sensors.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index 510f5dd..ff3794f 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -21,7 +21,7 @@ TorqueSensors::TorqueSensors(const Config& config) : hal::Timer::sleep_ms(2); for (uint8_t i = 0; i < num_of_sensors; i++) { - this->base_reading.at(i) = this->base_reading.at(i); + this->base_reading.at(i) = this->buffer.at(i); } } @@ -32,7 +32,8 @@ float TorqueSensors::get_torque(uint8_t sensor_index) const { template float TorqueSensors::get_current(uint8_t sensor_index) const { - return this->adc.reference_voltage * (this->buffer.at(sensor_index) / this->adc.max_reading - 0.5F) / + return this->adc.reference_voltage * + ((this->buffer.at(sensor_index) - this->base_reading.at(sensor_index)) / this->adc.max_reading - 0.5F) / this->shunt_resistor; } } // namespace micras::proxy From ca90ed247afb8c1281b7197c90dadc3f5043c843 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 3 Sep 2024 20:35:50 -0300 Subject: [PATCH 186/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20missing=20Initiali?= =?UTF-8?q?zers=20warning=20on=20rotary=5Fsensor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/libs/lsm6dsv-pid | 2 +- micras_proxy/src/rotary_sensor.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_proxy/libs/lsm6dsv-pid b/micras_proxy/libs/lsm6dsv-pid index 551af49..ceed5e6 160000 --- a/micras_proxy/libs/lsm6dsv-pid +++ b/micras_proxy/libs/lsm6dsv-pid @@ -1 +1 @@ -Subproject commit 551af49c8f98b3c31e8b505f3dc9bd32b2bc694c +Subproject commit ceed5e6ed33e6eb40e9754b5ddb8cad4a47e5130 diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index 147708d..b18067f 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -50,7 +50,7 @@ float RotarySensor::get_position() const { } uint16_t RotarySensor::read_register(uint16_t address) { - CommandFrame command_frame = {{.rw = 1, .address = address}}; + CommandFrame command_frame = {{.do_not_care = 0, .rw = 1, .address = address, .crc = 0}}; DataFrame data_frame; command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2) ^ 0xFF; From 37492b19df36ddac0b4acc7e1be14ca3b1387c6d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 12 Sep 2024 22:21:30 -0300 Subject: [PATCH 187/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 2 ++ micras_hal/stm32/src/flash.cpp | 7 ++++--- micras_proxy/include/micras/proxy/argb.hpp | 4 ++-- micras_proxy/include/micras/proxy/imu.hpp | 4 ++-- micras_proxy/src/imu.cpp | 22 ++++++++++------------ 5 files changed, 20 insertions(+), 19 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index ea5d4ca..2f3dc05 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -23,6 +23,8 @@ Checks: readability-*, -readability-duplicate-include, -readability-magic-numbers, +CheckOptions: + cppcoreguidelines-avoid-non-const-global-variables.AllowInternalLinkage: true WarningsAsErrors: '' HeaderFilterRegex: '(include|config)/.*.hpp' AnalyzeTemporaryDtors: false diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index 3a1785a..69c4ada 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -6,6 +6,8 @@ * @date 03/2024 */ +#include + #include "micras/hal/flash.hpp" namespace micras::hal { @@ -20,7 +22,7 @@ void Flash::read(uint32_t address, uint64_t data[], uint32_t size) { uint32_t end = base_address - 8 * address; for (address = end - 8 * (size - 1); address <= end; address += 8, data++) { - (*data) = *(reinterpret_cast(address)); // NOLINT(performance-no-int-to-ptr) + (*data) = *(std::bit_cast(address)); } } @@ -38,8 +40,7 @@ void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { while (address <= end) { if (size >= double_words_per_row) { - // NOLINTNEXTLINE(clang-diagnostic-pointer-to-int-cast) - HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, reinterpret_cast(data)); + HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, std::bit_cast(data)); address += bytes_per_row; data += double_words_per_row; size -= double_words_per_row; diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index 07c1767..bb34214 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -95,8 +95,8 @@ class Argb { static constexpr uint8_t bits_per_color{8}; static constexpr uint8_t colors_per_led{3}; static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; - static constexpr float low_duty_cycle{32.0F}; // NOLINT(bugprone-dynamic-static-initializers) - static constexpr float high_duty_cycle{64.0F}; // NOLINT(bugprone-dynamic-static-initializers) + static constexpr float low_duty_cycle{32.0F}; + static constexpr float high_duty_cycle{64.0F}; static constexpr uint8_t reset_length{225}; /** diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 597405b..138050b 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -123,11 +123,11 @@ class Imu { /** * @brief Function to convert half precision float to single precision float * - * @param x Half precision float + * @param n Half precision float * * @return float Single precision float */ - static float half_to_float(uint16_t x); // NOLINT(readability-identifier-length) + static float half_to_float(uint16_t n); /** * @brief Conversion constants diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index aa4ac50..373b135 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -16,7 +16,8 @@ Imu::Imu(const Config& config) : spi{config.spi}, gy_factor{ mdps_to_radps * 4.375F * - (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale)))}, + (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale))) + }, xl_factor{mg_to_mps2 * (0.61F * (1 << static_cast(config.accelerometer_scale)))} { this->dev_ctx.read_reg = platform_read; this->dev_ctx.write_reg = platform_write; @@ -221,25 +222,22 @@ void Imu::update_orientation(const uint16_t sflp[3]) { this->orientation[2] = std::atan2(sinp, cosp); } -// NOLINTBEGIN(readability-identifier-length, readability-implicit-bool-conversion) -float Imu::half_to_float(uint16_t x) { - static constexpr auto as_float = [](uint32_t x) -> float { - void* aux = &x; +float Imu::half_to_float(uint16_t n) { + static constexpr auto as_float = [](uint32_t n) -> float { + void* aux = &n; return *reinterpret_cast(aux); }; - static constexpr auto as_uint = [](float x) -> uint32_t { - void* aux = &x; + static constexpr auto as_uint = [](float n) -> uint32_t { + void* aux = &n; return *reinterpret_cast(aux); }; - const uint32_t e = (x & 0x7C00) >> 10; - const uint32_t m = (x & 0x03FF) << 13; + const uint32_t e = (n & 0x7C00) >> 10; + const uint32_t m = (n & 0x03FF) << 13; const uint32_t v = as_uint(static_cast(m)) >> 23; return as_float( - (x & 0x8000) << 16 | (e != 0) * ((e + 112) << 23 | m) | + (n & 0x8000) << 16 | (e != 0) * ((e + 112) << 23 | m) | ((e == 0) & (m != 0)) * ((v - 37) << 23 | ((m << (150 - v)) & 0x007FE000)) ); } - -// NOLINTEND(readability-identifier-length, readability-implicit-bool-conversion) } // namespace micras::proxy From e0d306a935b782d8d08fa6d43b84fe98f26da37e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 18 Sep 2024 16:44:09 -0300 Subject: [PATCH 188/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 36 ++++++++++++++++--- micras_hal/include/micras/hal/adc_dma.hpp | 20 +++++++++-- micras_hal/stm32/src/adc_dma.cpp | 12 ++++++- micras_hal/stm32/src/encoder.cpp | 1 + micras_hal/stm32/src/pwm_dma.cpp | 3 +- micras_nav/include/micras/nav/odometry.hpp | 10 +++--- micras_nav/src/odometry.cpp | 11 +++--- micras_proxy/include/micras/proxy/argb.hpp | 12 +++---- micras_proxy/include/micras/proxy/battery.hpp | 2 +- micras_proxy/include/micras/proxy/button.hpp | 8 ++--- .../include/micras/proxy/dip_switch.hpp | 2 +- .../include/micras/proxy/distance_sensors.hpp | 4 +-- micras_proxy/include/micras/proxy/imu.hpp | 6 ++-- micras_proxy/include/micras/proxy/storage.hpp | 5 +-- .../include/micras/proxy/torque_sensors.hpp | 6 ++-- micras_proxy/src/battery.cpp | 2 +- micras_proxy/src/distance_sensors.cpp | 6 ++-- micras_proxy/src/imu.cpp | 32 +++++++++-------- micras_proxy/src/storage.cpp | 10 +++--- micras_proxy/src/torque_sensors.cpp | 7 ++-- 20 files changed, 126 insertions(+), 69 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 2f3dc05..5533bf6 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -8,14 +8,15 @@ Checks: -clang-diagnostic-unused-command-line-argument, cppcoreguidelines-*, -cppcoreguidelines-avoid-magic-numbers, - -cppcoreguidelines-narrowing-conversions, -cppcoreguidelines-pro-bounds-pointer-arithmetic, - -cppcoreguidelines-pro-type-reinterpret-cast, + -cppcoreguidelines-avoid-do-while, -cppcoreguidelines-pro-type-union-access, google-*, llvm-*, -llvm-header-guard, misc-*, + -misc-include-cleaner, + -misc-const-correctness, modernize-*, -modernize-use-nodiscard, -modernize-use-trailing-return-type, @@ -23,10 +24,35 @@ Checks: readability-*, -readability-duplicate-include, -readability-magic-numbers, -CheckOptions: - cppcoreguidelines-avoid-non-const-global-variables.AllowInternalLinkage: true WarningsAsErrors: '' +HeaderFileExtensions: + - '' + - h + - hh + - hpp + - hxx +ImplementationFileExtensions: + - c + - cc + - cpp + - cxx HeaderFilterRegex: '(include|config)/.*.hpp' -AnalyzeTemporaryDtors: false FormatStyle: none +CheckOptions: + cppcoreguidelines-avoid-non-const-global-variables.AllowInternalLinkage: true + readability-magic-numbers.IgnorePowersOf2IntegerValues: true + readability-magic-numbers.IgnoreBitFieldsWidths: true + readability-magic-numbers.IgnoreUserDefinedLiterals: true + readability-identifier-length.MinimumVariableNameLength: 2 + readability-identifier-length.IgnoredVariableNames: ^[nxyz]$ + readability-identifier-length.MinimumParameterNameLength: 2 + readability-identifier-length.IgnoredParameterNames: ^[nxyz]$ + misc-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic: true + cppcoreguidelines-narrowing-conversions.WarnOnIntegerToFloatingPointNarrowingConversion: false +SystemHeaders: false +ExtraArgsBefore: + - '--target=arm-none-eabi' + - '--sysroot=/usr/lib/arm-none-eabi/' + - '-I/usr/lib/arm-none-eabi/include/c++/13.2.1/' + - '-I/usr/lib/arm-none-eabi/include/c++/13.2.1/arm-none-eabi/' ... diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 9121427..d006401 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -56,17 +56,31 @@ class AdcDma { */ void stop_dma(); + /** + * @brief Get the maximum reading of the ADC + * + * @return uint16_t Maximum reading of the ADC + */ + uint16_t get_max_reading() const; + + /** + * @brief Get the reference voltage for the ADC measurement + * + * @return float Reference voltage for the ADC measurement + */ + float get_reference_voltage() const; + +private: /** * @brief Maximum ADC reading */ - const uint16_t max_reading; // NOLINT(*-non-private-member-variables-in-classes) + uint16_t max_reading; /** * @brief Reference voltage for the ADC measurement */ - const float reference_voltage; // NOLINT(*-non-private-member-variables-in-classes) + float reference_voltage; -private: /** * @brief ADC handle */ diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 762011e..7ad67c2 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -6,6 +6,8 @@ * @date 03/2024 */ +#include + #include "micras/hal/adc_dma.hpp" namespace micras::hal { @@ -22,10 +24,18 @@ void AdcDma::start_dma(uint32_t buffer[], uint32_t size) { // NOLINTNEXTLINE(*-avoid-c-arrays) void AdcDma::start_dma(uint16_t buffer[], uint32_t size) { - this->start_dma(reinterpret_cast(buffer), size); + this->start_dma(std::bit_cast(buffer), size); } void AdcDma::stop_dma() { HAL_ADC_Stop_DMA(this->handle); } + +uint16_t AdcDma::get_max_reading() const { + return this->max_reading; +} + +float AdcDma::get_reference_voltage() const { + return this->reference_voltage; +} } // namespace micras::hal diff --git a/micras_hal/stm32/src/encoder.cpp b/micras_hal/stm32/src/encoder.cpp index 813de58..25e6da7 100644 --- a/micras_hal/stm32/src/encoder.cpp +++ b/micras_hal/stm32/src/encoder.cpp @@ -19,6 +19,7 @@ Encoder::Encoder(const Config& config) : handle{config.handle} { } int32_t Encoder::get_counter() const { + // NOLINTNEXTLINE(cppcoreguidelines-narrowing-conversions) return static_cast(__HAL_TIM_GET_COUNTER(this->handle)) - this->start_count; } } // namespace micras::hal diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index d104724..2f3c01e 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -6,6 +6,7 @@ * @date 03/2024 */ +#include #include #include "micras/hal/pwm_dma.hpp" @@ -26,7 +27,7 @@ void PwmDma::start_dma(uint32_t buffer[], uint32_t size) { // NOLINTNEXTLINE(*-avoid-c-arrays) void PwmDma::start_dma(uint16_t buffer[], uint32_t size) { - start_dma(reinterpret_cast(buffer), size); + start_dma(std::bit_cast(buffer), size); } void PwmDma::stop_dma() { diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index f6c2a91..4498161 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -27,7 +27,7 @@ class Odometry { */ Odometry( const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - hal::Timer::Config timer_config, float wheel_radius, float wheels_distance + hal::Timer::Config timer_config, float wheel_radius, float wheel_separation ); /** @@ -74,12 +74,12 @@ class Odometry { /** * @brief Left rotary sensor */ - const proxy::RotarySensor& left_rotary_sensor; + const proxy::RotarySensor& left_rotary_sensor; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) /** * @brief Right rotary sensor */ - const proxy::RotarySensor& right_rotary_sensor; + const proxy::RotarySensor& right_rotary_sensor; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) /** * @brief Timer for velocity calculation @@ -89,12 +89,12 @@ class Odometry { /** * @brief Wheel radius */ - const float wheel_radius; + float wheel_radius; /** * @brief Robot width */ - const float wheels_distance; + float wheel_separation; /** * @brief Last left rotary sensor position diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 5504016..ef2f842 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -14,17 +14,16 @@ namespace micras::nav { Odometry::Odometry( const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - hal::Timer::Config timer_config, float wheel_radius, float wheels_distance + hal::Timer::Config timer_config, float wheel_radius, float wheel_separation ) : left_rotary_sensor{left_rotary_sensor}, right_rotary_sensor{right_rotary_sensor}, timer{timer_config}, wheel_radius{wheel_radius}, - wheels_distance{wheels_distance} { - this->left_last_position = this->left_rotary_sensor.get_position(); - this->right_last_position = this->right_rotary_sensor.get_position(); -} + wheel_separation{wheel_separation}, + left_last_position{left_rotary_sensor.get_position()}, + right_last_position{right_rotary_sensor.get_position()} { } void Odometry::update() { float left_position = this->left_rotary_sensor.get_position(); @@ -44,7 +43,7 @@ void Odometry::update() { this->right_last_position = right_position; float linear_distance = (left_distance + right_distance) / 2; - float angular_distance = (right_distance - left_distance) / this->wheels_distance; + float angular_distance = (right_distance - left_distance) / this->wheel_separation; float linear_diagonal{}; if (angular_distance < 0.05) { diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index bb34214..d0411df 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -95,8 +95,8 @@ class Argb { static constexpr uint8_t bits_per_color{8}; static constexpr uint8_t colors_per_led{3}; static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; - static constexpr float low_duty_cycle{32.0F}; - static constexpr float high_duty_cycle{64.0F}; + static constexpr float low_duty_cycle{32.0F}; // NOLINT(bugprone-dynamic-static-initializers) + static constexpr float high_duty_cycle{64.0F}; // NOLINT(bugprone-dynamic-static-initializers) static constexpr uint8_t reset_length{225}; /** @@ -107,17 +107,17 @@ class Argb { /** * @brief PWM autoreload value for the low signal */ - const uint32_t low_bit; + uint32_t low_bit; /** * @brief PWM autoreload value for the high signal */ - const uint32_t high_bit; + uint32_t high_bit; /** * @brief Maximum brightness of the addressable RGB LED */ - const float brightness; + float brightness; /** * @brief Data buffer to send to the addressable RGB LED @@ -126,6 +126,6 @@ class Argb { }; } // namespace micras::proxy -#include "../src/argb.cpp" // NOLINT(bugprone-suspicious-include) +#include "../src/argb.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) #endif // MICRAS_PROXY_ARGB_HPP diff --git a/micras_proxy/include/micras/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp index c2c235d..c6a4757 100644 --- a/micras_proxy/include/micras/proxy/battery.hpp +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -62,7 +62,7 @@ class Battery { /** * @brief Voltage divider ratio */ - const float voltage_divider; + float voltage_divider; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index c1cad58..fb24178 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -102,14 +102,14 @@ class Button { /** * @brief Button pressing delays in ms */ - const uint16_t debounce_delay; - const uint16_t long_press_delay; - const uint16_t extra_long_press_delay; + uint16_t debounce_delay; + uint16_t long_press_delay; + uint16_t extra_long_press_delay; /** * @brief Gpio object for button */ - const hal::Gpio gpio; + hal::Gpio gpio; /** * @brief Pull resistor configuration diff --git a/micras_proxy/include/micras/proxy/dip_switch.hpp b/micras_proxy/include/micras/proxy/dip_switch.hpp index c500ebe..db3f724 100644 --- a/micras_proxy/include/micras/proxy/dip_switch.hpp +++ b/micras_proxy/include/micras/proxy/dip_switch.hpp @@ -58,6 +58,6 @@ class DipSwitch { }; } // namespace micras::proxy -#include "../src/dip_switch.cpp" // NOLINT(bugprone-suspicious-include) +#include "../src/dip_switch.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) #endif // MICRAS_PROXY_DIP_SWITCH_HPP diff --git a/micras_proxy/include/micras/proxy/distance_sensors.hpp b/micras_proxy/include/micras/proxy/distance_sensors.hpp index 5b88409..c191658 100644 --- a/micras_proxy/include/micras/proxy/distance_sensors.hpp +++ b/micras_proxy/include/micras/proxy/distance_sensors.hpp @@ -86,10 +86,10 @@ class DistanceSensors { /** * @brief Maximum distance reading in meters */ - const float max_distance; + float max_distance; }; } // namespace micras::proxy -#include "../src/distance_sensors.cpp" // NOLINT(bugprone-suspicious-include) +#include "../src/distance_sensors.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) #endif // MICRAS_PROXY_DISTANCE_SENSORS_HPP diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 138050b..59d9208 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -127,7 +127,7 @@ class Imu { * * @return float Single precision float */ - static float half_to_float(uint16_t n); + static float half_to_float(uint16_t x); /** * @brief Conversion constants @@ -163,12 +163,12 @@ class Imu { /** * @brief Gyroscope conversion factor */ - const float gy_factor; + float gy_factor; /** * @brief Accelerometer conversion factor */ - const float xl_factor; + float xl_factor; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index 7201164..5094953 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -9,6 +9,7 @@ #ifndef MICRAS_PROXY_STORAGE_HPP #define MICRAS_PROXY_STORAGE_HPP +#include #include #include #include @@ -19,7 +20,7 @@ namespace micras::proxy { template -concept Fundamental = std::is_fundamental::value; +concept Fundamental = std::is_fundamental_v; /** * @brief Class for controlling the storage @@ -72,7 +73,7 @@ class Storage { template void sync(const std::string& name, T& data) { if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr) { - data = reinterpret_cast(this->buffer.at(this->primitives.at(name).buffer_address)); + data = std::bit_cast(this->buffer.at(this->primitives.at(name).buffer_address)); } this->create(name, data); diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 5b5d11d..23f4c47 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -72,15 +72,15 @@ class TorqueSensors { /** * @brief Value of the shunt resistor in ohms */ - const float shunt_resistor; + float shunt_resistor; /** * @brief Maximum torque that can be measured by the sensor */ - const float max_torque; + float max_torque; }; } // namespace micras::proxy -#include "../src/torque_sensors.cpp" // NOLINT(bugprone-suspicious-include) +#include "../src/torque_sensors.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) #endif // MICRAS_PROXY_TORQUE_SENSORS_HPP diff --git a/micras_proxy/src/battery.cpp b/micras_proxy/src/battery.cpp index 06baca2..c3a9f0d 100644 --- a/micras_proxy/src/battery.cpp +++ b/micras_proxy/src/battery.cpp @@ -14,7 +14,7 @@ Battery::Battery(const Config& config) : adc{config.adc}, voltage_divider{config } float Battery::get_voltage() const { - return this->voltage_divider * this->raw_reading * this->adc.reference_voltage / this->adc.max_reading; + return this->voltage_divider * this->raw_reading * this->adc.get_reference_voltage() / this->adc.get_max_reading(); } uint32_t Battery::get_voltage_raw() const { diff --git a/micras_proxy/src/distance_sensors.cpp b/micras_proxy/src/distance_sensors.cpp index 060c61e..1566fae 100644 --- a/micras_proxy/src/distance_sensors.cpp +++ b/micras_proxy/src/distance_sensors.cpp @@ -16,8 +16,8 @@ template DistanceSensors::DistanceSensors(const Config& config) : adc{config.adc}, led_0_pwm{config.led_0_pwm}, led_1_pwm{config.led_1_pwm}, max_distance{config.max_distance} { this->adc.start_dma(this->buffer.data(), num_of_sensors); - this->led_0_pwm.set_duty_cycle(50.0F); - this->led_1_pwm.set_duty_cycle(50.0F); + this->led_0_pwm.set_duty_cycle(100.0F); + this->led_1_pwm.set_duty_cycle(100.0F); } template @@ -28,7 +28,7 @@ void DistanceSensors::set_led_intensity(float intensity) { template float DistanceSensors::get_distance(uint8_t sensor_index) const { - return this->max_distance * this->buffer.at(sensor_index) / this->adc.max_reading; + return 30.0 * this->max_distance * this->buffer.at(sensor_index) / this->adc.get_max_reading(); } template diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 373b135..6ecbc2d 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -7,6 +7,7 @@ */ #include +#include #include "micras/hal/timer.hpp" #include "micras/proxy/imu.hpp" @@ -64,7 +65,7 @@ Imu::Imu(const Config& config) : } bool Imu::check_whoami() { - uint8_t whoami; + uint8_t whoami = 0; lsm6dsv_device_id_get(&(this->dev_ctx), &whoami); @@ -84,7 +85,7 @@ void Imu::update_data() { while ((samples--) > 0) { lsm6dsv_fifo_out_raw_t f_data; lsm6dsv_fifo_out_raw_get(&(this->dev_ctx), &f_data); - auto* axis = reinterpret_cast(f_data.data); + auto* axis = reinterpret_cast(f_data.data); // NOLINT(cppcoreguidelines-pro-type-reinterpret-cast) switch (f_data.tag) { case lsm6dsv_fifo_out_raw_t::LSM6DSV_GY_NC_TAG: @@ -100,7 +101,7 @@ void Imu::update_data() { break; case lsm6dsv_fifo_out_raw_t::LSM6DSV_SFLP_GAME_ROTATION_VECTOR_TAG: - this->update_orientation(reinterpret_cast(axis)); + this->update_orientation(std::bit_cast(axis)); break; default: @@ -214,7 +215,7 @@ void Imu::update_orientation(const uint16_t sflp[3]) { // pitch (y-axis rotation) sinp = std::sqrt(1 + 2 * (quat[3] * quat[1] - quat[0] * quat[2])); cosp = std::sqrt(1 - 2 * (quat[3] * quat[1] - quat[0] * quat[2])); - this->orientation[1] = 2 * std::atan2(sinp, cosp) - M_PI / 2; + this->orientation[1] = 2 * std::atan2(sinp, cosp) - std::numbers::pi_v / 2; // yaw (z-axis rotation) sinp = 2 * (quat[3] * quat[2] + quat[0] * quat[1]); @@ -222,22 +223,23 @@ void Imu::update_orientation(const uint16_t sflp[3]) { this->orientation[2] = std::atan2(sinp, cosp); } -float Imu::half_to_float(uint16_t n) { - static constexpr auto as_float = [](uint32_t n) -> float { - void* aux = &n; - return *reinterpret_cast(aux); +// NOLINTBEGIN(readability-identifier-length, readability-implicit-bool-conversion) +float Imu::half_to_float(uint16_t x) { + static constexpr auto as_float = [](uint32_t x) -> float { + void* aux = &x; + return *std::bit_cast(aux); }; - static constexpr auto as_uint = [](float n) -> uint32_t { - void* aux = &n; - return *reinterpret_cast(aux); + static constexpr auto as_uint = [](float x) -> uint32_t { + void* aux = &x; + return *std::bit_cast(aux); }; - const uint32_t e = (n & 0x7C00) >> 10; - const uint32_t m = (n & 0x03FF) << 13; + const uint32_t e = (x & 0x7C00) >> 10; + const uint32_t m = (x & 0x03FF) << 13; const uint32_t v = as_uint(static_cast(m)) >> 23; return as_float( - (n & 0x8000) << 16 | (e != 0) * ((e + 112) << 23 | m) | + (x & 0x8000) << 16 | (e != 0) * ((e + 112) << 23 | m) | ((e == 0) & (m != 0)) * ((v - 37) << 23 | ((m << (150 - v)) & 0x007FE000)) ); -} +} // NOLINTEND(readability-identifier-length, readability-implicit-bool-conversion) } // namespace micras::proxy diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 8e568a4..31b803d 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -6,6 +6,8 @@ * @date 03/2024 */ +#include + #include "micras/hal/flash.hpp" #include "micras/proxy/storage.hpp" @@ -23,7 +25,7 @@ Storage::Storage(const Config& config) : start_page{config.start_page}, number_o uint16_t num_serializables = header; this->buffer.resize(8L * total_size); - hal::Flash::read(this->start_page, 1, reinterpret_cast(this->buffer.data()), total_size); + hal::Flash::read(this->start_page, 1, std::bit_cast(this->buffer.data()), total_size); this->primitives = deserialize_var_map(this->buffer, num_primitives); this->serializables = deserialize_var_map(this->buffer, num_serializables); @@ -52,7 +54,7 @@ void Storage::save() { continue; } - const auto* aux = reinterpret_cast(variable.ram_pointer); + const auto* aux = std::bit_cast(variable.ram_pointer); variable.buffer_address = buffer.size(); this->buffer.insert(this->buffer.end(), aux, aux + variable.size); } @@ -88,14 +90,14 @@ void Storage::save() { this->buffer.emplace_back(start_symbol); this->buffer.emplace_back(start_symbol >> 8); - hal::Flash::write(this->start_page, 0, reinterpret_cast(buffer.data()), buffer.size() / 8); + hal::Flash::write(this->start_page, 0, std::bit_cast(buffer.data()), buffer.size() / 8); } template std::vector Storage::serialize_var_map(const std::unordered_map& variables) { std::vector buffer; - for (auto [name, variable] : variables) { + for (const auto& [name, variable] : variables) { buffer.emplace_back(name.size()); buffer.insert(buffer.end(), name.begin(), name.end()); diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index ff3794f..bb47681 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -27,13 +27,14 @@ TorqueSensors::TorqueSensors(const Config& config) : template float TorqueSensors::get_torque(uint8_t sensor_index) const { - return this->buffer.at(sensor_index) * TorqueSensors::max_torque / this->adc.max_reading; + return this->buffer.at(sensor_index) * TorqueSensors::max_torque / this->adc.get_max_reading(); } template float TorqueSensors::get_current(uint8_t sensor_index) const { - return this->adc.reference_voltage * - ((this->buffer.at(sensor_index) - this->base_reading.at(sensor_index)) / this->adc.max_reading - 0.5F) / + return this->adc.get_reference_voltage() * + ((this->buffer.at(sensor_index) - this->base_reading.at(sensor_index)) / this->adc.get_max_reading() - 0.5F + ) / this->shunt_resistor; } } // namespace micras::proxy From cf8d3f337a89613b182f676fafaa00d48e6621e1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 18 Sep 2024 16:46:01 -0300 Subject: [PATCH 189/561] =?UTF-8?q?=F0=9F=9A=91=20Add=20packed=20attribute?= =?UTF-8?q?=20to=20rotary=20sensors=20registers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/proxy/rotary_sensor.hpp | 16 ++++++++-------- .../include/micras/proxy/rotary_sensor_reg.hpp | 14 +++++++------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/micras_proxy/include/micras/proxy/rotary_sensor.hpp b/micras_proxy/include/micras/proxy/rotary_sensor.hpp index 17d14f1..ac78214 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor.hpp @@ -35,11 +35,11 @@ class RotarySensor { }; union CommandFrame { - struct Fields { - uint8_t do_not_care : 1; - uint8_t rw : 1; - uint16_t address : 14; + struct __attribute__((__packed__)) Fields { uint8_t crc : 8; + uint16_t address : 14; + uint8_t rw : 1; + uint8_t do_not_care : 1; }; Fields fields; @@ -47,11 +47,11 @@ class RotarySensor { }; union DataFrame { - struct Fields { - uint8_t warning : 1; - uint8_t error : 1; - uint16_t data : 14; + struct __attribute__((__packed__)) Fields { uint8_t crc : 8; + uint16_t data : 14; + uint8_t error : 1; + uint8_t warning : 1; }; Fields fields; diff --git a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp index a6e750f..d1b1947 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp @@ -20,7 +20,7 @@ struct Registers { * @brief Registers union types definition */ union Disable { - struct Fields { + struct __attribute__((__packed__)) Fields { uint8_t UVW_off : 1; uint8_t ABI_off : 1; uint8_t na : 4; @@ -32,7 +32,7 @@ struct Registers { }; union Zposm { - struct Fields { + struct __attribute__((__packed__)) Fields { uint8_t ZPOSM : 8; }; @@ -41,7 +41,7 @@ struct Registers { }; union Zposl { - struct Fields { + struct __attribute__((__packed__)) Fields { uint8_t ZPOSL : 6; uint8_t Dia1_en : 1; uint8_t Dia2_en : 1; @@ -52,7 +52,7 @@ struct Registers { }; union Settings1 { - struct Fields { + struct __attribute__((__packed__)) Fields { uint8_t K_max : 3; uint8_t K_min : 3; uint8_t Dia3_en : 1; @@ -64,7 +64,7 @@ struct Registers { }; union Settings2 { - struct Fields { + struct __attribute__((__packed__)) Fields { uint8_t IWIDTH : 1; uint8_t NOISESET : 1; uint8_t DIR : 1; @@ -80,7 +80,7 @@ struct Registers { }; union Settings3 { - struct Fields { + struct __attribute__((__packed__)) Fields { uint8_t UVWPP : 3; uint8_t HYS : 2; uint8_t ABIRES : 3; @@ -91,7 +91,7 @@ struct Registers { }; union Ecc { - struct Fields { + struct __attribute__((__packed__)) Fields { uint8_t ECC_chsum : 7; uint8_t ECC_en : 1; }; From 1b7d03413837e0b95ec62baf79f477f591de7169 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 24 Sep 2024 21:26:15 -0300 Subject: [PATCH 190/561] =?UTF-8?q?=E2=9C=A8=20Add=20micras=20core=20packa?= =?UTF-8?q?ge?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/CMakeLists.txt | 13 +++ .../micras/core/butterworth_filter.hpp | 85 +++++++++++++++++ micras_core/include/micras/core/utils.hpp | 92 +++++++++++++++++++ micras_core/src/butterworth_filter.cpp | 65 +++++++++++++ micras_hal/CMakeLists.txt | 4 + 5 files changed, 259 insertions(+) create mode 100644 micras_core/CMakeLists.txt create mode 100644 micras_core/include/micras/core/butterworth_filter.hpp create mode 100644 micras_core/include/micras/core/utils.hpp create mode 100644 micras_core/src/butterworth_filter.cpp diff --git a/micras_core/CMakeLists.txt b/micras_core/CMakeLists.txt new file mode 100644 index 0000000..e4c6680 --- /dev/null +++ b/micras_core/CMakeLists.txt @@ -0,0 +1,13 @@ +project(micras_core) + +file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") + +add_library(${PROJECT_NAME} STATIC + ${PROJECT_SOURCES} +) + +add_library(micras::core ALIAS ${PROJECT_NAME}) + +target_include_directories(${PROJECT_NAME} PUBLIC + include +) diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp new file mode 100644 index 0000000..76b3081 --- /dev/null +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -0,0 +1,85 @@ +/** + * @file butterworth_filter.hpp + * + * @brief Core Butterworth filter class header + * + * @date 09/2024 + */ + +#ifndef MICRAS_CORE_BUTTERWORTH_FILTER_HPP +#define MICRAS_CORE_BUTTERWORTH_FILTER_HPP + +#include +#include + +namespace micras::core { +/** + * @brief Implementation of Butterworth second order low-pass filter + * A generic digital filter follows the relation + * a0 * y[k] = sum(bi * x[k - i]) - sum(aj * y[k - j]) + * Where + * x[k] - measurement at instant k + * y[k] - filtered signal at instant k + * The Butterworth filter have the special property of being a + * maximally flat magnitude filter, in other words, is the best + * filter that doesn't present distortions around the cutoff + * frequency + * The formula for the continuos coefficients of the Butterworth + * filter is available here: + * https://en.wikipedia.org/wiki/Butterworth_filter + * The discrete version were computed with the Tustin method: + * https://en.wikipedia.org/wiki/Bilinear_transform + */ +template +class ButterworthFilter { +public: + /** + * @brief Construct a new Butterworth Second Order filter object + * + * @param cutoff_frequency Low-pass cutoff frequency in Hz + * @param sampling_frequency Sampling frequency in Hz. + */ + explicit ButterworthFilter(float cutoff_frequency, float sampling_frequency = 1.0F); + + /** + * @brief Produces a new value from measured data + * + * @param x0 Last measure + * + * @return Filtered value + */ + float update(float x0); + + /** + * @brief Get the last filtered value + * + * @return Last filtered value + */ + float get_last() const; + +private: + /** + * @brief Last input values of the filter + */ + std::array x_array{}; + + /** + * @brief Last output values of the filter + */ + std::array y_array{}; + + /** + * @brief Coefficients of the filter related to the output value + */ + std::array a_array{}; + + /** + * @brief Coefficients of the filter related to the input value + */ + std::array b_array{}; +}; +} // namespace micras::core + +#include "../src/butterworth_filter.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) + +#endif // MICRAS_CORE_BUTTERWORTH_FILTER_HPP diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp new file mode 100644 index 0000000..ebea1e6 --- /dev/null +++ b/micras_core/include/micras/core/utils.hpp @@ -0,0 +1,92 @@ +/** + * @file utils.hpp + * + * @brief Core Utils functions + * + * @date 09/2024 + */ + +#ifndef MICRAS_CORE_UTILS_HPP +#define MICRAS_CORE_UTILS_HPP + +#include +#include + +namespace micras::core { +/** + * @brief Remap a value from one range to another + * + * @tparam T Type of the value + * @param value Value to be remapped + * @param in_min Minimum value of the input range + * @param in_max Maximum value of the input range + * @param out_min Minimum value of the output range + * @param out_max Maximum value of the output range + * @return T Remapped value + */ +template +inline constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) { + return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +/** + * @brief Move a value towards a target limiting the step size + * + * @tparam T Type of the value + * @param value Current value + * @param target Target value + * @param step Step size + * @return T New value + */ +template +inline constexpr T move_towards(T value, T target, T step) { + return std::max(std::min(value + step, target), value - step); +} + +/** + * @brief Decay a value towards zero with a damping factor + * + * @tparam T Type of the value + * @param value Current value + * @param damping Damping factor + * @return T New value + */ +template +inline constexpr T decay(T value, T damping) { + return damping / (damping + value * value); +} + +/** + * @brief Create an array objects calling its constructors from an array of parameters + * + * @tparam T Type of the array + * @tparam N Size of the array + * @tparam C Type of the parameters + * @param parameters List of parameters + * @return std::array Array with the objects created from the parameters + */ +template +inline constexpr std::array make_array(const std::array& parameters) { + return [&](std::index_sequence) -> std::array { + return {T{parameters[I]}...}; + }(std::make_index_sequence()); +} + +/** + * @brief Create an array objects calling its constructors from a single parameter + * + * @tparam T Type of the array + * @tparam N Size of the array + * @tparam C Type of the parameter + * @param value Parameter + * @return std::array Array with the objects created from the parameter + */ +template +inline constexpr std::array make_array(C value) { + return [&](std::index_sequence) -> std::array { + return {(static_cast(I), T{value})...}; + }(std::make_index_sequence()); +} +} // namespace micras::core + +#endif // MICRAS_CORE_UTILS_HPP diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp new file mode 100644 index 0000000..44ea607 --- /dev/null +++ b/micras_core/src/butterworth_filter.cpp @@ -0,0 +1,65 @@ +#ifndef MICRAS_CORE_BUTTERWORTH_FILTER_CPP +#define MICRAS_CORE_BUTTERWORTH_FILTER_CPP + +#include +#include + +#include "micras/core/butterworth_filter.hpp" + +namespace micras::core { +template +ButterworthFilter::ButterworthFilter(float cutoff_frequency, float sampling_frequency) { + float relative_frequency = cutoff_frequency / sampling_frequency; + float relative_frequency_2 = relative_frequency * relative_frequency; + + float b0 = 1; + float b1 = 2; + float b2 = 1; + + // Butterworth filter coefficients + float a0 = 1 + 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; + float a1 = 2 - 8 / relative_frequency_2; + float a2 = 1 - 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; + + this->a_array[0] = a2 / a0; + this->a_array[1] = a1 / a0; + + this->b_array[0] = b2 / a0; + this->b_array[1] = b1 / a0; + this->b_array[2] = b0 / a0; +} + +template +float ButterworthFilter::update(float x0) { + this->x_array[0] = this->x_array[1]; + this->x_array[1] = this->x_array[2]; + this->x_array[2] = x0; + + float x_b_dot = 0; + + for (int i = 0; i < filter_order + 1; i++) { + x_b_dot += this->x_array[i] * this->b_array[i]; // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index) + } + + float y_a_dot = 0; + + for (int i = 0; i < filter_order; i++) { + y_a_dot += this->y_array[i] * this->a_array[i]; // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index) + } + + float y0 = x_b_dot - y_a_dot; + + this->y_array[0] = this->y_array[1]; + this->y_array[1] = y0; + + return y0; +} + +template +float ButterworthFilter::get_last() const { + return this->y_array[1]; +} + +} // namespace micras::core + +#endif // MICRAS_CORE_BUTTERWORTH_FILTER_CPP diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt index aead3da..88587af 100644 --- a/micras_hal/CMakeLists.txt +++ b/micras_hal/CMakeLists.txt @@ -25,6 +25,10 @@ target_link_libraries(${PROJECT_NAME} PRIVATE stm32cubemx ) +target_link_libraries(${PROJECT_NAME} PUBLIC + micras::core +) + target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${CUBE_INCLUDE_DIRECTORIES} ) From 7e77a02bf0c578d5e3518b341c12bc73fe3190bb Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 24 Sep 2024 21:36:22 -0300 Subject: [PATCH 191/561] =?UTF-8?q?=E2=9C=A8=20Add=20filter=20to=20ADC=20p?= =?UTF-8?q?roxies=20and=20change=20C=20arrays=20to=20std:span?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../micras/core/butterworth_filter.hpp | 2 +- micras_hal/include/micras/hal/adc_dma.hpp | 19 ++++-------- micras_hal/include/micras/hal/pwm_dma.hpp | 7 ++--- micras_hal/include/micras/hal/spi.hpp | 7 ++--- micras_hal/stm32/src/adc_dma.cpp | 17 ++++------- micras_hal/stm32/src/pwm_dma.cpp | 10 +++---- micras_hal/stm32/src/spi.cpp | 10 +++---- micras_proxy/include/micras/proxy/battery.hpp | 12 ++++++++ .../include/micras/proxy/distance_sensors.hpp | 12 ++++++++ micras_proxy/src/argb.cpp | 4 +-- micras_proxy/src/battery.cpp | 12 ++++++-- micras_proxy/src/dip_switch.cpp | 5 ++-- micras_proxy/src/distance_sensors.cpp | 18 ++++++++++-- micras_proxy/src/imu.cpp | 8 ++--- micras_proxy/src/torque_sensors.cpp | 29 ++++++++++++++----- 15 files changed, 103 insertions(+), 69 deletions(-) diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp index 76b3081..e43313f 100644 --- a/micras_core/include/micras/core/butterworth_filter.hpp +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -39,7 +39,7 @@ class ButterworthFilter { * @param cutoff_frequency Low-pass cutoff frequency in Hz * @param sampling_frequency Sampling frequency in Hz. */ - explicit ButterworthFilter(float cutoff_frequency, float sampling_frequency = 1.0F); + explicit ButterworthFilter(float cutoff_frequency, float sampling_frequency = 100.0F); /** * @brief Produces a new value from measured data diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index d006401..a32f3bc 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -11,6 +11,7 @@ #include #include +#include namespace micras::hal { /** @@ -25,7 +26,6 @@ class AdcDma { void (*init_function)(); ADC_HandleTypeDef* handle; uint16_t max_reading; - float reference_voltage; }; /** @@ -39,17 +39,15 @@ class AdcDma { * @brief Enable ADC, start conversion of regular group and transfer result through DMA * * @param buffer Destination Buffer address - * @param size Number of data to be transferred from ADC DMA peripheral to memory */ - void start_dma(uint32_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) + void start_dma(std::span buffer); /** * @brief Enable ADC, start conversion of regular group and transfer result through DMA * * @param buffer Destination Buffer address - * @param size Number of data to be transferred from ADC DMA peripheral to memory */ - void start_dma(uint16_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) + void start_dma(std::span buffer); /** * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode) @@ -64,11 +62,9 @@ class AdcDma { uint16_t get_max_reading() const; /** - * @brief Get the reference voltage for the ADC measurement - * - * @return float Reference voltage for the ADC measurement + * @brief Reference voltage for the ADC measurement */ - float get_reference_voltage() const; + static constexpr float reference_voltage{3.3F}; private: /** @@ -76,11 +72,6 @@ class AdcDma { */ uint16_t max_reading; - /** - * @brief Reference voltage for the ADC measurement - */ - float reference_voltage; - /** * @brief ADC handle */ diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp index ae4bac2..44d8700 100644 --- a/micras_hal/include/micras/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -11,6 +11,7 @@ #include #include +#include namespace micras::hal { /** @@ -38,17 +39,15 @@ class PwmDma { * @brief Start PWM and DMA transfer * * @param buffer Buffer to transfer - * @param size Size of the buffer */ - void start_dma(uint32_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) + void start_dma(std::span buffer); /** * @brief Start PWM and DMA transfer * * @param buffer Buffer to transfer - * @param size Size of the buffer */ - void start_dma(uint16_t buffer[], uint32_t size); // NOLINT(*-avoid-c-arrays) + void start_dma(std::span buffer); /** * @brief Stop PWM and DMA transfer diff --git a/micras_hal/include/micras/hal/spi.hpp b/micras_hal/include/micras/hal/spi.hpp index 4524044..9102bb6 100644 --- a/micras_hal/include/micras/hal/spi.hpp +++ b/micras_hal/include/micras/hal/spi.hpp @@ -10,6 +10,7 @@ #define MICRAS_HAL_SPI_HPP #include +#include #include #include "micras/hal/gpio.hpp" @@ -53,17 +54,15 @@ class Spi { * @brief Transmit data over SPI * * @param data Data to transmit - * @param size Size of the buffer */ - void transmit(uint8_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) + void transmit(std::span data); /** * @brief Receive data over SPI * * @param data Data to receive data - * @param size Size of the data */ - void receive(uint8_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) + void receive(std::span data); private: /** diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 7ad67c2..4b75ea5 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -11,20 +11,17 @@ #include "micras/hal/adc_dma.hpp" namespace micras::hal { -AdcDma::AdcDma(const Config& config) : - max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { +AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, handle{config.handle} { config.init_function(); HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); } -// NOLINTNEXTLINE(*-avoid-c-arrays) -void AdcDma::start_dma(uint32_t buffer[], uint32_t size) { - HAL_ADC_Start_DMA(this->handle, buffer, size); +void AdcDma::start_dma(std::span buffer) { + HAL_ADC_Start_DMA(this->handle, buffer.data(), buffer.size()); } -// NOLINTNEXTLINE(*-avoid-c-arrays) -void AdcDma::start_dma(uint16_t buffer[], uint32_t size) { - this->start_dma(std::bit_cast(buffer), size); +void AdcDma::start_dma(std::span buffer) { + this->start_dma({std::bit_cast(buffer.data()), buffer.size()}); } void AdcDma::stop_dma() { @@ -34,8 +31,4 @@ void AdcDma::stop_dma() { uint16_t AdcDma::get_max_reading() const { return this->max_reading; } - -float AdcDma::get_reference_voltage() const { - return this->reference_voltage; -} } // namespace micras::hal diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index 2f3c01e..2f7ad3c 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -16,18 +16,16 @@ PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.tim config.init_function(); } -// NOLINTNEXTLINE(*-avoid-c-arrays) -void PwmDma::start_dma(uint32_t buffer[], uint32_t size) { +void PwmDma::start_dma(std::span buffer) { if (TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_CHANNEL_STATE_BUSY) { return; } - HAL_TIM_PWM_Start_DMA(this->handle, this->channel, buffer, size); + HAL_TIM_PWM_Start_DMA(this->handle, this->channel, buffer.data(), buffer.size()); } -// NOLINTNEXTLINE(*-avoid-c-arrays) -void PwmDma::start_dma(uint16_t buffer[], uint32_t size) { - start_dma(std::bit_cast(buffer), size); +void PwmDma::start_dma(std::span buffer) { + start_dma({std::bit_cast(buffer.data()), buffer.size()}); } void PwmDma::stop_dma() { diff --git a/micras_hal/stm32/src/spi.cpp b/micras_hal/stm32/src/spi.cpp index 0077087..1e647e9 100644 --- a/micras_hal/stm32/src/spi.cpp +++ b/micras_hal/stm32/src/spi.cpp @@ -26,13 +26,11 @@ void Spi::unselect_device() { this->cs_gpio.write(true); } -// NOLINTNEXTLINE(*-avoid-c-arrays) -void Spi::transmit(uint8_t data[], uint32_t size) { - HAL_SPI_Transmit(this->handle, data, size, this->timeout); +void Spi::transmit(std::span data) { + HAL_SPI_Transmit(this->handle, data.data(), data.size(), this->timeout); } -// NOLINTNEXTLINE(*-avoid-c-arrays) -void Spi::receive(uint8_t data[], uint32_t size) { - HAL_SPI_Receive(this->handle, data, size, this->timeout); +void Spi::receive(std::span data) { + HAL_SPI_Receive(this->handle, data.data(), data.size(), this->timeout); } } // namespace micras::hal diff --git a/micras_proxy/include/micras/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp index c6a4757..0eb8889 100644 --- a/micras_proxy/include/micras/proxy/battery.hpp +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -11,6 +11,7 @@ #include +#include "micras/core/butterworth_filter.hpp" #include "micras/hal/adc_dma.hpp" namespace micras::proxy { @@ -25,6 +26,7 @@ class Battery { struct Config { hal::AdcDma::Config adc; float voltage_divider; + float filter_cutoff; }; /** @@ -34,6 +36,11 @@ class Battery { */ explicit Battery(const Config& config); + /** + * @brief Update the battery reading + */ + void update(); + /** * @brief Get the battery voltage * @@ -63,6 +70,11 @@ class Battery { * @brief Voltage divider ratio */ float voltage_divider; + + /** + * @brief Butterworth filter for the battery reading + */ + core::ButterworthFilter<2> filter; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/distance_sensors.hpp b/micras_proxy/include/micras/proxy/distance_sensors.hpp index c191658..462549c 100644 --- a/micras_proxy/include/micras/proxy/distance_sensors.hpp +++ b/micras_proxy/include/micras/proxy/distance_sensors.hpp @@ -12,6 +12,7 @@ #include #include +#include "micras/core/butterworth_filter.hpp" #include "micras/hal/adc_dma.hpp" #include "micras/hal/pwm.hpp" @@ -30,6 +31,7 @@ class DistanceSensors { hal::Pwm::Config led_0_pwm; hal::Pwm::Config led_1_pwm; float max_distance; + float filter_cutoff; }; /** @@ -46,6 +48,11 @@ class DistanceSensors { */ void set_led_intensity(float intensity); + /** + * @brief Update the distance sensors readings + */ + void update(); + /** * @brief Get the distance from a sensor * @@ -87,6 +94,11 @@ class DistanceSensors { * @brief Maximum distance reading in meters */ float max_distance; + + /** + * @brief Butterworth filter for the ADC readings + */ + std::array, num_of_sensors> filters; }; } // namespace micras::proxy diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index 3c202b6..56dbf00 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -28,7 +28,7 @@ void Argb::set_color(const Color& color, uint8_t index) { } this->encode_color(color * this->brightness, index); - this->pwm.start_dma(this->buffer.data(), this->buffer.size()); + this->pwm.start_dma(this->buffer); } template @@ -37,7 +37,7 @@ void Argb::set_color(const Color& color) { this->encode_color(color * this->brightness, i); } - this->pwm.start_dma(this->buffer.data(), this->buffer.size()); + this->pwm.start_dma(this->buffer); } template diff --git a/micras_proxy/src/battery.cpp b/micras_proxy/src/battery.cpp index c3a9f0d..dbe2be9 100644 --- a/micras_proxy/src/battery.cpp +++ b/micras_proxy/src/battery.cpp @@ -6,15 +6,21 @@ * @date 03/2024 */ +#include "micras/core/utils.hpp" #include "micras/proxy/battery.hpp" namespace micras::proxy { -Battery::Battery(const Config& config) : adc{config.adc}, voltage_divider{config.voltage_divider} { - this->adc.start_dma(&(this->raw_reading), 1); +Battery::Battery(const Config& config) : + adc{config.adc}, voltage_divider{config.voltage_divider}, filter{config.filter_cutoff} { + this->adc.start_dma({&(this->raw_reading), 1}); +} + +void Battery::update() { + this->filter.update(this->raw_reading); } float Battery::get_voltage() const { - return this->voltage_divider * this->raw_reading * this->adc.get_reference_voltage() / this->adc.get_max_reading(); + return this->voltage_divider * this->filter.get_last() * this->adc.reference_voltage / this->adc.get_max_reading(); } uint32_t Battery::get_voltage_raw() const { diff --git a/micras_proxy/src/dip_switch.cpp b/micras_proxy/src/dip_switch.cpp index 55679fb..fe78da7 100644 --- a/micras_proxy/src/dip_switch.cpp +++ b/micras_proxy/src/dip_switch.cpp @@ -9,15 +9,14 @@ #ifndef MICRAS_PROXY_DIP_SWITCH_CPP #define MICRAS_PROXY_DIP_SWITCH_CPP +#include "micras/core/utils.hpp" #include "micras/hal/mcu.hpp" #include "micras/proxy/dip_switch.hpp" namespace micras::proxy { template DipSwitch::DipSwitch(const Config& config) : - gpio_array([&](std::index_sequence)->std::array { - return {hal::Gpio{config.gpio_array[I]}...}; - }(std::make_index_sequence())) { } + gpio_array{core::make_array(config.gpio_array)} { } template bool DipSwitch::get_switch_state(uint8_t switch_index) const { diff --git a/micras_proxy/src/distance_sensors.cpp b/micras_proxy/src/distance_sensors.cpp index 1566fae..f01ffdd 100644 --- a/micras_proxy/src/distance_sensors.cpp +++ b/micras_proxy/src/distance_sensors.cpp @@ -9,13 +9,18 @@ #ifndef MICRAS_PROXY_DISTANCE_SENSORS_CPP #define MICRAS_PROXY_DISTANCE_SENSORS_CPP +#include "micras/core/utils.hpp" #include "micras/proxy/distance_sensors.hpp" namespace micras::proxy { template DistanceSensors::DistanceSensors(const Config& config) : - adc{config.adc}, led_0_pwm{config.led_0_pwm}, led_1_pwm{config.led_1_pwm}, max_distance{config.max_distance} { - this->adc.start_dma(this->buffer.data(), num_of_sensors); + adc{config.adc}, + led_0_pwm{config.led_0_pwm}, + led_1_pwm{config.led_1_pwm}, + max_distance{config.max_distance}, + filters{core::make_array, num_of_sensors>(config.filter_cutoff)} { + this->adc.start_dma(this->buffer); this->led_0_pwm.set_duty_cycle(100.0F); this->led_1_pwm.set_duty_cycle(100.0F); } @@ -26,9 +31,16 @@ void DistanceSensors::set_led_intensity(float intensity) { this->led_1_pwm.set_duty_cycle(intensity); } +template +void DistanceSensors::update() { + for (uint8_t i = 0; i < num_of_sensors; i++) { + this->filter[i].update(this->buffer[i]); + } +} + template float DistanceSensors::get_distance(uint8_t sensor_index) const { - return 30.0 * this->max_distance * this->buffer.at(sensor_index) / this->adc.get_max_reading(); + return 30.0F * this->max_distance * this->filters.at(sensor_index).get_last() / this->adc.get_max_reading(); } template diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 6ecbc2d..c82ba03 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -164,8 +164,8 @@ int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t le while (not spi->select_device()) { } reg |= 0x80; - spi->transmit(®, 1); - spi->receive(bufp, len); + spi->transmit({®, 1}); + spi->receive({bufp, len}); spi->unselect_device(); return 0; @@ -176,8 +176,8 @@ int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint while (not spi->select_device()) { } - spi->transmit(®, 1); - spi->transmit(const_cast(bufp), len); // NOLINT(cppcoreguidelines-pro-type-const-cast) + spi->transmit({®, 1}); + spi->transmit({const_cast(bufp), len}); // NOLINT(cppcoreguidelines-pro-type-const-cast) spi->unselect_device(); return 0; diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index bb47681..15dcbfc 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -9,14 +9,18 @@ #ifndef MICRAS_PROXY_TORQUE_SENSORS_CPP #define MICRAS_PROXY_TORQUE_SENSORS_CPP +#include "micras/core/utils.hpp" #include "micras/hal/timer.hpp" #include "micras/proxy/torque_sensors.hpp" namespace micras::proxy { template TorqueSensors::TorqueSensors(const Config& config) : - adc{config.adc}, shunt_resistor{config.shunt_resistor}, max_torque{config.max_torque} { - this->adc.start_dma(this->buffer.data(), num_of_sensors); + adc{config.adc}, + shunt_resistor{config.shunt_resistor}, + max_torque{config.max_torque}, + filters{core::make_array, num_of_sensors>(config.filter_cutoff)} { + this->adc.start_dma(this->buffer); hal::Timer::sleep_ms(2); @@ -25,17 +29,28 @@ TorqueSensors::TorqueSensors(const Config& config) : } } +template +void TorqueSensors::update() { + for (uint8_t i = 0; i < num_of_sensors; i++) { + this->filter[i].update(this->buffer[i]); + } +} + template float TorqueSensors::get_torque(uint8_t sensor_index) const { - return this->buffer.at(sensor_index) * TorqueSensors::max_torque / this->adc.get_max_reading(); + return this->filters.at(sensor_index).get_last() * TorqueSensors::max_torque / this->adc.get_max_reading(); } template float TorqueSensors::get_current(uint8_t sensor_index) const { - return this->adc.get_reference_voltage() * - ((this->buffer.at(sensor_index) - this->base_reading.at(sensor_index)) / this->adc.get_max_reading() - 0.5F - ) / - this->shunt_resistor; + return this->adc.reference_voltage * + (this->filters.at(sensor_index).get_last() - this->base_reading.at(sensor_index)) / + (this->adc.get_max_reading() * this->shunt_resistor); +} + +template +uint32_t TorqueSensors::get_current_raw(uint8_t sensor_index) const { + return this->buffer.at(sensor_index); } } // namespace micras::proxy From 7772eec3e491b0d5b6b2aaceabe4b535fe56318f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 25 Sep 2024 05:07:32 -0300 Subject: [PATCH 192/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Improve=20filter?= =?UTF-8?q?=20implementation=20with=20adc?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/battery.hpp | 17 +++++--- .../include/micras/proxy/distance_sensors.hpp | 12 +++++- .../include/micras/proxy/torque_sensors.hpp | 42 +++++++++++++++++-- micras_proxy/src/battery.cpp | 16 ++++--- micras_proxy/src/distance_sensors.cpp | 11 +++-- micras_proxy/src/torque_sensors.cpp | 33 ++++++++++----- 6 files changed, 103 insertions(+), 28 deletions(-) diff --git a/micras_proxy/include/micras/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp index 0eb8889..a97484d 100644 --- a/micras_proxy/include/micras/proxy/battery.hpp +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -49,11 +49,18 @@ class Battery { float get_voltage() const; /** - * @brief Get the raw reading from the battery + * @brief Get the battery voltage in volts without the filter applied * - * @return uint32_t Raw reading from the battery + * @return float Battery voltage in volts + */ + float get_voltage_raw() const; + + /** + * @brief Get the battery reading from the ADC + * + * @return float Battery reading from 0 to 1 */ - uint32_t get_voltage_raw() const; + float get_adc_reading() const; private: /** @@ -67,9 +74,9 @@ class Battery { uint16_t raw_reading{}; /** - * @brief Voltage divider ratio + * @brief Maximum voltage that can be read */ - float voltage_divider; + float max_voltage; /** * @brief Butterworth filter for the battery reading diff --git a/micras_proxy/include/micras/proxy/distance_sensors.hpp b/micras_proxy/include/micras/proxy/distance_sensors.hpp index 462549c..5221033 100644 --- a/micras_proxy/include/micras/proxy/distance_sensors.hpp +++ b/micras_proxy/include/micras/proxy/distance_sensors.hpp @@ -65,9 +65,17 @@ class DistanceSensors { * @brief Get the distance from a sensor * * @param sensor_index Index of the sensor - * @return uint32_t Raw reading from the distance sensor + * @return float Raw reading from the distance sensor */ - uint32_t get_distance_raw(uint8_t sensor_index) const; + float get_distance_raw(uint8_t sensor_index) const; + + /** + * @brief Get the ADC reading from a sensor + * + * @param sensor_index Index of the sensor + * @return float ADC reading from the sensor from 0 to 1 + */ + float get_adc_reading(uint8_t sensor_index) const; private: /** diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 23f4c47..4579a88 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -12,6 +12,7 @@ #include #include +#include "micras/core/butterworth_filter.hpp" #include "micras/hal/adc_dma.hpp" namespace micras::proxy { @@ -28,6 +29,7 @@ class TorqueSensors { hal::AdcDma::Config adc; float shunt_resistor; float max_torque; + float filter_cutoff; }; /** @@ -37,6 +39,11 @@ class TorqueSensors { */ explicit TorqueSensors(const Config& config); + /** + * @brief Update the torque sensors readings + */ + void update(); + /** * @brief Get the torque from the sensor * @@ -45,6 +52,14 @@ class TorqueSensors { */ float get_torque(uint8_t sensor_index) const; + /** + * @brief Get the raw torque from the sensor without filtering + * + * @param sensor_index Index of the sensor + * @return float Raw torque reading from the sensor + */ + float get_torque_raw(uint8_t sensor_index) const; + /** * @brief Get the current from the sensor * @@ -53,6 +68,22 @@ class TorqueSensors { */ float get_current(uint8_t sensor_index) const; + /** + * @brief Get the raw current from the sensor without filtering + * + * @param sensor_index Index of the sensor + * @return float Raw current reading from the sensor + */ + float get_current_raw(uint8_t sensor_index) const; + + /** + * @brief Get the ADC reading from the sensor + * + * @param sensor_index Index of the sensor + * @return float Adc reading from the sensor from 0 to 1 + */ + float get_adc_reading(uint8_t sensor_index) const; + private: /** * @brief ADC DMA handle @@ -67,17 +98,22 @@ class TorqueSensors { /** * @brief Reading of each sensor when no current is flowing */ - std::array base_reading; + std::array base_reading{}; /** - * @brief Value of the shunt resistor in ohms + * @brief Value of the maximum current that can be measured by the sensor */ - float shunt_resistor; + float max_current; /** * @brief Maximum torque that can be measured by the sensor */ float max_torque; + + /** + * @brief Butterworth filters for the torque reading + */ + std::array, num_of_sensors> filters; }; } // namespace micras::proxy diff --git a/micras_proxy/src/battery.cpp b/micras_proxy/src/battery.cpp index dbe2be9..c0a27ef 100644 --- a/micras_proxy/src/battery.cpp +++ b/micras_proxy/src/battery.cpp @@ -11,19 +11,25 @@ namespace micras::proxy { Battery::Battery(const Config& config) : - adc{config.adc}, voltage_divider{config.voltage_divider}, filter{config.filter_cutoff} { + adc{config.adc}, + max_voltage{hal::AdcDma::reference_voltage * config.voltage_divider}, + filter{config.filter_cutoff} { this->adc.start_dma({&(this->raw_reading), 1}); } void Battery::update() { - this->filter.update(this->raw_reading); + this->filter.update(this->get_adc_reading()); } float Battery::get_voltage() const { - return this->voltage_divider * this->filter.get_last() * this->adc.reference_voltage / this->adc.get_max_reading(); + return this->filter.get_last() * this->max_voltage; } -uint32_t Battery::get_voltage_raw() const { - return this->raw_reading; +float Battery::get_voltage_raw() const { + return this->get_adc_reading() * this->max_voltage; +} + +float Battery::get_adc_reading() const { + return static_cast(this->raw_reading) / this->adc.get_max_reading(); } } // namespace micras::proxy diff --git a/micras_proxy/src/distance_sensors.cpp b/micras_proxy/src/distance_sensors.cpp index f01ffdd..69d6639 100644 --- a/micras_proxy/src/distance_sensors.cpp +++ b/micras_proxy/src/distance_sensors.cpp @@ -40,12 +40,17 @@ void DistanceSensors::update() { template float DistanceSensors::get_distance(uint8_t sensor_index) const { - return 30.0F * this->max_distance * this->filters.at(sensor_index).get_last() / this->adc.get_max_reading(); + return this->filters.at(sensor_index).get_last() * this->max_distance; } template -uint32_t DistanceSensors::get_distance_raw(uint8_t sensor_index) const { - return this->buffer.at(sensor_index); +float DistanceSensors::get_distance_raw(uint8_t sensor_index) const { + return this->get_adc_reading(sensor_index) * this->max_distance; +} + +template +float DistanceSensors::get_adc_reading(uint8_t sensor_index) const { + return static_cast(this->buffer.at(sensor_index)) / this->adc.get_max_reading(); } } // namespace micras::proxy diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index 15dcbfc..78b034e 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -17,40 +17,53 @@ namespace micras::proxy { template TorqueSensors::TorqueSensors(const Config& config) : adc{config.adc}, - shunt_resistor{config.shunt_resistor}, + max_current{AdcDma::reference_voltage / config.shunt_resistor}, max_torque{config.max_torque}, filters{core::make_array, num_of_sensors>(config.filter_cutoff)} { this->adc.start_dma(this->buffer); - hal::Timer::sleep_ms(2); + for (uint8_t i = 0; i < 50; i++) { + hal::Timer::sleep_ms(2); + this->update(); + } for (uint8_t i = 0; i < num_of_sensors; i++) { - this->base_reading.at(i) = this->buffer.at(i); + this->base_reading[i] = this->filters[sensor_index].get_last(); } } template void TorqueSensors::update() { for (uint8_t i = 0; i < num_of_sensors; i++) { - this->filter[i].update(this->buffer[i]); + this->filters[i].update(this->get_adc_reading(i)); } } template float TorqueSensors::get_torque(uint8_t sensor_index) const { - return this->filters.at(sensor_index).get_last() * TorqueSensors::max_torque / this->adc.get_max_reading(); + return this->filters.at(sensor_index).get_last() * this->max_torque; +} + +template +float TorqueSensors::get_torque_raw(uint8_t sensor_index) const { + return this->get_adc_reading(sensor_index) * this->max_torque; } template float TorqueSensors::get_current(uint8_t sensor_index) const { - return this->adc.reference_voltage * - (this->filters.at(sensor_index).get_last() - this->base_reading.at(sensor_index)) / - (this->adc.get_max_reading() * this->shunt_resistor); + return this->filters.at(sensor_index).get_last() * this->max_current; +} + +template +float TorqueSensors::get_current_raw(uint8_t sensor_index) const { + return this->get_adc_reading(sensor_index) * this->max_current; + ; } template -uint32_t TorqueSensors::get_current_raw(uint8_t sensor_index) const { - return this->buffer.at(sensor_index); +float TorqueSensors::get_adc_reading(uint8_t sensor_index) const { + return static_cast(this->buffer.at(sensor_index) - this->base_reading.at(sensor_index)) / + this->adc.get_max_reading(); } } // namespace micras::proxy From 14b8b695b2b08c7f4ffcc4550953592b9ff6db17 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 25 Sep 2024 05:08:24 -0300 Subject: [PATCH 193/561] =?UTF-8?q?=F0=9F=A9=B9=20Fix=20buzzer=20stutterin?= =?UTF-8?q?g?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/buzzer.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_proxy/include/micras/proxy/buzzer.hpp b/micras_proxy/include/micras/proxy/buzzer.hpp index 03068da..08cad67 100644 --- a/micras_proxy/include/micras/proxy/buzzer.hpp +++ b/micras_proxy/include/micras/proxy/buzzer.hpp @@ -73,12 +73,12 @@ class Buzzer { /** * @brief Flag to check if the buzzer is playing */ - bool is_playing{false}; + bool is_playing{}; /** * @brief Duration of the sound */ - uint32_t duration{0}; + uint32_t duration{}; }; } // namespace micras::proxy From 062a85b51053ed47ab476f852549c0dfc002e7c1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 25 Sep 2024 05:09:17 -0300 Subject: [PATCH 194/561] =?UTF-8?q?=E2=8F=AA=20Revert=20bit=5Fcast=20ot=20?= =?UTF-8?q?reinterpret=5Fcast=20in=20storage?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/storage.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index 5094953..b70e27e 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -9,7 +9,6 @@ #ifndef MICRAS_PROXY_STORAGE_HPP #define MICRAS_PROXY_STORAGE_HPP -#include #include #include #include @@ -73,7 +72,8 @@ class Storage { template void sync(const std::string& name, T& data) { if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr) { - data = std::bit_cast(this->buffer.at(this->primitives.at(name).buffer_address)); + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) + data = reinterpret_cast(this->buffer.at(this->primitives.at(name).buffer_address)); } this->create(name, data); From 18d5fb3ed21b66f1053a101b3b41be3e86dbd1ad Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 25 Sep 2024 05:11:23 -0300 Subject: [PATCH 195/561] =?UTF-8?q?=E2=9C=A8=20Add=20acceleration=20ramp?= =?UTF-8?q?=20to=20the=20fan=20driver?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/fan.hpp | 27 +++++++++++++++++++++++ micras_proxy/src/fan.cpp | 27 ++++++++++++++++++----- 2 files changed, 49 insertions(+), 5 deletions(-) diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index 1f70366..05b0015 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -13,6 +13,7 @@ #include "micras/hal/gpio.hpp" #include "micras/hal/pwm.hpp" +#include "micras/hal/timer.hpp" namespace micras::proxy { /** @@ -27,6 +28,7 @@ class Fan { hal::Pwm::Config pwm; hal::Gpio::Config direction_gpio; hal::Gpio::Config enable_gpio; + float max_acceleration; }; /** @@ -53,6 +55,11 @@ class Fan { */ void set_speed(float speed); + /** + * @brief Update the speed of the fan + */ + float update_speed(); + /** * @brief Stop the fan */ @@ -88,6 +95,26 @@ class Fan { * @brief GPIO handle for the fan enable pin */ hal::Gpio enable_gpio; + + /** + * @brief Current speed of the fan + */ + float current_speed{}; + + /** + * @brief Target speed of the fan + */ + float target_speed{}; + + /** + * @brief Maximum acceleration of the fan in percentage per millisecond + */ + float max_acceleration; + + /** + * @brief Timer for limiting the acceleration of the fan + */ + hal::Timer acceleration_timer; }; } // namespace micras::proxy diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp index b8a044c..9b3c4da 100644 --- a/micras_proxy/src/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -6,11 +6,15 @@ * @date 03/2024 */ +#include "micras/core/utils.hpp" #include "micras/proxy/fan.hpp" namespace micras::proxy { Fan::Fan(const Config& config) : - pwm{config.pwm}, direction_gpio{config.direction_gpio}, enable_gpio{config.enable_gpio} { + pwm{config.pwm}, + direction_gpio{config.direction_gpio}, + enable_gpio{config.enable_gpio}, + max_acceleration{config.max_acceleration} { this->stop(); this->enable(); } @@ -24,15 +28,28 @@ void Fan::disable() { } void Fan::set_speed(float speed) { - if (speed > 0.0F) { + this->update_speed(); + this->target_speed = speed; +} + +float Fan::update_speed() { + this->current_speed = core::move_towards( + this->current_speed, this->target_speed, this->acceleration_timer.elapsed_time_ms() * this->max_acceleration + ); + + this->acceleration_timer.reset_ms(); + + if (this->current_speed > 0.0F) { this->set_direction(RotationDirection::FORWARD); - this->pwm.set_duty_cycle(speed); - } else if (speed < 0.0F) { + this->pwm.set_duty_cycle(this->current_speed); + } else if (this->current_speed < 0.0F) { this->set_direction(RotationDirection::BACKWARD); - this->pwm.set_duty_cycle(-speed); + this->pwm.set_duty_cycle(-this->current_speed); } else { this->stop(); } + + return this->current_speed; } void Fan::stop() { From 7a34e7b1f8af215a24fc9086c63867aaa7d25d01 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 25 Sep 2024 05:13:17 -0300 Subject: [PATCH 196/561] =?UTF-8?q?=F0=9F=9A=A8=20Add=20missing=20float=20?= =?UTF-8?q?type=20indicator=20to=20constants?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index ef2f842..623d73b 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -46,7 +46,7 @@ void Odometry::update() { float angular_distance = (right_distance - left_distance) / this->wheel_separation; float linear_diagonal{}; - if (angular_distance < 0.05) { + if (angular_distance < 0.05F) { linear_diagonal = linear_distance; } else { linear_diagonal = std::abs( From 20a7702888985cd8c1060454548b262759038e78 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 25 Sep 2024 06:13:08 -0300 Subject: [PATCH 197/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= =?UTF-8?q?=20on=20tests?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 2 +- micras_core/include/micras/core/utils.hpp | 12 ++++++------ micras_hal/include/micras/hal/pwm_dma.hpp | 2 +- micras_proxy/src/distance_sensors.cpp | 2 +- micras_proxy/src/torque_sensors.cpp | 4 ++-- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 5533bf6..bf997e9 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -17,6 +17,7 @@ Checks: misc-*, -misc-include-cleaner, -misc-const-correctness, + -misc-use-anonymous-namespace, modernize-*, -modernize-use-nodiscard, -modernize-use-trailing-return-type, @@ -39,7 +40,6 @@ ImplementationFileExtensions: HeaderFilterRegex: '(include|config)/.*.hpp' FormatStyle: none CheckOptions: - cppcoreguidelines-avoid-non-const-global-variables.AllowInternalLinkage: true readability-magic-numbers.IgnorePowersOf2IntegerValues: true readability-magic-numbers.IgnoreBitFieldsWidths: true readability-magic-numbers.IgnoreUserDefinedLiterals: true diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index ebea1e6..4fe3cd2 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -9,8 +9,8 @@ #ifndef MICRAS_CORE_UTILS_HPP #define MICRAS_CORE_UTILS_HPP -#include #include +#include namespace micras::core { /** @@ -25,7 +25,7 @@ namespace micras::core { * @return T Remapped value */ template -inline constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) { +constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) { return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } @@ -39,7 +39,7 @@ inline constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) { * @return T New value */ template -inline constexpr T move_towards(T value, T target, T step) { +constexpr T move_towards(T value, T target, T step) { return std::max(std::min(value + step, target), value - step); } @@ -52,7 +52,7 @@ inline constexpr T move_towards(T value, T target, T step) { * @return T New value */ template -inline constexpr T decay(T value, T damping) { +constexpr T decay(T value, T damping) { return damping / (damping + value * value); } @@ -66,7 +66,7 @@ inline constexpr T decay(T value, T damping) { * @return std::array Array with the objects created from the parameters */ template -inline constexpr std::array make_array(const std::array& parameters) { +constexpr std::array make_array(const std::array& parameters) { return [&](std::index_sequence) -> std::array { return {T{parameters[I]}...}; }(std::make_index_sequence()); @@ -82,7 +82,7 @@ inline constexpr std::array make_array(const std::array& parameters) * @return std::array Array with the objects created from the parameter */ template -inline constexpr std::array make_array(C value) { +constexpr std::array make_array(C value) { return [&](std::index_sequence) -> std::array { return {(static_cast(I), T{value})...}; }(std::make_index_sequence()); diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp index 44d8700..cf688de 100644 --- a/micras_hal/include/micras/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -10,8 +10,8 @@ #define MICRAS_HAL_PWM_DMA_HPP #include -#include #include +#include namespace micras::hal { /** diff --git a/micras_proxy/src/distance_sensors.cpp b/micras_proxy/src/distance_sensors.cpp index 69d6639..7c61d0d 100644 --- a/micras_proxy/src/distance_sensors.cpp +++ b/micras_proxy/src/distance_sensors.cpp @@ -34,7 +34,7 @@ void DistanceSensors::set_led_intensity(float intensity) { template void DistanceSensors::update() { for (uint8_t i = 0; i < num_of_sensors; i++) { - this->filter[i].update(this->buffer[i]); + this->filters[i].update(this->buffer[i]); } } diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index 78b034e..29f0d0f 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -17,7 +17,7 @@ namespace micras::proxy { template TorqueSensors::TorqueSensors(const Config& config) : adc{config.adc}, - max_current{AdcDma::reference_voltage / config.shunt_resistor}, + max_current{hal::AdcDma::reference_voltage / config.shunt_resistor}, max_torque{config.max_torque}, filters{core::make_array, num_of_sensors>(config.filter_cutoff)} { this->adc.start_dma(this->buffer); @@ -28,7 +28,7 @@ TorqueSensors::TorqueSensors(const Config& config) : } for (uint8_t i = 0; i < num_of_sensors; i++) { - this->base_reading[i] = this->filters[sensor_index].get_last(); + this->base_reading[i] = this->filters[i].get_last(); } } From 1b283b29b13420534094d93e4284114b1a3f324a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 25 Sep 2024 16:23:18 -0300 Subject: [PATCH 198/561] =?UTF-8?q?=F0=9F=8E=A8=20Move=20argb=20high=20and?= =?UTF-8?q?=20low=20duty=20cycle=20to=20config=20struct?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/argb.hpp | 4 ++-- micras_proxy/src/argb.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index d0411df..9cafdd0 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -26,6 +26,8 @@ class Argb { */ struct Config { hal::PwmDma::Config pwm; + float low_duty_cycle; + float high_duty_cycle; float max_brightness; }; @@ -95,8 +97,6 @@ class Argb { static constexpr uint8_t bits_per_color{8}; static constexpr uint8_t colors_per_led{3}; static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; - static constexpr float low_duty_cycle{32.0F}; // NOLINT(bugprone-dynamic-static-initializers) - static constexpr float high_duty_cycle{64.0F}; // NOLINT(bugprone-dynamic-static-initializers) static constexpr uint8_t reset_length{225}; /** diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index 56dbf00..e7b451b 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -15,8 +15,8 @@ namespace micras::proxy { template Argb::Argb(const Config& config) : pwm{config.pwm}, - low_bit{pwm.get_compare(low_duty_cycle)}, - high_bit{pwm.get_compare(high_duty_cycle)}, + low_bit{pwm.get_compare(config.low_duty_cycle)}, + high_bit{pwm.get_compare(config.high_duty_cycle)}, brightness{config.max_brightness / 100.0F} { this->set_color({0, 0, 0}); } From f9038bd4e8f6343339e210ea170f12ac7228ed32 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 25 Sep 2024 16:55:46 -0300 Subject: [PATCH 199/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20storage=20bug=20of?= =?UTF-8?q?=20erasing=20element=20while=20iterating?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/storage.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 31b803d..6b7c38c 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -48,20 +48,25 @@ void Storage::save() { this->buffer.clear(); hal::Flash::erase_pages(this->start_page, this->number_of_pages); - for (auto& [name, variable] : this->primitives) { + for (auto it = this->primitives.begin(); it != this->primitives.end();) { + auto& [name, variable] = *it; + if (variable.ram_pointer == nullptr) { - this->primitives.erase(name); + it = this->primitives.erase(it); continue; } const auto* aux = std::bit_cast(variable.ram_pointer); variable.buffer_address = buffer.size(); this->buffer.insert(this->buffer.end(), aux, aux + variable.size); + it++; } - for (auto& [name, variable] : this->serializables) { + for (auto it = this->serializables.begin(); it != this->serializables.end();) { + auto& [name, variable] = *it; + if (variable.ram_pointer == nullptr) { - this->serializables.erase(name); + it = this->serializables.erase(it); continue; } @@ -69,6 +74,7 @@ void Storage::save() { variable.buffer_address = this->buffer.size(); variable.size = aux.size(); this->buffer.insert(this->buffer.end(), aux.begin(), aux.end()); + it++; } auto serialized_serializables = serialize_var_map(this->serializables); From 418a315ca1666e05e0bab76b48560df8cef5190e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 30 Sep 2024 06:12:02 -0300 Subject: [PATCH 200/561] =?UTF-8?q?=F0=9F=9A=91=20Fix=20flash=20memory=20e?= =?UTF-8?q?rase=20error?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/flash.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index 69c4ada..0f7ed0f 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -63,13 +63,15 @@ void Flash::write(uint16_t page, uint16_t page_address, const uint64_t data[], u void Flash::erase_pages(uint16_t page, uint16_t number_of_pages) { FLASH_EraseInitTypeDef erase_struct = { .TypeErase = FLASH_TYPEERASE_PAGES, - .Banks = 0, + .Banks = FLASH_BANK_2, .Page = FLASH_PAGE_NB - page - number_of_pages, .NbPages = number_of_pages, }; uint32_t page_error{}; + HAL_FLASH_Unlock(); HAL_FLASHEx_Erase(&erase_struct, &page_error); + HAL_FLASH_Lock(); } } // namespace micras::hal From f4b34d5494f1955e8c5a582c9b217564a9b12a1b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 30 Sep 2024 06:17:58 -0300 Subject: [PATCH 201/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20torque=20sensors?= =?UTF-8?q?=20base=20reading?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/torque_sensors.hpp | 2 +- micras_proxy/src/torque_sensors.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 4579a88..61050f1 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -98,7 +98,7 @@ class TorqueSensors { /** * @brief Reading of each sensor when no current is flowing */ - std::array base_reading{}; + std::array base_reading{}; /** * @brief Value of the maximum current that can be measured by the sensor diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index 29f0d0f..8c4a15b 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -62,8 +62,8 @@ float TorqueSensors::get_current_raw(uint8_t sensor_index) const template float TorqueSensors::get_adc_reading(uint8_t sensor_index) const { - return static_cast(this->buffer.at(sensor_index) - this->base_reading.at(sensor_index)) / - this->adc.get_max_reading(); + return static_cast(this->buffer.at(sensor_index)) / this->adc.get_max_reading() - + this->base_reading.at(sensor_index); } } // namespace micras::proxy From 63817f6fbb767e55e43dc898f77c4eb41f0352de Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 07:29:18 -0300 Subject: [PATCH 202/561] =?UTF-8?q?=E2=9C=A8=20Add=20assert=5Fangle=20func?= =?UTF-8?q?tion=20to=20core=20utils?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/utils.hpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index 4fe3cd2..0e5bde8 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -11,6 +11,8 @@ #include #include +#include +#include namespace micras::core { /** @@ -87,6 +89,17 @@ constexpr std::array make_array(C value) { return {(static_cast(I), T{value})...}; }(std::make_index_sequence()); } + +/** + * @brief Assert an angle to be in the range [-pi, pi] + * + * @param angle Angle to be asserted + * @return float Asserted angle + */ +constexpr float assert_angle(float angle) { + angle = std::fmod(angle, 2 * std::numbers::pi_v); + return angle > std::numbers::pi_v ? angle - 2 * std::numbers::pi_v : angle; +} } // namespace micras::core #endif // MICRAS_CORE_UTILS_HPP From fe39751ac1b637db3f16a0fff5f2a66f198ceb46 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 07:33:41 -0300 Subject: [PATCH 203/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20template=20from?= =?UTF-8?q?=20butterworth=20filter?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/core/butterworth_filter.hpp | 8 +++++--- micras_core/src/butterworth_filter.cpp | 15 +++------------ micras_proxy/include/micras/proxy/battery.hpp | 2 +- .../include/micras/proxy/distance_sensors.hpp | 2 +- .../include/micras/proxy/torque_sensors.hpp | 2 +- micras_proxy/src/distance_sensors.cpp | 2 +- micras_proxy/src/torque_sensors.cpp | 2 +- 7 files changed, 13 insertions(+), 20 deletions(-) diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp index e43313f..f9bc3c4 100644 --- a/micras_core/include/micras/core/butterworth_filter.hpp +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -30,7 +30,6 @@ namespace micras::core { * The discrete version were computed with the Tustin method: * https://en.wikipedia.org/wiki/Bilinear_transform */ -template class ButterworthFilter { public: /** @@ -58,6 +57,11 @@ class ButterworthFilter { float get_last() const; private: + /** + * @brief Order of the filter + */ + static constexpr uint8_t filter_order{2}; + /** * @brief Last input values of the filter */ @@ -80,6 +84,4 @@ class ButterworthFilter { }; } // namespace micras::core -#include "../src/butterworth_filter.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) - #endif // MICRAS_CORE_BUTTERWORTH_FILTER_HPP diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index 44ea607..3e783e6 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -1,14 +1,10 @@ -#ifndef MICRAS_CORE_BUTTERWORTH_FILTER_CPP -#define MICRAS_CORE_BUTTERWORTH_FILTER_CPP - #include #include #include "micras/core/butterworth_filter.hpp" namespace micras::core { -template -ButterworthFilter::ButterworthFilter(float cutoff_frequency, float sampling_frequency) { +ButterworthFilter::ButterworthFilter(float cutoff_frequency, float sampling_frequency) { float relative_frequency = cutoff_frequency / sampling_frequency; float relative_frequency_2 = relative_frequency * relative_frequency; @@ -29,8 +25,7 @@ ButterworthFilter::ButterworthFilter(float cutoff_frequency, float this->b_array[2] = b0 / a0; } -template -float ButterworthFilter::update(float x0) { +float ButterworthFilter::update(float x0) { this->x_array[0] = this->x_array[1]; this->x_array[1] = this->x_array[2]; this->x_array[2] = x0; @@ -55,11 +50,7 @@ float ButterworthFilter::update(float x0) { return y0; } -template -float ButterworthFilter::get_last() const { +float ButterworthFilter::get_last() const { return this->y_array[1]; } - } // namespace micras::core - -#endif // MICRAS_CORE_BUTTERWORTH_FILTER_CPP diff --git a/micras_proxy/include/micras/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp index a97484d..1eabdb9 100644 --- a/micras_proxy/include/micras/proxy/battery.hpp +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -81,7 +81,7 @@ class Battery { /** * @brief Butterworth filter for the battery reading */ - core::ButterworthFilter<2> filter; + core::ButterworthFilter filter; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/distance_sensors.hpp b/micras_proxy/include/micras/proxy/distance_sensors.hpp index 5221033..fd0f2de 100644 --- a/micras_proxy/include/micras/proxy/distance_sensors.hpp +++ b/micras_proxy/include/micras/proxy/distance_sensors.hpp @@ -106,7 +106,7 @@ class DistanceSensors { /** * @brief Butterworth filter for the ADC readings */ - std::array, num_of_sensors> filters; + std::array filters; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 61050f1..02c4b04 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -113,7 +113,7 @@ class TorqueSensors { /** * @brief Butterworth filters for the torque reading */ - std::array, num_of_sensors> filters; + std::array filters; }; } // namespace micras::proxy diff --git a/micras_proxy/src/distance_sensors.cpp b/micras_proxy/src/distance_sensors.cpp index 7c61d0d..3c5e546 100644 --- a/micras_proxy/src/distance_sensors.cpp +++ b/micras_proxy/src/distance_sensors.cpp @@ -19,7 +19,7 @@ DistanceSensors::DistanceSensors(const Config& config) : led_0_pwm{config.led_0_pwm}, led_1_pwm{config.led_1_pwm}, max_distance{config.max_distance}, - filters{core::make_array, num_of_sensors>(config.filter_cutoff)} { + filters{core::make_array(config.filter_cutoff)} { this->adc.start_dma(this->buffer); this->led_0_pwm.set_duty_cycle(100.0F); this->led_1_pwm.set_duty_cycle(100.0F); diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index 8c4a15b..3f9b82d 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -19,7 +19,7 @@ TorqueSensors::TorqueSensors(const Config& config) : adc{config.adc}, max_current{hal::AdcDma::reference_voltage / config.shunt_resistor}, max_torque{config.max_torque}, - filters{core::make_array, num_of_sensors>(config.filter_cutoff)} { + filters{core::make_array(config.filter_cutoff)} { this->adc.start_dma(this->buffer); for (uint8_t i = 0; i < 50; i++) { From 211af66a4684b01e8f65dae17791360bf8a98c91 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 07:34:22 -0300 Subject: [PATCH 204/561] =?UTF-8?q?=F0=9F=9A=9A=20Change=20fan=20method=20?= =?UTF-8?q?name=20from=20update=5Fspeed=20to=20update?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/fan.hpp | 2 +- micras_proxy/src/fan.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index 05b0015..a8a8115 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -58,7 +58,7 @@ class Fan { /** * @brief Update the speed of the fan */ - float update_speed(); + float update(); /** * @brief Stop the fan diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp index 9b3c4da..5ed3e24 100644 --- a/micras_proxy/src/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -28,11 +28,11 @@ void Fan::disable() { } void Fan::set_speed(float speed) { - this->update_speed(); + this->update(); this->target_speed = speed; } -float Fan::update_speed() { +float Fan::update() { this->current_speed = core::move_towards( this->current_speed, this->target_speed, this->acceleration_timer.elapsed_time_ms() * this->max_acceleration ); From ddfccfd2071b528625482711339d599c15b27b92 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 07:50:46 -0300 Subject: [PATCH 205/561] =?UTF-8?q?=F0=9F=A5=85=20Fix=20rotary=5Fsensor=20?= =?UTF-8?q?register=20communication?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/rotary_sensor.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index b18067f..5ee80e5 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -50,15 +50,17 @@ float RotarySensor::get_position() const { } uint16_t RotarySensor::read_register(uint16_t address) { - CommandFrame command_frame = {{.do_not_care = 0, .rw = 1, .address = address, .crc = 0}}; - DataFrame data_frame; + CommandFrame command_frame = {{.crc = 0, .address = address, .rw = 1, .do_not_care = 0}}; + DataFrame data_frame{}; command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2) ^ 0xFF; while (not this->spi.select_device()) { } + this->spi.transmit({std::bit_cast(&command_frame.raw), 3}); + this->spi.unselect_device(); - this->spi.transmit(reinterpret_cast(&command_frame.raw), 3); - this->spi.receive(reinterpret_cast(&data_frame.raw), 3); + while (not this->spi.select_device()) { } + this->spi.receive({std::bit_cast(&data_frame.raw), 3}); this->spi.unselect_device(); return data_frame.fields.data; @@ -69,9 +71,11 @@ void RotarySensor::write_register(CommandFrame& command_frame, DataFrame& data_f data_frame.fields.crc = this->crc.calculate(&data_frame.raw, 2) ^ 0xFF; while (not this->spi.select_device()) { } + this->spi.transmit({std::bit_cast(&command_frame.raw), 3}); + this->spi.unselect_device(); - this->spi.transmit(reinterpret_cast(&command_frame.raw), 3); - this->spi.transmit(reinterpret_cast(&data_frame.raw), 3); + while (not this->spi.select_device()) { } + this->spi.transmit({std::bit_cast(&data_frame.raw), 3}); this->spi.unselect_device(); } } // namespace micras::proxy From dd4ddf63dbe485ed394ec9e84728087b8718b3d1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 08:55:42 -0300 Subject: [PATCH 206/561] =?UTF-8?q?=E2=9C=A8=20Add=20maze=20solving=20algo?= =?UTF-8?q?rithm=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/grid_pose.hpp | 161 +++++++++++++++++ micras_nav/include/micras/nav/maze.hpp | 126 +++++++++++++ micras_nav/src/grid_pose.cpp | 76 ++++++++ micras_nav/src/maze.cpp | 190 ++++++++++++++++++++ 4 files changed, 553 insertions(+) create mode 100644 micras_nav/include/micras/nav/grid_pose.hpp create mode 100644 micras_nav/include/micras/nav/maze.hpp create mode 100644 micras_nav/src/grid_pose.cpp create mode 100644 micras_nav/src/maze.cpp diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp new file mode 100644 index 0000000..268d75f --- /dev/null +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -0,0 +1,161 @@ +/** + * @file grid_pose.hpp + * + * @brief Types for grid-based navigation + * + * @date 10/2024 + */ + +#ifndef MICRAS_NAV_GRID_POSE_HPP +#define MICRAS_NAV_GRID_POSE_HPP + +#include +#include + +namespace micras::nav { +/** + * @brief Possible directions in the grid + */ +enum Side : uint8_t { + RIGHT = 0, + UP = 1, + LEFT = 2, + DOWN = 3 +}; + +/** + * @brief Converts an angle in radians to a Grid side + * + * @param angle The angle in radians + * @return The corresponding Grid side + */ +Side angle_to_grid(float angle); + +/** + * @brief Type to store information originating from the distance sensors + */ +struct Information { + /** + * @brief Possible values for the existence of a wall + */ + enum Existence : uint8_t { + UNKNOWN = 0, + WALL = 1, + FREE = 2 + }; + + /** + * @brief Possible walls in the grid to be checked + */ + Existence left; + Existence front_left; + Existence front; + Existence front_right; + Existence right; +}; + +/** + * @brief Type to store a point in the grid + */ +struct GridPoint { + /** + * @brief Returns the direction from this point to the next one + * + * @param next The next point + * @return The direction from this point to the next one + */ + Side direction(const GridPoint& next) const; + + /** + * @brief Moves in the grid in the direction of the side + * + * @param side The side to move to + * @return The new point after moving + */ + GridPoint operator+(const Side& side) const; + + /** + * @brief Compares two points for equality + * + * @param other The other point to compare + * @return True if the points are equal, false otherwise + */ + bool operator==(const GridPoint& other) const; + + /** + * @brief The x coordinate of the point on the grid + */ + uint8_t x; + + /** + * @brief The y coordinate of the point on the grid + */ + uint8_t y; +}; + +struct GridPose { + /** + * @brief Returns the pose after moving forward + * + * @return The pose after moving forward + */ + GridPose front() const; + + /** + * @brief Returns the pose after turning back + * + * @return The pose after turning back + */ + GridPose turned_back() const; + + /** + * @brief Returns the pose after turning left + * + * @return The pose after turning left + */ + GridPose turned_left() const; + + /** + * @brief Returns the pose after turning right + * + * @return The pose after turning right + */ + GridPose turned_right() const; + + /** + * @brief Compares two poses for equality + * + * @param other The other pose to compare + * @return True if the poses are equal, false otherwise + */ + bool operator==(const GridPose& other) const; + + /** + * @brief The position of the pose on the grid + */ + GridPoint position; + + /** + * @brief The orientation of the pose on the grid + */ + Side orientation; +}; +} // namespace micras::nav + +namespace std { +/** + * @brief Hash specialization for the GridPoint type + * + * @tparam T GridPoint type + */ +template <> +struct hash { + size_t operator()(const micras::nav::GridPoint& point) const noexcept { + size_t h1 = hash{}(point.x); + size_t h2 = hash{}(point.y); + return h1 ^ (h2 << 1); + } +}; +} // namespace std + +#endif // MICRAS_NAV_GRID_POSE_HPP diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp new file mode 100644 index 0000000..bdbf929 --- /dev/null +++ b/micras_nav/include/micras/nav/maze.hpp @@ -0,0 +1,126 @@ +/** + * @file maze.hpp + * + * @brief Nav Maze class declaration + * + * @date 10/2024 + */ + +#ifndef MICRAS_NAV_MAZE_HPP +#define MICRAS_NAV_MAZE_HPP + +#include +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +/** + * @brief Class for storing the robot information about the maze + * + * @tparam width The width of the maze + * @tparam height The height of the maze + */ +template +class Maze { +public: + /** + * @brief Construct a new Maze object + * + * @param start The start pose of the robot + */ + explicit Maze(const GridPose& start); + + /** + * @brief Updates the maze walls with the current pose and new information + * + * @param pose The pose of the robot + * @param information The information from the distance sensors + */ + void update(const GridPose& pose, Information information); + + /** + * @brief Returns the next point the robot should go to + * + * @param position The current position of the robot + * @param force_costmap Whether to force the use of the update of the costmap + * @return The next point the robot should go to + */ + GridPoint get_current_goal(const GridPoint& position, bool force_costmap = false) const; + +private: + /** + * @brief Type to store the information of a cell in the maze + */ + struct Cell { + std::array wall_count{}; + std::array free_count{}; + uint16_t cost{0xFFFF}; + }; + + /** + * @brief Calculates the costmap for the flood fill algorithm + */ + void calculate_costmap(); + + /** + * @brief Returns the cell at the given position + * + * @param position The position of the cell + * @return The cell at the given position + */ + const Cell& get_cell(const GridPoint& position) const; + + /** + * @brief Returns the cell at the given position + * + * @param position The position of the cell + * @return The cell at the given position + */ + Cell& get_cell(const GridPoint& position); + + /** + * @brief Update the probability of a wall in the maze + * + * @param pose The pose of the robot + * @param wall Whether there is a wall in front of the robot + */ + void update_wall(const GridPose& pose, bool wall); + + /** + * @brief Cells matrix representing the maze + */ + std::array, height> cells{}; + + /** + * @brief Start pose of the robot in the maze + */ + GridPose start; + + /** + * @brief Goal points in the maze + */ + std::unordered_set goal; + + /** + * @brief Whether the robot is returning to the start + */ + bool returning{}; + + /** + * @brief Whether the robot is exploring the maze + */ + bool exploring{true}; + + /** + * @brief Current best found route to the goal + */ + std::map> best_route; +}; +} // namespace micras::nav + +#include "../src/maze.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) + +#endif // MICRAS_NAV_MAZE_HPP diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp new file mode 100644 index 0000000..5dad365 --- /dev/null +++ b/micras_nav/src/grid_pose.cpp @@ -0,0 +1,76 @@ +/** + * @file grid_pose.cpp + * + * @brief Types for grid-based navigation + * + * @date 10/2024 + */ + +#include + +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +Side angle_to_grid(float angle) { + return static_cast(std::lround(2.0F * angle / std::numbers::pi_v) % 4); +} + +Side GridPoint::direction(const GridPoint& next) const { + if (next.x > this->x) { + return Side::RIGHT; + } + + if (next.y > this->y) { + return Side::UP; + } + + if (next.x < this->x) { + return Side::LEFT; + } + + if (next.y < this->y) { + return Side::DOWN; + } + + return Side::UP; +} + +GridPoint GridPoint::operator+(const Side& side) const { + switch (side) { + case Side::RIGHT: + return {static_cast(this->x + 1), this->y}; + case Side::UP: + return {this->x, static_cast(this->y + 1)}; + case Side::LEFT: + return {static_cast(this->x - 1), this->y}; + case Side::DOWN: + return {this->x, static_cast(this->y - 1)}; + } + + return *this; +} + +bool GridPoint::operator==(const GridPoint& other) const { + return this->x == other.x and this->y == other.y; +} + +GridPose GridPose::front() const { + return {this->position + this->orientation, this->orientation}; +} + +GridPose GridPose::turned_back() const { + return {this->position, static_cast((this->orientation + 2) % 4)}; +} + +GridPose GridPose::turned_left() const { + return {this->position, static_cast((this->orientation + 1) % 4)}; +} + +GridPose GridPose::turned_right() const { + return {this->position, static_cast((this->orientation + 3) % 4)}; +} + +bool GridPose::operator==(const GridPose& other) const { + return this->position == other.position and this->orientation == other.orientation; +} +} // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp new file mode 100644 index 0000000..b6114e6 --- /dev/null +++ b/micras_nav/src/maze.cpp @@ -0,0 +1,190 @@ +/** + * @file maze.cpp + * + * @brief Nav Maze class implementation + * + * @date 10/2024 + */ + +#ifndef MICRAS_NAV_MAZE_CPP +#define MICRAS_NAV_MAZE_CPP + +#include +#include + +#include "micras/nav/maze.hpp" + +namespace micras::nav { +template +Maze::Maze(const GridPose& start) : + start(start), + goal( + {{width / 2, height / 2}, + {(width - 1) / 2, height / 2}, + {width / 2, (height - 1) / 2}, + {(width - 1) / 2, (height - 1) / 2}} + ) { + for (uint8_t row = 0; row < height; row++) { + this->cells[row][0].wall_count[Side::LEFT] = 0xFFFF; + this->cells[row][width - 1].wall_count[Side::RIGHT] = 0xFFFF; + } + + for (uint8_t col = 0; col < width; col++) { + this->cells[0][col].wall_count[Side::DOWN] = 0xFFFF; + this->cells[height - 1][col].wall_count[Side::UP] = 0xFFFF; + } + + for (const auto& position : this->goal) { + this->get_cell(position).cost = 0; + } + + this->calculate_costmap(); +} + +template +void Maze::update(const GridPose& pose, Information information) { + if (this->goal.contains(pose.position)) { + this->returning = true; + } else if (pose == start.turned_back()) { + this->exploring = false; + this->returning = false; + } + + if (not this->exploring) { + return; + } + + if (information.left != Information::UNKNOWN) { + this->update_wall(pose.turned_left(), information.left == Information::WALL); + } + + if (information.front_left != Information::UNKNOWN) { + this->update_wall(pose.front().turned_left(), information.front_left == Information::WALL); + } + + if (information.front != Information::UNKNOWN) { + this->update_wall(pose, information.front == Information::WALL); + } + + if (information.front_right != Information::UNKNOWN) { + this->update_wall(pose.front().turned_right(), information.front_right == Information::WALL); + } + + if (information.right != Information::UNKNOWN) { + this->update_wall(pose.turned_right(), information.right == Information::WALL); + } + + this->calculate_costmap(); +} + +template +GridPoint Maze::get_current_goal(const GridPoint& position, bool force_costmap) const { + const Cell& current_cell = this->get_cell(position); + uint16_t current_cost = current_cell.cost; + + if (not force_costmap and (not this->exploring or this->returning) and this->best_route.contains(current_cost) and + this->best_route.at(current_cost) == position) { + return (this->returning ? std::prev(this->best_route.find(current_cost)) : + std::next(this->best_route.find(current_cost))) + ->second; + } + + GridPoint next_position = position; + + for (uint8_t i = Side::LEFT; i <= Side::DOWN; i++) { + Side side = static_cast(i); + GridPoint front_position = position + side; + + if ((current_cell.wall_count[side] < current_cell.free_count[side]) and + this->get_cell(front_position).cost <= current_cost) { + current_cost = this->get_cell(front_position).cost; + next_position = front_position; + } + } + + return next_position; +} + +template +const Maze::Cell& Maze::get_cell(const GridPoint& position) const { + return this->cells.at(position.y).at(position.x); +} + +template +Maze::Cell& Maze::get_cell(const GridPoint& position) { + return this->cells.at(position.y).at(position.x); +} + +template +void Maze::update_wall(const GridPose& pose, bool wall) { + if (wall) { + this->get_cell(pose.position).wall_count[pose.orientation]++; + } else { + this->get_cell(pose.position).free_count[pose.orientation]++; + } + + GridPose front_pose = pose.front(); + + if (front_pose.position.x >= width or front_pose.position.y >= height) { + return; + } + + if (wall) { + this->get_cell(front_pose.position).wall_count[pose.turned_back().orientation]++; + } else { + this->get_cell(front_pose.position).free_count[pose.turned_back().orientation]++; + } +} + +template +void Maze::calculate_costmap() { + std::array, height> visited{}; + std::array, height> origin{}; + std::queue queue; + + for (const auto& position : this->goal) { + queue.push(position); + visited.at(position.y).at(position.x) = true; + } + + while (not queue.empty()) { + GridPoint current_position = queue.front(); + queue.pop(); + + Cell& current_cell = this->get_cell(current_position); + + for (uint8_t i = Side::LEFT; i <= Side::DOWN; i++) { + Side side = static_cast(i); + GridPoint front_position = current_position + side; + + if ((current_cell.wall_count[side] < current_cell.free_count[side]) and + not visited.at(front_position.y).at(front_position.x)) { + visited.at(front_position.y).at(front_position.x) = true; + origin.at(front_position.y).at(front_position.x) = side; + + this->get_cell(front_position).cost = + (current_cell.cost + (this->goal.contains(current_position) or + (side == origin.at(current_position.y).at(current_position.x)) ? + 1 : + 2)); + queue.push(front_position); + } + } + } + + if (not this->returning) { + return; + } + + GridPoint current_position = this->start.position; + this->best_route.clear(); + this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start.position); + + while (not this->goal.contains(current_position)) { + current_position = this->get_current_goal(current_position, true); + this->best_route.try_emplace(this->get_cell(current_position).cost, current_position); + } +} +} // namespace micras::nav + +#endif // MICRAS_NAV_MAZE_CPP From 89bf8df0a0acd4ecb3c93ced439aa187c5259dda Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 08:57:00 -0300 Subject: [PATCH 207/561] =?UTF-8?q?=E2=9C=A8=20Add=20mapping=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/mapping.hpp | 166 ++++++++++++++++++++++ micras_nav/include/micras/nav/state.hpp | 125 ++++++++++++++++ micras_nav/src/mapping.cpp | 121 ++++++++++++++++ micras_nav/src/state.cpp | 38 +++++ 4 files changed, 450 insertions(+) create mode 100644 micras_nav/include/micras/nav/mapping.hpp create mode 100644 micras_nav/include/micras/nav/state.hpp create mode 100644 micras_nav/src/mapping.cpp create mode 100644 micras_nav/src/state.cpp diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp new file mode 100644 index 0000000..b074792 --- /dev/null +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -0,0 +1,166 @@ +/** + * @file mapping.hpp + * + * @brief Nav Mapping class declaration + * + * @date 10/2024 + */ + +#ifndef MICRAS_NAV_MAPPING_HPP +#define MICRAS_NAV_MAPPING_HPP + +#include + +#include "micras/nav/maze.hpp" +#include "micras/nav/state.hpp" +#include "micras/proxy/distance_sensors.hpp" + +namespace micras::nav { +/** + * @brief Class for mapping the maze + * + * @tparam width The width of the maze + * @tparam height The height of the maze + */ +template +class Mapping { +public: + /** + * @brief Configuration for the Mapping class + */ + struct Config { + float wall_thickness; + float cell_size; + float wall_distance_threshold; + float free_distance_threshold; + float alignment_threshold; + Pose front_sensor_pose; + Pose side_sensor_pose; + GridPose start; + }; + + /** + * @brief Type to identify the distance sensors + */ + enum Sensor : uint8_t { + FRONT_LEFT, + LEFT, + RIGHT, + FRONT_RIGHT + }; + + /** + * @brief Type to store an action to be taken + */ + struct Action { + /** + * @brief TPossible types of actions + */ + enum Type : uint8_t { + LOOK_AT, + GO_TO + }; + + /** + * @brief Type of the action + */ + Type type; + + /** + * @brief The next goal point + */ + Point point; + }; + + /** + * @brief Constructor for the Mapping class + * + * @param distance_sensors The distance sensors + * @param config The configuration for the mapping + */ + Mapping(const proxy::DistanceSensors<4>& distance_sensors, Config config); + + /** + * @brief Update the mapping of the maze using the current pose and distance sensors + * + * @param pose The current pose of the robot + */ + void update(const Pose& pose); + + /** + * @brief Get the action to take based on the current pose + * + * @param pose The current pose of the robot + * @return The action to take + */ + Action get_action(const Pose& pose) const; + +private: + /** + * @brief Get the wall information for a sensor + * + * @param sensor The sensor to get the wall information + * @return The wall information for the sensor + */ + Information::Existence get_wall_information(Sensor sensor) const; + + /** + * @brief Distance sensors of the robot + */ + const proxy::DistanceSensors<4>& distance_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + + /** + * @brief The maze information the robot has + */ + Maze maze; + + /** + * @brief Thickness of the maze walls + */ + float wall_thickness; + + /** + * @brief Size of the cells in the maze + */ + float cell_size; + + /** + * @brief Threshold for the distance to consider a wall + */ + float wall_distance_threshold; + + /** + * @brief Threshold for the distance to consider free space + */ + float free_distance_threshold; + + /** + * @brief Threshold for the robot alignment with the side walls + */ + float alignment_threshold; + + /** + * @brief Pose of the front distance sensors + */ + Pose front_sensor_pose; + + /** + * @brief Pose of the side distance sensors + */ + Pose side_sensor_pose; + + /** + * @brief Region of the cell to consider the information of the side sensors to the current cell + */ + float side_sensors_region_division; + + /** + * @brief Region of the cell to consider the information of the front sensors to the current cell + */ + float front_sensors_region_division; +}; +} // namespace micras::nav + +#include "../src/mapping.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) + +#endif // MICRAS_NAV_MAPPING_HPP diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp new file mode 100644 index 0000000..fa7409f --- /dev/null +++ b/micras_nav/include/micras/nav/state.hpp @@ -0,0 +1,125 @@ +/** + * @file state.hpp + * + * @brief Types for robot state representation + * + * @date 10/2024 + */ + +#ifndef MICRAS_NAV_POSE_HPP +#define MICRAS_NAV_POSE_HPP + +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +/** + * @brief Type to store a point in 2D space + */ +struct Point { + /** + * @brief Calculates the distance to another point + * + * @param other The other point + * @return The distance to the other point + */ + float distance(const Point& other) const; + + /** + * @brief Calculates the angle between two points + * + * @param other The other point + * @return The angle between the two points + */ + float angle_between(const Point& other) const; + + /** + * @brief Converts the point to a grid point + * + * @param cell_size The size of the grid cells + * @return The grid point corresponding to the point + */ + GridPoint to_grid(float cell_size) const; + + /** + * @brief Calculates the remainder of the division of the point by a value + * + * @param value The value to divide the point by + * @return The remainder of the division + */ + Point operator%(float value) const; + + /** + * @brief Converts a grid point to a point + * + * @param grid_point The grid point to convert + * @param cell_size The size of the grid cells + * @return The point corresponding to the grid point + */ + static Point from_grid(const GridPoint& grid_point, float cell_size); + + /** + * @brief The x coordinate of the point in 2D space + */ + float x; + + /** + * @brief The y coordinate of the point in 2D space + */ + float y; +}; + +/** + * @brief Type to store a pose in 2D space + */ +struct Pose { + /** + * @brief Converts the pose to a grid pose + * + * @param cell_size The size of the grid cells + * @return The grid pose corresponding to the pose + */ + GridPose to_grid(float cell_size) const; + + /** + * @brief The position of the pose + */ + Point position; + + /** + * @brief The orientation of the pose + */ + float orientation; +}; + +/** + * @brief Type to store a twist in 2D space + */ +struct Twist { + /** + * @brief The linear velocity of the twist + */ + float linear; + + /** + * @brief The angular velocity of the twist + */ + float angular; +}; + +/** + * @brief Type to store the state of the robot + */ +struct State { + /** + * @brief The pose of the robot + */ + Pose pose; + + /** + * @brief The velocity of the robot + */ + Twist velocity; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_POSE_HPP diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp new file mode 100644 index 0000000..755a498 --- /dev/null +++ b/micras_nav/src/mapping.cpp @@ -0,0 +1,121 @@ +/** + * @file mapping.cpp + * + * @brief Nav Mapping class implementation + * + * @date 10/2024 + */ + +#ifndef MICRAS_NAV_MAPPING_CPP +#define MICRAS_NAV_MAPPING_CPP + +#include + +#include "micras/nav/mapping.hpp" + +namespace micras::nav { +template +Mapping::Mapping(const proxy::DistanceSensors<4>& distance_sensors, Mapping::Config config) : + distance_sensors{distance_sensors}, + maze{config.start}, + wall_thickness{config.wall_thickness}, + cell_size{config.cell_size}, + wall_distance_threshold{config.wall_distance_threshold}, + free_distance_threshold{config.free_distance_threshold}, + alignment_threshold{config.alignment_threshold}, + front_sensor_pose{config.front_sensor_pose}, + side_sensor_pose{config.side_sensor_pose}, + side_sensors_region_division{ + cell_size - wall_thickness / 2.0F - side_sensor_pose.position.y + + (side_sensor_pose.position.x + (wall_thickness - cell_size) / 2.0F) * std::tan(side_sensor_pose.orientation) + }, + front_sensors_region_division{cell_size - wall_thickness / 2.0F - front_sensor_pose.position.y} { } + +template +void Mapping::update(const Pose& pose) { + float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); + + if (reliability < 50.0F) { + return; + } + + Point cell_position = pose.position % this->cell_size; + Side side = angle_to_grid(pose.orientation); + + float cell_advance = 0.0F; + float cell_alignment = 0.0F; + + switch (side) { + case Side::RIGHT: + cell_advance = cell_position.x; + cell_alignment = std::abs(this->cell_size / 2.0F - cell_position.y); + break; + case Side::UP: + cell_advance = cell_position.y; + cell_alignment = std::abs(this->cell_size / 2.0F - cell_position.x); + break; + case Side::LEFT: + cell_advance = this->cell_size - cell_position.x; + cell_alignment = std::abs(this->cell_size / 2.0F - cell_position.y); + break; + case Side::DOWN: + cell_advance = this->cell_size - cell_position.y; + cell_alignment = std::abs(this->cell_size / 2.0F - cell_position.x); + break; + } + + Information information{}; + + if (cell_advance < this->front_sensors_region_division) { + if (this->get_wall_information(Sensor::FRONT_LEFT) == Information::WALL and + this->get_wall_information(Sensor::FRONT_RIGHT) == Information::WALL) { + information.front = Information::WALL; + } else if (this->get_wall_information(Sensor::FRONT_LEFT) == Information::FREE and + this->get_wall_information(Sensor::FRONT_RIGHT) == Information::FREE) { + information.front = Information::FREE; + } + } + + if (cell_alignment < this->alignment_threshold * this->cell_size) { + if (cell_advance < this->side_sensors_region_division) { + information.left = this->get_wall_information(Sensor::LEFT); + information.right = this->get_wall_information(Sensor::RIGHT); + } else if (information.front == Information::FREE and + cell_advance > this->side_sensors_region_division + this->wall_thickness) { + information.front_left = this->get_wall_information(Sensor::LEFT); + information.front_right = this->get_wall_information(Sensor::RIGHT); + } + } + + this->maze.update({pose.position.to_grid(this->cell_size), side}, information); +} + +template +Mapping::Action Mapping::get_action(const Pose& pose) const { + Point current_goal = + Point::from_grid(this->maze.get_current_goal(pose.position.to_grid(this->cell_size)), this->cell_size); + + float angle_error = core::assert_angle(pose.orientation - pose.position.angle_between(current_goal)); + + if (std::abs(angle_error) > std::numbers::pi_v / 4.0F) { + return {Action::Type::LOOK_AT, current_goal}; + } + + return {Action::Type::GO_TO, current_goal}; +} + +template +Information::Existence Mapping::get_wall_information(Sensor sensor) const { + if (this->distance_sensors.get_distance(sensor) < this->wall_distance_threshold) { + return Information::WALL; + } + + if (this->distance_sensors.get_distance(sensor) < this->free_distance_threshold) { + return Information::FREE; + } + + return Information::UNKNOWN; +} +} // namespace micras::nav + +#endif // MICRAS_NAV_MAPPING_CPP diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp new file mode 100644 index 0000000..12faa2c --- /dev/null +++ b/micras_nav/src/state.cpp @@ -0,0 +1,38 @@ +/** + * @file state.cpp + * + * @brief Types for robot state representation + * + * @date 10/2024 + */ + +#include +#include + +#include "micras/nav/state.hpp" + +namespace micras::nav { +float Point::distance(const Point& other) const { + return std::hypot(other.x - this->x, other.y - this->y); +} + +float Point::angle_between(const Point& other) const { + return std::atan2(other.y - this->y, other.x - this->x); +} + +GridPoint Point::to_grid(float cell_size) const { + return {static_cast(this->x / cell_size), static_cast(this->y / cell_size)}; +} + +Point Point::operator%(float value) const { + return {std::fmod(this->x, value), std::fmod(this->y, value)}; +} + +Point Point::from_grid(const GridPoint& grid_point, float cell_size) { + return {cell_size * (grid_point.x + 0.5F), cell_size * (grid_point.y + 0.5F)}; +} + +GridPose Pose::to_grid(float cell_size) const { + return {this->position.to_grid(cell_size), angle_to_grid(this->orientation)}; +}; +} // namespace micras::nav From ccdea8f4bb40ba3289de633368da6f9231429a33 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 09:01:04 -0300 Subject: [PATCH 208/561] =?UTF-8?q?=E2=9C=A8=20Add=20controller=20to=20nex?= =?UTF-8?q?t=20goal=20point?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/go_to_point.hpp | 85 +++++++++++++++++++ .../include/micras/nav/look_at_point.hpp | 67 +++++++++++++++ .../include/micras/nav/pid_controller.hpp | 83 ++++++++++++++++++ micras_nav/src/go_to_point.cpp | 35 ++++++++ micras_nav/src/look_at_point.cpp | 23 +++++ micras_nav/src/pid_controller.cpp | 79 +++++++++++++++++ 6 files changed, 372 insertions(+) create mode 100644 micras_nav/include/micras/nav/go_to_point.hpp create mode 100644 micras_nav/include/micras/nav/look_at_point.hpp create mode 100644 micras_nav/include/micras/nav/pid_controller.hpp create mode 100644 micras_nav/src/go_to_point.cpp create mode 100644 micras_nav/src/look_at_point.cpp create mode 100644 micras_nav/src/pid_controller.cpp diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp new file mode 100644 index 0000000..fa966b6 --- /dev/null +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -0,0 +1,85 @@ +/** + * @file go_to_point.hpp + * + * @brief Nav GoToPoint class declaration + * + * @date 10/2024 + */ + +#ifndef MICRAS_NAV_GO_TO_POINT_HPP +#define MICRAS_NAV_GO_TO_POINT_HPP + +#include "micras/nav/pid_controller.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Class to calculate a command to go to a point + */ +class GoToPoint { +public: + /** + * @brief Configuration for the GoToPoint class + */ + struct Config { + PidController::Config linear_pid; + PidController::Config angular_pid; + float base_speed{}; + float linear_decay_damping{}; + float tolerance{}; + }; + + /** + * @brief Constructor for the GoToPoint class + * + * @param config The configuration for the GoToPoint class + */ + explicit GoToPoint(Config config); + + /** + * @brief Calculates the command to go to a point + * + * @param state The current state of the robot + * @param goal The goal point + * @return The command to go to the point + */ + Twist action(const State& state, const Point& goal); + + /** + * @brief Checks if the robot has reached the goal + * + * @param state The current state of the robot + * @param goal The goal point + * @return True if the robot has reached the goal, false otherwise + */ + bool finished(const State& state, const Point& goal) const; + +private: + /** + * @brief PID controller for the linear velocity + */ + PidController linear_pid; + + /** + * @brief PID controller for the orientation + */ + PidController angular_pid; + + /** + * @brief Base linear speed for the robot + */ + float base_speed; + + /** + * @brief Damping factor for the linear decay + */ + float linear_decay_damping; + + /** + * @brief Linear tolerance for the goal point + */ + float tolerance; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_GO_TO_POINT_HPP diff --git a/micras_nav/include/micras/nav/look_at_point.hpp b/micras_nav/include/micras/nav/look_at_point.hpp new file mode 100644 index 0000000..5f3b9f1 --- /dev/null +++ b/micras_nav/include/micras/nav/look_at_point.hpp @@ -0,0 +1,67 @@ +/** + * @file look_at_point.hpp + * + * @brief Nav LookAtPoint class declaration + * + * @date 10/2024 + */ + +#ifndef MICRAS_NAV_LOOK_AT_POINT_HPP +#define MICRAS_NAV_LOOK_AT_POINT_HPP + +#include "micras/nav/pid_controller.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Class to calculate a command to look at a point + */ +class LookAtPoint { +public: + /** + * @brief Configuration for the LookAtPoint class + */ + struct Config { + PidController::Config pid; + float tolerance{}; + }; + + /** + * @brief Constructor for the LookAtPoint class + * + * @param config The configuration for the LookAtPoint class + */ + explicit LookAtPoint(Config config); + + /** + * @brief Calculates the command to look at a point + * + * @param pose The current pose of the robot + * @param goal The goal point + * @return The command to look at the point + */ + Twist action(const Pose& pose, const Point& goal); + + /** + * @brief Checks if the robot has reached the goal + * + * @param pose The current pose of the robot + * @param goal The goal point + * @return True if the robot has reached the goal, false otherwise + */ + bool finished(const Pose& pose, const Point& goal) const; + +private: + /** + * @brief PID controller for the angular velocity + */ + PidController pid; + + /** + * @brief Angular tolerance for the goal + */ + float tolerance; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_LOOK_AT_POINT_HPP diff --git a/micras_nav/include/micras/nav/pid_controller.hpp b/micras_nav/include/micras/nav/pid_controller.hpp new file mode 100644 index 0000000..bcc7fbb --- /dev/null +++ b/micras_nav/include/micras/nav/pid_controller.hpp @@ -0,0 +1,83 @@ +/** + * @file pid_controller.hpp + * + * @brief Nav Pid Controller class declaration + * + * @date 10/2024 + */ + +#ifndef MICRAS_NAV_PID_CONTROLLER_HPP +#define MICRAS_NAV_PID_CONTROLLER_HPP + +#include "micras/hal/timer.hpp" + +namespace micras::nav { +/** + * @brief Implementation of simple PID controller + * Response = Kp(error + Ki * integral(error) Kd * d/dt(error)) + */ +class PidController { +public: + struct Config { + float kp{}; /**< Proportional constant */ + float ki{}; /**< Integrative constant */ + float kd{}; /**< Derivative constant */ + float setpoint{}; /**< Desired state */ + float saturation{-1.0F}; /**< Maximum response returned by the controller */ + float max_integral{-1.0F}; /**< Maximum integrative response */ + }; + + /** + * @brief Construct a new Pid Controller object + * + * @param config Controller parameters + */ + explicit PidController(Config config); + + /** + * @brief Set the setpoint object + * + * @param setpoint Desired state + */ + void set_setpoint(float setpoint); + + /** + * @brief Reset prev_error and error_acc objects + */ + void reset(); + + /** + * @brief Update PID with new state and return response + * + * @param state Current value of the controlled variable + * + * @return Response + */ + float update(float state); + + /** + * @brief Update PID with new state and return response + * + * @param state Current value of the controlled variable + * @param state_change Derivative of the controlled variable + * + * @return Response + */ + float update(float state, float state_change); + +private: + float kp; /**< Proportional constant */ + float ki; /**< Integrative constant */ + float kd; /**< Derivative constant */ + float setpoint; /**< Desired state */ + float saturation; /**< Maximum response returned by the controller */ + float max_integral; /**< Maximum integrative accumulative response */ + float error_acc = 0; /**< Accumulated error for i term */ + float prev_state = 0; /**< Previous state for d term */ + float last_response = 0; /**< Last response returned by the controller */ + + hal::Timer timer; /**< Timer used to compute the loop time */ +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_PID_CONTROLLER_HPP diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp new file mode 100644 index 0000000..16cc54f --- /dev/null +++ b/micras_nav/src/go_to_point.cpp @@ -0,0 +1,35 @@ +/** + * @file go_to_point.cpp + * + * @brief Nav GoToPoint class implementation + * + * @date 10/2024 + */ + +#include + +#include "micras/core/utils.hpp" +#include "micras/nav/go_to_point.hpp" + +namespace micras::nav { +GoToPoint::GoToPoint(Config config) : + linear_pid(config.linear_pid), + angular_pid(config.angular_pid), + base_speed{config.base_speed}, + linear_decay_damping{config.linear_decay_damping}, + tolerance{config.tolerance} { } + +Twist GoToPoint::action(const State& state, const Point& goal) { + float angular_error = core::assert_angle(state.pose.orientation - state.pose.position.angle_between(goal)); + float linear_target = core::decay(angular_error, this->linear_decay_damping) * this->base_speed; + + float angular_command = this->angular_pid.update(angular_error); + float linear_command = this->linear_pid.update(state.velocity.linear - linear_target); + + return {linear_command, angular_command}; +} + +bool GoToPoint::finished(const State& state, const Point& goal) const { + return std::hypot(goal.x - state.pose.position.x, goal.y - state.pose.position.y) <= this->tolerance; +} +} // namespace micras::nav diff --git a/micras_nav/src/look_at_point.cpp b/micras_nav/src/look_at_point.cpp new file mode 100644 index 0000000..78cf096 --- /dev/null +++ b/micras_nav/src/look_at_point.cpp @@ -0,0 +1,23 @@ +/** + * @file look_at_point.cpp + * + * @brief Nav LookAtPoint class implementation + * + * @date 10/2024 + */ + +#include "micras/core/utils.hpp" +#include "micras/nav/look_at_point.hpp" + +namespace micras::nav { +LookAtPoint::LookAtPoint(Config config) : pid(config.pid), tolerance{config.tolerance} { } + +Twist LookAtPoint::action(const Pose& pose, const Point& goal) { + float angular_target = pose.position.angle_between(goal); + return {0.0F, this->pid.update(core::assert_angle(pose.orientation - angular_target))}; +} + +bool LookAtPoint::finished(const Pose& pose, const Point& goal) const { + return std::abs(core::assert_angle(pose.orientation - pose.position.angle_between(goal))) <= this->tolerance; +} +} // namespace micras::nav diff --git a/micras_nav/src/pid_controller.cpp b/micras_nav/src/pid_controller.cpp new file mode 100644 index 0000000..86be891 --- /dev/null +++ b/micras_nav/src/pid_controller.cpp @@ -0,0 +1,79 @@ +/** + * @file pid_controller.cpp + * + * @brief Nav Pid Controller class implementation + * + * @date 10/2024 + */ + +#include +#include + +#include "micras/nav/pid_controller.hpp" + +namespace micras::nav { +PidController::PidController(Config config) : + kp{config.kp}, + ki{config.ki}, + kd{config.kd}, + setpoint{config.setpoint}, + saturation{config.saturation}, + max_integral{config.max_integral} { } + +void PidController::set_setpoint(float setpoint) { + this->setpoint = setpoint; +} + +void PidController::reset() { + this->error_acc = 0; + this->prev_state = 0; + this->last_response = 0; +} + +float PidController::update(float state) { + float loop_time = this->timer.elapsed_time_us() / 1000000.0F; + + if (loop_time == 0) { + return this->last_response; + } + + float state_change = (state - this->prev_state) / loop_time; + + return this->update(state, state_change); +} + +float PidController::update(float state, float state_change) { + float loop_time = this->timer.elapsed_time_us() / 1000000.0F; + + if (loop_time == 0) { + return this->last_response; + } + + float error = this->setpoint - state; + this->prev_state = state; + + float response = this->kp * (error + this->ki * this->error_acc + this->kd * state_change); + + if (this->saturation < 0 or std::abs(response) < this->saturation or + (this->error_acc != 0 and std::signbit(this->error_acc) != std::signbit(error))) { + this->error_acc += error * loop_time; + } + + if (this->max_integral >= 0 and this->ki > 0) { + this->error_acc = std::clamp( + this->error_acc, -this->max_integral / (this->kp * this->ki), this->max_integral / (this->kp * this->ki) + ); + } + + response = this->kp * (error + this->ki * this->error_acc + this->kd * state_change); + + if (this->saturation >= 0 and std::abs(response) >= this->saturation) { + response = std::clamp(response, -this->saturation, this->saturation); + } + + this->last_response = response; + this->timer.reset_us(); + + return response; +} +} // namespace micras::nav From 1c145d56fd2ded376c3aca2bb0227f91df9bf1b1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 09:01:56 -0300 Subject: [PATCH 209/561] =?UTF-8?q?=E2=9C=A8=20Add=20filter=20to=20odometr?= =?UTF-8?q?y=20velocity=20and=20imu=20odometry?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/odometry.hpp | 79 ++++++++++++---------- micras_nav/src/odometry.cpp | 64 +++++++++--------- 2 files changed, 74 insertions(+), 69 deletions(-) diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 4498161..3596aca 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -3,7 +3,7 @@ * * @brief Nav Odometry class declaration * - * @date 04/2024 + * @date 10/2024 */ #ifndef MICRAS_NAV_ODOMETRY_HPP @@ -11,7 +11,10 @@ #include +#include "micras/core/butterworth_filter.hpp" #include "micras/hal/timer.hpp" +#include "micras/nav/state.hpp" +#include "micras/proxy/imu.hpp" #include "micras/proxy/rotary_sensor.hpp" namespace micras::nav { @@ -20,14 +23,24 @@ namespace micras::nav { */ class Odometry { public: + struct Config { + float linear_cutoff_frequency; + float angular_cutoff_frequency; + float wheel_radius; + float wheel_separation; + Pose initial_pose; + }; + /** * @brief Constructor for the Odometry class * + * @param left_rotary_sensor Left rotary sensor + * @param right_rotary_sensor Right rotary sensor * @param config Configuration for the odometry */ Odometry( const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - hal::Timer::Config timer_config, float wheel_radius, float wheel_separation + const proxy::Imu& imu, hal::Timer::Config timer_config, Config config ); /** @@ -36,39 +49,18 @@ class Odometry { void update(); /** - * @brief Get the position x - * - * @return float Position x - */ - float get_position_x() const; - - /** - * @brief Get the position y - * - * @return float Position y - */ - float get_position_y() const; - - /** - * @brief Get the orientation - * - * @return float Orientation - */ - float get_orientation() const; - - /** - * @brief Get the linear velocity + * @brief Get the state of the robot * - * @return float Linear velocity + * @return State current state of the robot in space */ - float get_linear_velocity() const; + const State& get_state() const; /** - * @brief Get the angular velocity + * @brief Get the state of the robot calculated using the IMU * - * @return float Angular velocity + * @return State current state of the robot in space */ - float get_angular_velocity() const; + const State& get_imu_state() const; private: /** @@ -81,6 +73,11 @@ class Odometry { */ const proxy::RotarySensor& right_rotary_sensor; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + /** + * @brief IMU sensor + */ + const proxy::Imu& imu; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + /** * @brief Timer for velocity calculation */ @@ -107,15 +104,25 @@ class Odometry { float right_last_position{}; /** - * @brief Odometry variables + * @brief Linear velocity filter */ - float position_x{}; - float position_y{}; - float orientation{}; - float linear_velocity{}; - float angular_velocity{}; -}; + core::ButterworthFilter linear_filter; + + /** + * @brief Angular velocity filter + */ + core::ButterworthFilter angular_filter; + /** + * @brief Current state of the robot in space + */ + State state; + + /** + * @brief Current state of the robot in space calculated using the IMU + */ + State imu_state; +}; } // namespace micras::nav #endif // MICRAS_NAV_ODOMETRY_HPP diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 623d73b..032bcb8 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -14,16 +14,20 @@ namespace micras::nav { Odometry::Odometry( const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - hal::Timer::Config timer_config, float wheel_radius, float wheel_separation + const proxy::Imu& imu, hal::Timer::Config timer_config, Config config ) : - left_rotary_sensor{left_rotary_sensor}, right_rotary_sensor{right_rotary_sensor}, + imu{imu}, timer{timer_config}, - wheel_radius{wheel_radius}, - wheel_separation{wheel_separation}, + wheel_radius{config.wheel_radius}, + wheel_separation{config.wheel_separation}, left_last_position{left_rotary_sensor.get_position()}, - right_last_position{right_rotary_sensor.get_position()} { } + right_last_position{right_rotary_sensor.get_position()}, + linear_filter{config.linear_cutoff_frequency}, + angular_filter{config.angular_cutoff_frequency}, + state{config.initial_pose, {0.0F, 0.0F}}, + imu_state{config.initial_pose, {0.0F, 0.0F}} { } void Odometry::update() { float left_position = this->left_rotary_sensor.get_position(); @@ -44,43 +48,37 @@ void Odometry::update() { float linear_distance = (left_distance + right_distance) / 2; float angular_distance = (right_distance - left_distance) / this->wheel_separation; - float linear_diagonal{}; - - if (angular_distance < 0.05F) { - linear_diagonal = linear_distance; - } else { - linear_diagonal = std::abs( - std::sqrt(1 - std::cos(angular_distance)) * std::numbers::sqrt2_v * linear_distance / - angular_distance - ); - } + float half_angle = angular_distance / 2; + float linear_diagonal = + angular_distance < 0.05F ? linear_distance : std::abs(std::sin(half_angle) * linear_distance / half_angle); - this->position_x += linear_diagonal * std::cos(this->orientation + angular_distance / 2); - this->position_y += linear_diagonal * std::sin(this->orientation + angular_distance / 2); + this->state.pose.position.x += linear_diagonal * std::cos(this->state.pose.orientation + half_angle); + this->state.pose.position.y += linear_diagonal * std::sin(this->state.pose.orientation + half_angle); - this->orientation += angular_distance; + this->state.pose.orientation += angular_distance; - this->linear_velocity = linear_distance / elapsed_time; - this->angular_velocity = angular_distance / elapsed_time; -} + this->state.velocity.linear = this->linear_filter.update(linear_distance / elapsed_time); + this->state.velocity.angular = this->angular_filter.update(angular_distance / elapsed_time); -float Odometry::get_position_x() const { - return this->position_x; -} + angular_distance = this->imu.get_angular_velocity(proxy::Imu::Axis::Z) * elapsed_time; + half_angle = angular_distance / 2; + linear_diagonal = + angular_distance < 0.05F ? linear_distance : std::abs(std::sin(half_angle) * linear_distance / half_angle); -float Odometry::get_position_y() const { - return this->position_y; -} + this->imu_state.pose.position.x += linear_diagonal * std::cos(this->imu_state.pose.orientation + half_angle); + this->imu_state.pose.position.y += linear_diagonal * std::sin(this->imu_state.pose.orientation + half_angle); + + this->imu_state.pose.orientation += angular_distance; -float Odometry::get_orientation() const { - return this->orientation; + this->imu_state.velocity.linear = linear_distance / elapsed_time; + this->imu_state.velocity.angular = this->imu.get_angular_velocity(proxy::Imu::Axis::Z); } -float Odometry::get_linear_velocity() const { - return this->linear_velocity; +const nav::State& Odometry::get_state() const { + return this->state; } -float Odometry::get_angular_velocity() const { - return this->angular_velocity; +const nav::State& Odometry::get_imu_state() const { + return this->imu_state; } } // namespace micras::nav From 907a98a1c6d5f9f3c11d3ecb6e3a83aaa31821c5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 11:30:30 -0300 Subject: [PATCH 210/561] =?UTF-8?q?=F0=9F=8E=A8=20Add=20has=5Fwall=20funct?= =?UTF-8?q?ion=20to=20mapping?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/grid_pose.hpp | 4 ++-- micras_nav/include/micras/nav/maze.hpp | 8 ++++++++ micras_nav/src/maze.cpp | 20 ++++++++++++-------- 3 files changed, 22 insertions(+), 10 deletions(-) diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index 268d75f..618ead4 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -40,8 +40,8 @@ struct Information { */ enum Existence : uint8_t { UNKNOWN = 0, - WALL = 1, - FREE = 2 + FREE = 1, + WALL = 2 }; /** diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index bdbf929..5c7b9b8 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -89,6 +89,14 @@ class Maze { */ void update_wall(const GridPose& pose, bool wall); + /** + * @brief Checks whether there is a wall at the front of a given pose + * + * @param pose The pose to check + * @return True if there is a wall, false otherwise + */ + bool has_wall(const GridPose& pose) const; + /** * @brief Cells matrix representing the maze */ diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index b6114e6..358804f 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -79,8 +79,7 @@ void Maze::update(const GridPose& pose, Information information) template GridPoint Maze::get_current_goal(const GridPoint& position, bool force_costmap) const { - const Cell& current_cell = this->get_cell(position); - uint16_t current_cost = current_cell.cost; + uint16_t current_cost = this->get_cell(position).cost; if (not force_costmap and (not this->exploring or this->returning) and this->best_route.contains(current_cost) and this->best_route.at(current_cost) == position) { @@ -91,12 +90,11 @@ GridPoint Maze::get_current_goal(const GridPoint& position, bool GridPoint next_position = position; - for (uint8_t i = Side::LEFT; i <= Side::DOWN; i++) { + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { Side side = static_cast(i); GridPoint front_position = position + side; - if ((current_cell.wall_count[side] < current_cell.free_count[side]) and - this->get_cell(front_position).cost <= current_cost) { + if (not this->has_wall({position, side}) and this->get_cell(front_position).cost <= current_cost) { current_cost = this->get_cell(front_position).cost; next_position = front_position; } @@ -136,6 +134,12 @@ void Maze::update_wall(const GridPose& pose, bool wall) { } } +template +bool Maze::has_wall(const GridPose& pose) const { + return this->get_cell(pose.position).wall_count[pose.orientation] > + this->get_cell(pose.position).free_count[pose.orientation]; +} + template void Maze::calculate_costmap() { std::array, height> visited{}; @@ -151,13 +155,13 @@ void Maze::calculate_costmap() { GridPoint current_position = queue.front(); queue.pop(); - Cell& current_cell = this->get_cell(current_position); + const Cell& current_cell = this->get_cell(current_position); - for (uint8_t i = Side::LEFT; i <= Side::DOWN; i++) { + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { Side side = static_cast(i); GridPoint front_position = current_position + side; - if ((current_cell.wall_count[side] < current_cell.free_count[side]) and + if (not this->has_wall({current_position, side}) and not visited.at(front_position.y).at(front_position.x)) { visited.at(front_position.y).at(front_position.x) = true; origin.at(front_position.y).at(front_position.x) = side; From 0f89b14d3e444fcedf25462f6c82f5628fe172f4 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 11:32:57 -0300 Subject: [PATCH 211/561] =?UTF-8?q?=E2=9C=85=20Add=20locomotion=20to=20rot?= =?UTF-8?q?ary=20sensors=20test?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/rotary_sensor.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index 5ee80e5..c63c7ec 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -6,6 +6,7 @@ * @date 03/2024 */ +#include #include #include "micras/proxy/rotary_sensor.hpp" From ec4310cae8133786b59df63919c667d2619e1e5d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Oct 2024 16:21:46 -0300 Subject: [PATCH 212/561] =?UTF-8?q?=F0=9F=9A=B8=20Add=20goal=20parameter?= =?UTF-8?q?=20to=20maze=20class=20e=20mapping=20configuration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/mapping.hpp | 23 +++++++++++++++-------- micras_nav/include/micras/nav/maze.hpp | 3 ++- micras_nav/src/mapping.cpp | 2 +- micras_nav/src/maze.cpp | 9 +-------- 4 files changed, 19 insertions(+), 18 deletions(-) diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index b074792..94641e8 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -29,14 +29,21 @@ class Mapping { * @brief Configuration for the Mapping class */ struct Config { - float wall_thickness; - float cell_size; - float wall_distance_threshold; - float free_distance_threshold; - float alignment_threshold; - Pose front_sensor_pose; - Pose side_sensor_pose; - GridPose start; + float wall_thickness; + float cell_size; + float wall_distance_threshold; + float free_distance_threshold; + float alignment_threshold; + Pose front_sensor_pose; + Pose side_sensor_pose; + + GridPose start; + std::unordered_set goal{ + {{width / 2, height / 2}, + {(width - 1) / 2, height / 2}, + {width / 2, (height - 1) / 2}, + {(width - 1) / 2, (height - 1) / 2}} + }; }; /** diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 5c7b9b8..fb7eef8 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -30,8 +30,9 @@ class Maze { * @brief Construct a new Maze object * * @param start The start pose of the robot + * @param goal The goal points in the maze */ - explicit Maze(const GridPose& start); + Maze(const GridPose& start, std::unordered_set goal); /** * @brief Updates the maze walls with the current pose and new information diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 755a498..b79eeef 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -17,7 +17,7 @@ namespace micras::nav { template Mapping::Mapping(const proxy::DistanceSensors<4>& distance_sensors, Mapping::Config config) : distance_sensors{distance_sensors}, - maze{config.start}, + maze{config.start, config.goal}, wall_thickness{config.wall_thickness}, cell_size{config.cell_size}, wall_distance_threshold{config.wall_distance_threshold}, diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 358804f..a5ff2f7 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -16,14 +16,7 @@ namespace micras::nav { template -Maze::Maze(const GridPose& start) : - start(start), - goal( - {{width / 2, height / 2}, - {(width - 1) / 2, height / 2}, - {width / 2, (height - 1) / 2}, - {(width - 1) / 2, (height - 1) / 2}} - ) { +Maze::Maze(const GridPose& start, std::unordered_set goal) : start{start}, goal{goal} { for (uint8_t row = 0; row < height; row++) { this->cells[row][0].wall_count[Side::LEFT] = 0xFFFF; this->cells[row][width - 1].wall_count[Side::RIGHT] = 0xFFFF; From 9d89e0be467367bafc1a0026c3cb9026effd9ed3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 3 Oct 2024 22:24:01 -0300 Subject: [PATCH 213/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Add=20motor=20prox?= =?UTF-8?q?y=20and=20deadzone?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/pwm.cpp | 2 +- micras_nav/src/pid_controller.cpp | 3 +- micras_proxy/include/micras/proxy/fan.hpp | 2 +- .../include/micras/proxy/locomotion.hpp | 26 ++----- micras_proxy/include/micras/proxy/motor.hpp | 67 +++++++++++++++++++ micras_proxy/src/fan.cpp | 2 +- micras_proxy/src/locomotion.cpp | 35 ++-------- micras_proxy/src/motor.cpp | 41 ++++++++++++ 8 files changed, 124 insertions(+), 54 deletions(-) create mode 100644 micras_proxy/include/micras/proxy/motor.hpp create mode 100644 micras_proxy/src/motor.cpp diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index 0dd0dd8..775a6d2 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -18,7 +18,7 @@ Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_cha } void Pwm::set_duty_cycle(float duty_cycle) { - uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100)); + uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100.0F)); __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); } diff --git a/micras_nav/src/pid_controller.cpp b/micras_nav/src/pid_controller.cpp index 86be891..b1c6998 100644 --- a/micras_nav/src/pid_controller.cpp +++ b/micras_nav/src/pid_controller.cpp @@ -49,6 +49,8 @@ float PidController::update(float state, float state_change) { return this->last_response; } + this->timer.reset_us(); + float error = this->setpoint - state; this->prev_state = state; @@ -72,7 +74,6 @@ float PidController::update(float state, float state_change) { } this->last_response = response; - this->timer.reset_us(); return response; } diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index a8a8115..d9ce2ad 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -71,7 +71,7 @@ class Fan { */ enum RotationDirection : uint8_t { FORWARD, - BACKWARD + BACKWARDS }; /** diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index 9ca9ba5..8c3f155 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -12,7 +12,7 @@ #include #include "micras/hal/gpio.hpp" -#include "micras/hal/pwm.hpp" +#include "micras/proxy/motor.hpp" namespace micras::proxy { /** @@ -24,10 +24,8 @@ class Locomotion { * @brief Configuration structure for the locomotion */ struct Config { - hal::Pwm::Config pwm_left_forward; - hal::Pwm::Config pwm_left_backwards; - hal::Pwm::Config pwm_right_forward; - hal::Pwm::Config pwm_right_backwards; + Motor::Config left_motor; + Motor::Config right_motor; hal::Gpio::Config enable_gpio; }; @@ -71,24 +69,14 @@ class Locomotion { private: /** - * @brief PWM handle for the left motor forward + * @brief Left motor of the robot */ - hal::Pwm pwm_left_forward; + Motor left_motor; /** - * @brief PWM handle for the left motor backward + * @brief Right motor of the robot */ - hal::Pwm pwm_left_backwards; - - /** - * @brief PWM handle for the right motor forward - */ - hal::Pwm pwm_right_forward; - - /** - * @brief PWM handle for the right motor backward - */ - hal::Pwm pwm_right_backwards; + Motor right_motor; /** * @brief GPIO handle for the motor driver enable pin diff --git a/micras_proxy/include/micras/proxy/motor.hpp b/micras_proxy/include/micras/proxy/motor.hpp new file mode 100644 index 0000000..324a6c4 --- /dev/null +++ b/micras_proxy/include/micras/proxy/motor.hpp @@ -0,0 +1,67 @@ +/** + * @file motor.hpp + * + * @brief Proxy Motor class declaration + * + * @date 10/2024 + */ + +#ifndef MICRAS_PROXY_MOTOR_HPP +#define MICRAS_PROXY_MOTOR_HPP + +#include "micras/hal/pwm.hpp" + +namespace micras::proxy { /** + * @brief Class for controlling a motor driver + */ + +class Motor { +public: + /** + * @brief Configuration structure for the motor + */ + struct Config { + hal::Pwm::Config backwards_pwm; + hal::Pwm::Config forward_pwm; + float max_stopped_command; + float deadzone; + }; + + /** + * @brief Construct a new motor object + * + * @param config Configuration for the motor driver + */ + Motor(const Config& config); + + /** + * @brief Set the command for the motor + * + * @param command Command for the motor in percentage + */ + void set_command(float command); + +private: + /** + * @brief PWM object for controlling the motor in the backwards direction + */ + hal::Pwm backwards_pwm; + + /** + * @brief PWM object for controlling the motor in the forward direction + */ + hal::Pwm forward_pwm; + + /** + * @brief Maximum command value for the motor to be considered stopped + */ + float max_stopped_command; + + /** + * @brief Deadzone for the motor + */ + float deadzone; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_MOTOR_HPP diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp index 5ed3e24..6a16293 100644 --- a/micras_proxy/src/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -43,7 +43,7 @@ float Fan::update() { this->set_direction(RotationDirection::FORWARD); this->pwm.set_duty_cycle(this->current_speed); } else if (this->current_speed < 0.0F) { - this->set_direction(RotationDirection::BACKWARD); + this->set_direction(RotationDirection::BACKWARDS); this->pwm.set_duty_cycle(-this->current_speed); } else { this->stop(); diff --git a/micras_proxy/src/locomotion.cpp b/micras_proxy/src/locomotion.cpp index bd85041..45be2be 100644 --- a/micras_proxy/src/locomotion.cpp +++ b/micras_proxy/src/locomotion.cpp @@ -12,11 +12,7 @@ namespace micras::proxy { Locomotion::Locomotion(const Config& config) : - pwm_left_forward{config.pwm_left_forward}, - pwm_left_backwards{config.pwm_left_backwards}, - pwm_right_forward{config.pwm_right_forward}, - pwm_right_backwards{config.pwm_right_backwards}, - enable_gpio{config.enable_gpio} { + left_motor{config.left_motor}, right_motor{config.right_motor}, enable_gpio{config.enable_gpio} { this->stop(); this->enable(); } @@ -30,27 +26,8 @@ void Locomotion::disable() { } void Locomotion::set_wheel_command(float left_command, float right_command) { - if (left_command > 0.0F) { - this->pwm_left_forward.set_duty_cycle(left_command); - this->pwm_left_backwards.set_duty_cycle(0.0F); - } else if (left_command < 0.0F) { - this->pwm_left_forward.set_duty_cycle(0.0F); - this->pwm_left_backwards.set_duty_cycle(-left_command); - } else { - this->pwm_left_forward.set_duty_cycle(0.0F); - this->pwm_left_backwards.set_duty_cycle(0.0F); - } - - if (right_command > 0.0F) { - this->pwm_right_forward.set_duty_cycle(right_command); - this->pwm_right_backwards.set_duty_cycle(0.0F); - } else if (right_command < 0.0F) { - this->pwm_right_forward.set_duty_cycle(0.0F); - this->pwm_right_backwards.set_duty_cycle(-right_command); - } else { - this->pwm_right_forward.set_duty_cycle(0.0F); - this->pwm_right_backwards.set_duty_cycle(0.0F); - } + this->left_motor.set_command(left_command); + this->right_motor.set_command(right_command); } void Locomotion::set_command(float linear, float angular) { @@ -71,10 +48,6 @@ void Locomotion::set_command(float linear, float angular) { } void Locomotion::stop() { - this->pwm_left_forward.set_duty_cycle(0.0F); - this->pwm_left_backwards.set_duty_cycle(0.0F); - - this->pwm_right_forward.set_duty_cycle(0.0F); - this->pwm_right_backwards.set_duty_cycle(0.0F); + this->set_wheel_command(0.0F, 0.0F); } } // namespace micras::proxy diff --git a/micras_proxy/src/motor.cpp b/micras_proxy/src/motor.cpp new file mode 100644 index 0000000..1761d7c --- /dev/null +++ b/micras_proxy/src/motor.cpp @@ -0,0 +1,41 @@ +/** + * @file motor.cpp + * + * @brief Proxy Motor class source + * + * @date 10/2024 + */ + +#include + +#include "micras/core/utils.hpp" +#include "micras/proxy/motor.hpp" + +namespace micras::proxy { +Motor::Motor(const Config& config) : + backwards_pwm{config.backwards_pwm}, + forward_pwm{config.forward_pwm}, + max_stopped_command{config.max_stopped_command}, + deadzone{config.deadzone} { + this->set_command(0.0F); +} + +void Motor::set_command(float command) { + bool is_positive = (command >= 0.0F); + command = std::abs(command); + + if (command <= max_stopped_command) { + command = 0.0F; + } else { + command = core::remap(command, 0.0F, 100.0F, this->deadzone, 100.0F); + } + + if (is_positive) { + this->forward_pwm.set_duty_cycle(command); + this->backwards_pwm.set_duty_cycle(0.0F); + } else { + this->forward_pwm.set_duty_cycle(0.0F); + this->backwards_pwm.set_duty_cycle(command); + } +} +} // namespace micras::proxy From 95bf585fba532a17a96780f7c53d9647ca31d0b8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 4 Oct 2024 19:31:43 -0300 Subject: [PATCH 214/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20overflow=20timer?= =?UTF-8?q?=20bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/timer.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index 5126696..852097b 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -6,6 +6,8 @@ * @date 03/2024 */ +#include + #include "micras/hal/timer.hpp" namespace micras::hal { @@ -40,7 +42,13 @@ uint32_t Timer::elapsed_time_ms() const { } uint32_t Timer::elapsed_time_us() const { - return this->get_counter_us() - this->counter; + uint32_t counter = this->get_counter_us(); + + if (counter < this->counter) { + return std::numeric_limits::max() - this->counter + counter; + } else { + return counter - this->counter; + } } void Timer::sleep_ms(uint32_t time) { From e3f789a69a3cde12427e95a590956b5a9dbe2375 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 4 Oct 2024 19:33:03 -0300 Subject: [PATCH 215/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20sensor=20fusion?= =?UTF-8?q?=20from=20imu=20and=20change=20to=20data=20poling=20mode?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/imu.hpp | 16 +-- micras_proxy/src/imu.cpp | 132 +++------------------- 2 files changed, 15 insertions(+), 133 deletions(-) diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 59d9208..bb1cf77 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -59,16 +59,7 @@ class Imu { /** * @brief Update the IMU data */ - void update_data(); - - /** - * @brief Get the IMU orientation over an axis - * - * @param axis Axis to get the orientation from - * - * @return float Orientation over the desired axis using quaternions - */ - float get_orientation(Axis axis) const; + void update(); /** * @brief Get the IMU angular velocity over an axis @@ -155,11 +146,6 @@ class Imu { */ std::array linear_acceleration{}; - /** - * @brief Current orientation - */ - std::array orientation{}; - /** * @brief Gyroscope conversion factor */ diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index c82ba03..aa9f8bf 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -39,17 +39,9 @@ Imu::Imu(const Config& config) : } while (rst != LSM6DSV_READY); lsm6dsv_block_data_update_set(&(this->dev_ctx), PROPERTY_ENABLE); - lsm6dsv_fifo_watermark_set(&(this->dev_ctx), 3); - lsm6dsv_fifo_stop_on_wtm_set(&(this->dev_ctx), PROPERTY_ENABLE); - lsm6dsv_fifo_mode_set(&(this->dev_ctx), LSM6DSV_STREAM_MODE); lsm6dsv_gy_data_rate_set(&(this->dev_ctx), config.gyroscope_data_rate); lsm6dsv_xl_data_rate_set(&(this->dev_ctx), config.accelerometer_data_rate); - lsm6dsv_sflp_data_rate_set(&(this->dev_ctx), config.orientation_data_rate); - - lsm6dsv_fifo_gy_batch_set(&(this->dev_ctx), static_cast(config.gyroscope_data_rate)); - lsm6dsv_fifo_xl_batch_set(&(this->dev_ctx), static_cast(config.accelerometer_data_rate)); - lsm6dsv_fifo_sflp_batch_set(&dev_ctx, {.game_rotation = 1, .gravity = 0, .gbias = 0}); lsm6dsv_gy_full_scale_set(&(this->dev_ctx), config.gyroscope_scale); lsm6dsv_xl_full_scale_set(&(this->dev_ctx), config.accelerometer_scale); @@ -60,8 +52,6 @@ Imu::Imu(const Config& config) : lsm6dsv_filt_gy_lp1_bandwidth_set(&(this->dev_ctx), config.gyroscope_filter); lsm6dsv_filt_xl_lp2_set(&(this->dev_ctx), PROPERTY_ENABLE); lsm6dsv_filt_xl_lp2_bandwidth_set(&(this->dev_ctx), config.accelerometer_filter); - - lsm6dsv_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE); } bool Imu::check_whoami() { @@ -72,57 +62,23 @@ bool Imu::check_whoami() { return whoami == LSM6DSV_ID; } -void Imu::update_data() { - lsm6dsv_fifo_status_t fifo_status; - lsm6dsv_fifo_status_get(&(this->dev_ctx), &fifo_status); +void Imu::update() { + std::array raw_data{}; + lsm6dsv_all_sources_t all_sources; + lsm6dsv_all_sources_get(&dev_ctx, &all_sources); - if (not fifo_status.fifo_th) { - return; + if (all_sources.drdy_xl) { + lsm6dsv_acceleration_raw_get(&dev_ctx, raw_data.data()); + this->linear_acceleration[0] = raw_data[0] * xl_factor; + this->linear_acceleration[1] = raw_data[1] * xl_factor; + this->linear_acceleration[2] = raw_data[2] * xl_factor; } - uint16_t samples = fifo_status.fifo_level; - - while ((samples--) > 0) { - lsm6dsv_fifo_out_raw_t f_data; - lsm6dsv_fifo_out_raw_get(&(this->dev_ctx), &f_data); - auto* axis = reinterpret_cast(f_data.data); // NOLINT(cppcoreguidelines-pro-type-reinterpret-cast) - - switch (f_data.tag) { - case lsm6dsv_fifo_out_raw_t::LSM6DSV_GY_NC_TAG: - this->angular_velocity[0] = axis[0] * gy_factor; - this->angular_velocity[1] = axis[1] * gy_factor; - this->angular_velocity[2] = axis[2] * gy_factor; - break; - - case lsm6dsv_fifo_out_raw_t::LSM6DSV_XL_NC_TAG: - this->linear_acceleration[0] = axis[0] * xl_factor; - this->linear_acceleration[1] = axis[1] * xl_factor; - this->linear_acceleration[2] = axis[2] * xl_factor; - break; - - case lsm6dsv_fifo_out_raw_t::LSM6DSV_SFLP_GAME_ROTATION_VECTOR_TAG: - this->update_orientation(std::bit_cast(axis)); - break; - - default: - break; - } - } -} - -float Imu::get_orientation(Axis axis) const { - switch (axis) { - case Axis::X: - return this->orientation[0]; - - case Axis::Y: - return this->orientation[1]; - - case Axis::Z: - return this->orientation[2]; - - default: - return 0.0F; + if (all_sources.drdy_gy) { + lsm6dsv_angular_rate_raw_get(&dev_ctx, raw_data.data()); + this->angular_velocity[0] = raw_data[0] * gy_factor; + this->angular_velocity[1] = raw_data[1] * gy_factor; + this->angular_velocity[2] = raw_data[2] * gy_factor; } } @@ -182,64 +138,4 @@ int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint return 0; } - -// NOLINTNEXTLINE(*-avoid-c-arrays) -void Imu::update_orientation(const uint16_t sflp[3]) { - std::array quat{}; - - quat[0] = half_to_float(sflp[0]); - quat[1] = half_to_float(sflp[1]); - quat[2] = half_to_float(sflp[2]); - - float sumsq = 0; - - for (uint8_t i = 0; i < 3; i++) { - sumsq += quat[i] * quat[i]; // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index) - } - - if (sumsq > 1.0F) { - float norm = std::sqrt(sumsq); - quat[0] /= norm; - quat[1] /= norm; - quat[2] /= norm; - sumsq = 1.0F; - } - - quat[3] = std::sqrt(1.0F - sumsq); - - // roll (x-axis rotation) - float sinp = 2 * (quat[3] * quat[0] + quat[1] * quat[2]); - float cosp = 1 - 2 * (quat[0] * quat[0] + quat[1] * quat[1]); - this->orientation[0] = std::atan2(sinp, cosp); - - // pitch (y-axis rotation) - sinp = std::sqrt(1 + 2 * (quat[3] * quat[1] - quat[0] * quat[2])); - cosp = std::sqrt(1 - 2 * (quat[3] * quat[1] - quat[0] * quat[2])); - this->orientation[1] = 2 * std::atan2(sinp, cosp) - std::numbers::pi_v / 2; - - // yaw (z-axis rotation) - sinp = 2 * (quat[3] * quat[2] + quat[0] * quat[1]); - cosp = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]); - this->orientation[2] = std::atan2(sinp, cosp); -} - -// NOLINTBEGIN(readability-identifier-length, readability-implicit-bool-conversion) -float Imu::half_to_float(uint16_t x) { - static constexpr auto as_float = [](uint32_t x) -> float { - void* aux = &x; - return *std::bit_cast(aux); - }; - static constexpr auto as_uint = [](float x) -> uint32_t { - void* aux = &x; - return *std::bit_cast(aux); - }; - - const uint32_t e = (x & 0x7C00) >> 10; - const uint32_t m = (x & 0x03FF) << 13; - const uint32_t v = as_uint(static_cast(m)) >> 23; - return as_float( - (x & 0x8000) << 16 | (e != 0) * ((e + 112) << 23 | m) | - ((e == 0) & (m != 0)) * ((v - 37) << 23 | ((m << (150 - v)) & 0x007FE000)) - ); -} // NOLINTEND(readability-identifier-length, readability-implicit-bool-conversion) } // namespace micras::proxy From 87fd2dd4293b2366f9e8e939ef29c2780635d4e3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 4 Oct 2024 20:16:06 -0300 Subject: [PATCH 216/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Centralize=20micro?= =?UTF-8?q?seconds=20loop=20timer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/go_to_point.hpp | 5 ++- .../include/micras/nav/look_at_point.hpp | 15 +++++-- micras_nav/include/micras/nav/odometry.hpp | 36 ++++++++++++---- .../include/micras/nav/pid_controller.hpp | 8 ++-- micras_nav/src/go_to_point.cpp | 6 +-- micras_nav/src/look_at_point.cpp | 12 ++++-- micras_nav/src/odometry.cpp | 42 ++++--------------- micras_nav/src/pid_controller.cpp | 25 +++-------- .../include/micras/proxy/locomotion.hpp | 7 ++++ 9 files changed, 81 insertions(+), 75 deletions(-) diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index fa966b6..922493f 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -41,15 +41,18 @@ class GoToPoint { * * @param state The current state of the robot * @param goal The goal point + * @param elapsed_time The time elapsed since the last update + * * @return The command to go to the point */ - Twist action(const State& state, const Point& goal); + Twist action(const State& state, const Point& goal, float elapsed_time); /** * @brief Checks if the robot has reached the goal * * @param state The current state of the robot * @param goal The goal point + * * @return True if the robot has reached the goal, false otherwise */ bool finished(const State& state, const Point& goal) const; diff --git a/micras_nav/include/micras/nav/look_at_point.hpp b/micras_nav/include/micras/nav/look_at_point.hpp index 5f3b9f1..20e98e3 100644 --- a/micras_nav/include/micras/nav/look_at_point.hpp +++ b/micras_nav/include/micras/nav/look_at_point.hpp @@ -22,7 +22,8 @@ class LookAtPoint { * @brief Configuration for the LookAtPoint class */ struct Config { - PidController::Config pid; + PidController::Config linear_pid; + PidController::Config angular_pid; float tolerance{}; }; @@ -38,24 +39,32 @@ class LookAtPoint { * * @param pose The current pose of the robot * @param goal The goal point + * @param elapsed_time The time elapsed since the last update + * * @return The command to look at the point */ - Twist action(const Pose& pose, const Point& goal); + Twist action(const State& state, const Point& goal, float elapsed_time); /** * @brief Checks if the robot has reached the goal * * @param pose The current pose of the robot * @param goal The goal point + * * @return True if the robot has reached the goal, false otherwise */ bool finished(const Pose& pose, const Point& goal) const; private: + /** + * @brief PID controller for the linear velocity + */ + PidController linear_pid; + /** * @brief PID controller for the angular velocity */ - PidController pid; + PidController angular_pid; /** * @brief Angular tolerance for the goal diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 3596aca..338de65 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -40,13 +40,15 @@ class Odometry { */ Odometry( const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - const proxy::Imu& imu, hal::Timer::Config timer_config, Config config + const proxy::Imu& imu, Config config ); /** * @brief Update the odometry + * + * @param elapsed_time Time since the last update */ - void update(); + void update(float elapsed_time); /** * @brief Get the state of the robot @@ -63,6 +65,31 @@ class Odometry { const State& get_imu_state() const; private: + /** + * @brief Update a state based on the position variation + * + * @param state State to be updated + * @param linear_distance Linear distance from last state + * @param angular_distance Angular distance from last state + * @param linear_velocity Measured linear velocity of the robot + * @param angular_velocity Measured angular velocity of the robot + */ + static constexpr void update_state( + State& state, float linear_distance, float angular_distance, float linear_velocity, float angular_velocity + ) { + float half_angle = angular_distance / 2; + float linear_diagonal = + angular_distance < 0.05F ? linear_distance : std::abs(std::sin(half_angle) * linear_distance / half_angle); + + state.pose.position.x += linear_diagonal * std::cos(state.pose.orientation + half_angle); + state.pose.position.y += linear_diagonal * std::sin(state.pose.orientation + half_angle); + + state.pose.orientation += angular_distance; + + state.velocity.linear = linear_velocity; + state.velocity.angular = angular_velocity; + } + /** * @brief Left rotary sensor */ @@ -78,11 +105,6 @@ class Odometry { */ const proxy::Imu& imu; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) - /** - * @brief Timer for velocity calculation - */ - hal::Timer timer; - /** * @brief Wheel radius */ diff --git a/micras_nav/include/micras/nav/pid_controller.hpp b/micras_nav/include/micras/nav/pid_controller.hpp index bcc7fbb..7a5d370 100644 --- a/micras_nav/include/micras/nav/pid_controller.hpp +++ b/micras_nav/include/micras/nav/pid_controller.hpp @@ -50,20 +50,22 @@ class PidController { * @brief Update PID with new state and return response * * @param state Current value of the controlled variable + * @param elapsed_time Time since the last update * * @return Response */ - float update(float state); + float update(float state, float elapsed_time); /** * @brief Update PID with new state and return response * * @param state Current value of the controlled variable * @param state_change Derivative of the controlled variable + * @param elapsed_time Time since the last update * * @return Response */ - float update(float state, float state_change); + float update(float state, float elapsed_time, float state_change); private: float kp; /**< Proportional constant */ @@ -75,8 +77,6 @@ class PidController { float error_acc = 0; /**< Accumulated error for i term */ float prev_state = 0; /**< Previous state for d term */ float last_response = 0; /**< Last response returned by the controller */ - - hal::Timer timer; /**< Timer used to compute the loop time */ }; } // namespace micras::nav diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp index 16cc54f..83ac83d 100644 --- a/micras_nav/src/go_to_point.cpp +++ b/micras_nav/src/go_to_point.cpp @@ -19,12 +19,12 @@ GoToPoint::GoToPoint(Config config) : linear_decay_damping{config.linear_decay_damping}, tolerance{config.tolerance} { } -Twist GoToPoint::action(const State& state, const Point& goal) { +Twist GoToPoint::action(const State& state, const Point& goal, float elapsed_time) { float angular_error = core::assert_angle(state.pose.orientation - state.pose.position.angle_between(goal)); float linear_target = core::decay(angular_error, this->linear_decay_damping) * this->base_speed; - float angular_command = this->angular_pid.update(angular_error); - float linear_command = this->linear_pid.update(state.velocity.linear - linear_target); + float angular_command = this->angular_pid.update(angular_error, elapsed_time); + float linear_command = this->linear_pid.update(state.velocity.linear - linear_target, elapsed_time); return {linear_command, angular_command}; } diff --git a/micras_nav/src/look_at_point.cpp b/micras_nav/src/look_at_point.cpp index 78cf096..0f6829f 100644 --- a/micras_nav/src/look_at_point.cpp +++ b/micras_nav/src/look_at_point.cpp @@ -10,11 +10,15 @@ #include "micras/nav/look_at_point.hpp" namespace micras::nav { -LookAtPoint::LookAtPoint(Config config) : pid(config.pid), tolerance{config.tolerance} { } +LookAtPoint::LookAtPoint(Config config) : + linear_pid(config.linear_pid), angular_pid{config.angular_pid}, tolerance{config.tolerance} { } -Twist LookAtPoint::action(const Pose& pose, const Point& goal) { - float angular_target = pose.position.angle_between(goal); - return {0.0F, this->pid.update(core::assert_angle(pose.orientation - angular_target))}; +Twist LookAtPoint::action(const State& state, const Point& goal, float elapsed_time) { + float angular_target = state.pose.position.angle_between(goal); + return { + this->linear_pid.update(state.velocity.linear, elapsed_time), + this->angular_pid.update(core::assert_angle(state.pose.orientation - angular_target), elapsed_time) + }; } bool LookAtPoint::finished(const Pose& pose, const Point& goal) const { diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 032bcb8..6cd8875 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -9,17 +9,17 @@ #include #include +#include "micras/core/utils.hpp" #include "micras/nav/odometry.hpp" namespace micras::nav { Odometry::Odometry( const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - const proxy::Imu& imu, hal::Timer::Config timer_config, Config config + const proxy::Imu& imu, Config config ) : left_rotary_sensor{left_rotary_sensor}, right_rotary_sensor{right_rotary_sensor}, imu{imu}, - timer{timer_config}, wheel_radius{config.wheel_radius}, wheel_separation{config.wheel_separation}, left_last_position{left_rotary_sensor.get_position()}, @@ -29,17 +29,10 @@ Odometry::Odometry( state{config.initial_pose, {0.0F, 0.0F}}, imu_state{config.initial_pose, {0.0F, 0.0F}} { } -void Odometry::update() { +void Odometry::update(float elapsed_time) { float left_position = this->left_rotary_sensor.get_position(); float right_position = this->right_rotary_sensor.get_position(); - float elapsed_time = this->timer.elapsed_time_us() / 1000000.0F; - this->timer.reset_us(); - - if (elapsed_time == 0) { - return; - } - float left_distance = this->wheel_radius * (left_position - this->left_last_position); float right_distance = this->wheel_radius * (right_position - this->right_last_position); @@ -48,30 +41,13 @@ void Odometry::update() { float linear_distance = (left_distance + right_distance) / 2; float angular_distance = (right_distance - left_distance) / this->wheel_separation; - float half_angle = angular_distance / 2; - float linear_diagonal = - angular_distance < 0.05F ? linear_distance : std::abs(std::sin(half_angle) * linear_distance / half_angle); - - this->state.pose.position.x += linear_diagonal * std::cos(this->state.pose.orientation + half_angle); - this->state.pose.position.y += linear_diagonal * std::sin(this->state.pose.orientation + half_angle); - - this->state.pose.orientation += angular_distance; - - this->state.velocity.linear = this->linear_filter.update(linear_distance / elapsed_time); - this->state.velocity.angular = this->angular_filter.update(angular_distance / elapsed_time); - - angular_distance = this->imu.get_angular_velocity(proxy::Imu::Axis::Z) * elapsed_time; - half_angle = angular_distance / 2; - linear_diagonal = - angular_distance < 0.05F ? linear_distance : std::abs(std::sin(half_angle) * linear_distance / half_angle); - - this->imu_state.pose.position.x += linear_diagonal * std::cos(this->imu_state.pose.orientation + half_angle); - this->imu_state.pose.position.y += linear_diagonal * std::sin(this->imu_state.pose.orientation + half_angle); - - this->imu_state.pose.orientation += angular_distance; + float linear_velocity = this->linear_filter.update(linear_distance / elapsed_time); + float angular_velocity = this->angular_filter.update(angular_distance / elapsed_time); + update_state(this->state, linear_distance, angular_distance, linear_velocity, angular_velocity); - this->imu_state.velocity.linear = linear_distance / elapsed_time; - this->imu_state.velocity.angular = this->imu.get_angular_velocity(proxy::Imu::Axis::Z); + angular_velocity = this->imu.get_angular_velocity(proxy::Imu::Axis::Z); + angular_distance = angular_velocity * elapsed_time; + update_state(this->imu_state, linear_distance, angular_distance, linear_velocity, angular_velocity); } const nav::State& Odometry::get_state() const { diff --git a/micras_nav/src/pid_controller.cpp b/micras_nav/src/pid_controller.cpp index b1c6998..bb2fdb4 100644 --- a/micras_nav/src/pid_controller.cpp +++ b/micras_nav/src/pid_controller.cpp @@ -30,27 +30,12 @@ void PidController::reset() { this->last_response = 0; } -float PidController::update(float state) { - float loop_time = this->timer.elapsed_time_us() / 1000000.0F; - - if (loop_time == 0) { - return this->last_response; - } - - float state_change = (state - this->prev_state) / loop_time; - - return this->update(state, state_change); +float PidController::update(float state, float elapsed_time) { + float state_change = (state - this->prev_state) / elapsed_time; + return this->update(state, elapsed_time, state_change); } -float PidController::update(float state, float state_change) { - float loop_time = this->timer.elapsed_time_us() / 1000000.0F; - - if (loop_time == 0) { - return this->last_response; - } - - this->timer.reset_us(); - +float PidController::update(float state, float elapsed_time, float state_change) { float error = this->setpoint - state; this->prev_state = state; @@ -58,7 +43,7 @@ float PidController::update(float state, float state_change) { if (this->saturation < 0 or std::abs(response) < this->saturation or (this->error_acc != 0 and std::signbit(this->error_acc) != std::signbit(error))) { - this->error_acc += error * loop_time; + this->error_acc += error * elapsed_time; } if (this->max_integral >= 0 and this->ki > 0) { diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index 8c3f155..2249c0e 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -67,6 +67,13 @@ class Locomotion { */ void stop(); + /** + * @brief Update the locomotion state + * + * @param elapsed_time Time since the last update + */ + void update(float elapsed_time); + private: /** * @brief Left motor of the robot From 461e2dccf8b51484cf51d702dcc77cca4ed2d610 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 18:28:40 -0300 Subject: [PATCH 217/561] =?UTF-8?q?=F0=9F=94=92=EF=B8=8F=20Make=20locomoti?= =?UTF-8?q?on=20driver=20disabled=20by=20default?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/locomotion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_proxy/src/locomotion.cpp b/micras_proxy/src/locomotion.cpp index 45be2be..a5bf2d2 100644 --- a/micras_proxy/src/locomotion.cpp +++ b/micras_proxy/src/locomotion.cpp @@ -14,7 +14,7 @@ namespace micras::proxy { Locomotion::Locomotion(const Config& config) : left_motor{config.left_motor}, right_motor{config.right_motor}, enable_gpio{config.enable_gpio} { this->stop(); - this->enable(); + this->disable(); } void Locomotion::enable() { From 85f6114b2c1fdf079801ee1516e0861ccbb4660c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 18:30:26 -0300 Subject: [PATCH 218/561] =?UTF-8?q?=F0=9F=8E=A8=20Add=20busy=20check=20to?= =?UTF-8?q?=20PwmDma?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/pwm_dma.hpp | 7 +++++++ micras_hal/stm32/src/pwm_dma.cpp | 6 +++++- micras_proxy/src/argb.cpp | 6 +++++- 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp index cf688de..3eb652b 100644 --- a/micras_hal/include/micras/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -62,6 +62,13 @@ class PwmDma { */ uint32_t get_compare(float duty_cycle) const; + /** + * @brief Check if the PWM is busy + * + * @return true If the PWM is busy, false otherwise + */ + bool is_busy(); + private: /** * @brief Timer handle diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index 2f7ad3c..1ecdd70 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -17,7 +17,7 @@ PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.tim } void PwmDma::start_dma(std::span buffer) { - if (TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_CHANNEL_STATE_BUSY) { + if (this->is_busy()) { return; } @@ -35,4 +35,8 @@ void PwmDma::stop_dma() { uint32_t PwmDma::get_compare(float duty_cycle) const { return std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100) - 1); } + +bool PwmDma::is_busy() { + return TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_CHANNEL_STATE_BUSY; +} } // namespace micras::hal diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index e7b451b..80e7486 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -23,7 +23,7 @@ Argb::Argb(const Config& config) : template void Argb::set_color(const Color& color, uint8_t index) { - if (index >= num_of_leds) { + if (index >= num_of_leds or this->pwm.is_busy()) { return; } @@ -33,6 +33,10 @@ void Argb::set_color(const Color& color, uint8_t index) { template void Argb::set_color(const Color& color) { + if (this->pwm.is_busy()) { + return; + } + for (uint8_t i = 0; i < num_of_leds; i++) { this->encode_color(color * this->brightness, i); } From 3b38e0dfe0b6be619b4d1c6695b5e0879297b6f1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 18:32:47 -0300 Subject: [PATCH 219/561] =?UTF-8?q?=E2=9C=A8=20=F0=9F=9A=91=20Fix=20assert?= =?UTF-8?q?=20angle=20functions=20and=20add=20is=5Fnear=20to=20utils?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/utils.hpp | 39 ++++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index 0e5bde8..323380b 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -98,7 +98,44 @@ constexpr std::array make_array(C value) { */ constexpr float assert_angle(float angle) { angle = std::fmod(angle, 2 * std::numbers::pi_v); - return angle > std::numbers::pi_v ? angle - 2 * std::numbers::pi_v : angle; + + if (angle > std::numbers::pi_v) { + angle -= 2 * std::numbers::pi_v; + } else if (angle < -std::numbers::pi_v) { + angle += 2 * std::numbers::pi_v; + } + + return angle; +} + +/** + * @brief Assert an angle to be in the range [-pi/2, pi/2] + * + * @param angle Angle to be asserted + * @return float Asserted angle + */ +constexpr float assert_half_angle(float angle) { + angle = std::fmod(angle, std::numbers::pi_v); + + if (angle > std::numbers::pi_v / 2.0F) { + angle -= std::numbers::pi_v; + } else if (angle < -std::numbers::pi_v / 2.0F) { + angle += std::numbers::pi_v; + } + + return angle; +} + +/** + * @brief Check if two floating point numbers are near each other + * + * @param a First number + * @param b Second number + * @param tolerance Tolerance + * @return true If the numbers are near each other, false otherwise + */ +constexpr bool is_near(float a, float b, float tolerance = 0.001) { + return std::abs(a - b) < tolerance; } } // namespace micras::core From bdb3394fa29b23c58c652144dd2b21beb86ef6ee Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 18:34:29 -0300 Subject: [PATCH 220/561] =?UTF-8?q?=F0=9F=92=A9=20Hardcode=20wall=20at=20t?= =?UTF-8?q?he=20right=20of=20robot=20on=20start?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index a5ff2f7..75f7cb9 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -27,6 +27,10 @@ Maze::Maze(const GridPose& start, std::unordered_set g this->cells[height - 1][col].wall_count[Side::UP] = 0xFFFF; } + this->cells[start.position.y][start.position.x].wall_count[start.turned_right().orientation] = 0xFFFF; + GridPose right_pose = start.turned_right().front(); + this->get_cell(right_pose.position).wall_count[right_pose.turned_back().orientation] = 0xFFFF; + for (const auto& position : this->goal) { this->get_cell(position).cost = 0; } From 5b282dbef039777d1dc76d2877287308ac370393 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 18:39:13 -0300 Subject: [PATCH 221/561] =?UTF-8?q?=F0=9F=9A=B8=20Change=20next=20goal=20t?= =?UTF-8?q?o=20pose=20instead=20of=20point?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/types.hpp | 17 +++++++++ micras_nav/include/micras/nav/grid_pose.hpp | 23 ++++------- micras_nav/include/micras/nav/maze.hpp | 6 +-- micras_nav/src/maze.cpp | 42 ++++++++++++--------- 4 files changed, 52 insertions(+), 36 deletions(-) create mode 100644 micras_core/include/micras/core/types.hpp diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp new file mode 100644 index 0000000..d42e802 --- /dev/null +++ b/micras_core/include/micras/core/types.hpp @@ -0,0 +1,17 @@ +#ifndef MICRAS_CORE_TYPES_HPP +#define MICRAS_CORE_TYPES_HPP + +#include + +namespace micras::core { +/** + * @brief Possible values for the observation of a wall + */ +enum Observation : uint8_t { + UNKNOWN = 0, + FREE_SPACE = 1, + WALL = 2 +}; +} // namespace micras::core + +#endif // MICRAS_CORE_TYPES_HPP diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index 618ead4..eff3d6e 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -12,6 +12,8 @@ #include #include +#include "micras/core/types.hpp" + namespace micras::nav { /** * @brief Possible directions in the grid @@ -32,26 +34,17 @@ enum Side : uint8_t { Side angle_to_grid(float angle); /** - * @brief Type to store information originating from the distance sensors + * @brief Type to store information originating from the wall sensors */ struct Information { - /** - * @brief Possible values for the existence of a wall - */ - enum Existence : uint8_t { - UNKNOWN = 0, - FREE = 1, - WALL = 2 - }; - /** * @brief Possible walls in the grid to be checked */ - Existence left; - Existence front_left; - Existence front; - Existence front_right; - Existence right; + core::Observation left; + core::Observation front_left; + core::Observation front; + core::Observation front_right; + core::Observation right; }; /** diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index fb7eef8..d0d2290 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -38,7 +38,7 @@ class Maze { * @brief Updates the maze walls with the current pose and new information * * @param pose The pose of the robot - * @param information The information from the distance sensors + * @param information The information from the wall sensors */ void update(const GridPose& pose, Information information); @@ -49,7 +49,7 @@ class Maze { * @param force_costmap Whether to force the use of the update of the costmap * @return The next point the robot should go to */ - GridPoint get_current_goal(const GridPoint& position, bool force_costmap = false) const; + GridPose get_current_goal(const GridPoint& position, bool force_costmap = false) const; private: /** @@ -126,7 +126,7 @@ class Maze { /** * @brief Current best found route to the goal */ - std::map> best_route; + std::map> best_route; }; } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 75f7cb9..729af8f 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -51,41 +51,42 @@ void Maze::update(const GridPose& pose, Information information) return; } - if (information.left != Information::UNKNOWN) { - this->update_wall(pose.turned_left(), information.left == Information::WALL); + if (information.left != core::Observation::UNKNOWN) { + this->update_wall(pose.turned_left(), information.left == core::Observation::WALL); } - if (information.front_left != Information::UNKNOWN) { - this->update_wall(pose.front().turned_left(), information.front_left == Information::WALL); + if (information.front_left != core::Observation::UNKNOWN) { + this->update_wall(pose.front().turned_left(), information.front_left == core::Observation::WALL); } - if (information.front != Information::UNKNOWN) { - this->update_wall(pose, information.front == Information::WALL); + if (information.front != core::Observation::UNKNOWN) { + this->update_wall(pose, information.front == core::Observation::WALL); } - if (information.front_right != Information::UNKNOWN) { - this->update_wall(pose.front().turned_right(), information.front_right == Information::WALL); + if (information.front_right != core::Observation::UNKNOWN) { + this->update_wall(pose.front().turned_right(), information.front_right == core::Observation::WALL); } - if (information.right != Information::UNKNOWN) { - this->update_wall(pose.turned_right(), information.right == Information::WALL); + if (information.right != core::Observation::UNKNOWN) { + this->update_wall(pose.turned_right(), information.right == core::Observation::WALL); } this->calculate_costmap(); } template -GridPoint Maze::get_current_goal(const GridPoint& position, bool force_costmap) const { +GridPose Maze::get_current_goal(const GridPoint& position, bool force_costmap) const { uint16_t current_cost = this->get_cell(position).cost; if (not force_costmap and (not this->exploring or this->returning) and this->best_route.contains(current_cost) and - this->best_route.at(current_cost) == position) { + this->best_route.at(current_cost).position == position) { return (this->returning ? std::prev(this->best_route.find(current_cost)) : std::next(this->best_route.find(current_cost))) ->second; } GridPoint next_position = position; + Side next_side{}; for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { Side side = static_cast(i); @@ -94,10 +95,11 @@ GridPoint Maze::get_current_goal(const GridPoint& position, bool if (not this->has_wall({position, side}) and this->get_cell(front_position).cost <= current_cost) { current_cost = this->get_cell(front_position).cost; next_position = front_position; + next_side = side; } } - return next_position; + return {next_position, next_side}; } template @@ -112,6 +114,10 @@ Maze::Cell& Maze::get_cell(const GridPoint& positi template void Maze::update_wall(const GridPose& pose, bool wall) { + if (pose.position.x >= width or pose.position.y >= height) { + return; + } + if (wall) { this->get_cell(pose.position).wall_count[pose.orientation]++; } else { @@ -177,13 +183,13 @@ void Maze::calculate_costmap() { return; } - GridPoint current_position = this->start.position; + GridPose current_pose = this->start; this->best_route.clear(); - this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start.position); + this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start); - while (not this->goal.contains(current_position)) { - current_position = this->get_current_goal(current_position, true); - this->best_route.try_emplace(this->get_cell(current_position).cost, current_position); + while (not this->goal.contains(current_pose.position)) { + current_pose = this->get_current_goal(current_pose.position, true); + this->best_route.try_emplace(this->get_cell(current_pose.position).cost, current_pose); } } } // namespace micras::nav From 4bb1fd06da9c8790bff65b7da05a178bfeec2536 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 19:29:10 -0300 Subject: [PATCH 222/561] =?UTF-8?q?=E2=9C=A8=20Add=20rotate=20and=20operat?= =?UTF-8?q?ors=20to=20point=20structure?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/state.hpp | 24 ++++++++++++++++++++++++ micras_nav/src/state.cpp | 23 +++++++++++++++++++++++ 2 files changed, 47 insertions(+) diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index fa7409f..0b77a83 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -40,6 +40,30 @@ struct Point { */ GridPoint to_grid(float cell_size) const; + /** + * @brief Rotates the point by a given angle + * + * @param angle The angle to rotate the point by + * @return The rotated point + */ + Point rotate(Side angle); + + /** + * @brief Subtracts a point from another + * + * @param other The point to subtract + * @return The result of the subtraction + */ + Point operator-(const Point& other) const; + + /** + * @brief Compares two points for equality + * + * @param other The other point to compare + * @return True if the points are equal, false otherwise + */ + bool operator==(const Point& other) const; + /** * @brief Calculates the remainder of the division of the point by a value * diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 12faa2c..6c8327e 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -24,6 +24,29 @@ GridPoint Point::to_grid(float cell_size) const { return {static_cast(this->x / cell_size), static_cast(this->y / cell_size)}; } +Point Point::rotate(Side angle) { + switch (angle) { + case Side::RIGHT: + return {-this->y, this->x}; + case Side::UP: + return *this; + case Side::LEFT: + return {this->y, -this->x}; + case Side::DOWN: + return {-this->x, -this->y}; + } + + return *this; +} + +Point Point::operator-(const Point& other) const { + return {this->x - other.x, this->y - other.y}; +} + +bool Point::operator==(const Point& other) const { + return this->x == other.x and this->y == other.y; +} + Point Point::operator%(float value) const { return {std::fmod(this->x, value), std::fmod(this->y, value)}; } From 20b6c8fe3c772d1755eda320d03094e4f8901f4f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 19:36:22 -0300 Subject: [PATCH 223/561] =?UTF-8?q?=F0=9F=94=A8=20Add=20save=20parameter?= =?UTF-8?q?=20to=20pid=20for=20calibration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/pid_controller.hpp | 6 +++-- micras_nav/src/pid_controller.cpp | 24 +++++++++++++++---- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/micras_nav/include/micras/nav/pid_controller.hpp b/micras_nav/include/micras/nav/pid_controller.hpp index 7a5d370..8756353 100644 --- a/micras_nav/include/micras/nav/pid_controller.hpp +++ b/micras_nav/include/micras/nav/pid_controller.hpp @@ -51,10 +51,11 @@ class PidController { * * @param state Current value of the controlled variable * @param elapsed_time Time since the last update + * @param save Whether to save the variables for calibration * * @return Response */ - float update(float state, float elapsed_time); + float update(float state, float elapsed_time, bool save = false); /** * @brief Update PID with new state and return response @@ -62,10 +63,11 @@ class PidController { * @param state Current value of the controlled variable * @param state_change Derivative of the controlled variable * @param elapsed_time Time since the last update + * @param save Whether to save the variables for calibration * * @return Response */ - float update(float state, float elapsed_time, float state_change); + float update(float state, float elapsed_time, float state_change, bool save = false); private: float kp; /**< Proportional constant */ diff --git a/micras_nav/src/pid_controller.cpp b/micras_nav/src/pid_controller.cpp index bb2fdb4..c23ddbf 100644 --- a/micras_nav/src/pid_controller.cpp +++ b/micras_nav/src/pid_controller.cpp @@ -11,6 +11,12 @@ #include "micras/nav/pid_controller.hpp" +static volatile float test_error; +static volatile float test_proportional; +static volatile float test_integrative; +static volatile float test_derivative; +static volatile float test_response; + namespace micras::nav { PidController::PidController(Config config) : kp{config.kp}, @@ -30,16 +36,16 @@ void PidController::reset() { this->last_response = 0; } -float PidController::update(float state, float elapsed_time) { +float PidController::update(float state, float elapsed_time, bool save) { float state_change = (state - this->prev_state) / elapsed_time; - return this->update(state, elapsed_time, state_change); + return this->update(state, elapsed_time, state_change, save); } -float PidController::update(float state, float elapsed_time, float state_change) { +float PidController::update(float state, float elapsed_time, float state_change, bool save) { float error = this->setpoint - state; this->prev_state = state; - float response = this->kp * (error + this->ki * this->error_acc + this->kd * state_change); + float response = this->kp * (error + this->ki * this->error_acc - this->kd * state_change); if (this->saturation < 0 or std::abs(response) < this->saturation or (this->error_acc != 0 and std::signbit(this->error_acc) != std::signbit(error))) { @@ -52,7 +58,7 @@ float PidController::update(float state, float elapsed_time, float state_change) ); } - response = this->kp * (error + this->ki * this->error_acc + this->kd * state_change); + response = this->kp * (error + this->ki * this->error_acc - this->kd * state_change); if (this->saturation >= 0 and std::abs(response) >= this->saturation) { response = std::clamp(response, -this->saturation, this->saturation); @@ -60,6 +66,14 @@ float PidController::update(float state, float elapsed_time, float state_change) this->last_response = response; + if (save) { + test_error = error; + test_proportional = this->kp * error; + test_integrative = this->kp * this->ki * this->error_acc; + test_derivative = -this->kp * this->kd * state_change; + test_response = response; + } + return response; } } // namespace micras::nav From f8843b48056867170528486edd073f60e65581e0 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 19:45:36 -0300 Subject: [PATCH 224/561] =?UTF-8?q?=E2=9C=A8=20Add=20FollowWall=20controll?= =?UTF-8?q?er=20and=20update=20GoToPoint=20and=20LookAtPoint?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 119 ++++++++++++++++++ micras_nav/include/micras/nav/go_to_point.hpp | 56 +++++++-- .../include/micras/nav/look_at_point.hpp | 17 ++- micras_nav/src/follow_wall.cpp | 54 ++++++++ micras_nav/src/go_to_point.cpp | 51 ++++++-- micras_nav/src/look_at_point.cpp | 21 +++- 6 files changed, 294 insertions(+), 24 deletions(-) create mode 100644 micras_nav/include/micras/nav/follow_wall.hpp create mode 100644 micras_nav/src/follow_wall.cpp diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp new file mode 100644 index 0000000..b63fa23 --- /dev/null +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -0,0 +1,119 @@ +/** + * @file follow_wall.hpp + * + * @brief Nav FollowWall class declaration + * + * @date 10/2024 + */ + +#ifndef MICRAS_NAV_FOLLOW_WALL +#define MICRAS_NAV_FOLLOW_WALL + +#include "micras/core/butterworth_filter.hpp" +#include "micras/nav/pid_controller.hpp" +#include "micras/proxy/wall_sensors.hpp" + +namespace micras::nav { +/** + * @brief Class to follow a wall using a PID controller + */ +class FollowWall { +public: + /** + * @brief Configuration for the FollowWall class + */ + struct Config { + PidController::Config pid; + float can_follow_tolerance; + float base_left_reading; + float base_right_reading; + float cutoff_frequency; + }; + + /** + * @brief Construct a new FollowWall object + * + * @param wall_sensors The wall sensors of the robot + * @param config The configuration for the FollowWall class + */ + FollowWall(const proxy::WallSensors<4>& wall_sensors, const Config& config); + + /** + * @brief Update the PID controller and return the response + * + * @param elapsed_time The time elapsed since the last update + * + * @return The response of the PID controller + */ + float action(float elapsed_time); + + /** + * @brief Checks if the robot can follow the wall + * + * @return True if the robot can follow the wall, false otherwise + */ + bool can_follow(); + + /** + * @brief Resets the PID controller + */ + void reset(); + + /** + * @brief Resets the base readings of the wall sensors + */ + void reset_base_readings(); + +private: + /** + * @briefGet the reading of the left wall sensor + * + * @return float + */ + float get_left_value() const; + + /** + * @brief Get the reading of the right wall sensor + * + * @return float + */ + float get_right_value() const; + + /** + * @brief Wall sensors of the robot + */ + const proxy::WallSensors<4>& wall_sensors; + + /** + * @brief PID controller for the wall following + */ + PidController pid; + + /** + * @brief Butterworth filter for the derivative of the error + */ + core::ButterworthFilter filter; + + /** + * @brief Last difference between the left and right wall sensors + */ + float last_diff{}; + + /** + * @brief Tolerance for the robot to follow the wall + */ + float can_follow_tolerance; + + /** + * @brief Base reading of the left wall sensor when aligned with the wall + */ + float base_left_reading; + + /** + * @brief Base reading of the right wall sensor when aligned with the wall + */ + float base_right_reading; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_FOLLOW_WALL diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index 922493f..9b522f7 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -9,6 +9,7 @@ #ifndef MICRAS_NAV_GO_TO_POINT_HPP #define MICRAS_NAV_GO_TO_POINT_HPP +#include "micras/nav/follow_wall.hpp" #include "micras/nav/pid_controller.hpp" #include "micras/nav/state.hpp" @@ -23,18 +24,25 @@ class GoToPoint { */ struct Config { PidController::Config linear_pid; + PidController::Config stop_pid; PidController::Config angular_pid; - float base_speed{}; - float linear_decay_damping{}; - float tolerance{}; + float cell_size; + float base_speed; + float linear_decay_damping; + float distance_tolerance; + float velocity_tolerance; }; /** * @brief Constructor for the GoToPoint class * + * @param wall_sensors The wall sensors of the robot * @param config The configuration for the GoToPoint class + * @param follow_wall_config The configuration for the FollowWall class */ - explicit GoToPoint(Config config); + explicit GoToPoint( + const proxy::WallSensors<4>& wall_sensors, const Config& config, const FollowWall::Config& follow_wall_config + ); /** * @brief Calculates the command to go to a point @@ -42,20 +50,32 @@ class GoToPoint { * @param state The current state of the robot * @param goal The goal point * @param elapsed_time The time elapsed since the last update + * @param stop Whether to stop at the goal * * @return The command to go to the point */ - Twist action(const State& state, const Point& goal, float elapsed_time); + Twist action(const State& state, const Point& goal, float elapsed_time, bool stop = true); /** * @brief Checks if the robot has reached the goal * * @param state The current state of the robot * @param goal The goal point + * @param stop Whether to stop at the goal * * @return True if the robot has reached the goal, false otherwise */ - bool finished(const State& state, const Point& goal) const; + bool finished(const State& state, const Point& goal, bool stop = true) const; + + /** + * @brief Resets the PID controllers + */ + void reset(); + + /** + * @brief Calibrates the wall sensors + */ + void calibrate(); private: /** @@ -63,11 +83,26 @@ class GoToPoint { */ PidController linear_pid; + /** + * @brief PID controller for stoppping at the goal + */ + PidController stop_pid; + /** * @brief PID controller for the orientation */ PidController angular_pid; + /** + * @brief FollowWall controller + */ + FollowWall follow_wall; + + /** + * @brief Size of the grid cells + */ + float cell_size; + /** * @brief Base linear speed for the robot */ @@ -81,7 +116,14 @@ class GoToPoint { /** * @brief Linear tolerance for the goal point */ - float tolerance; + float distance_tolerance; + + /** + * @brief Velocity tolerance at the goal point + */ + float velocity_tolerance; + + friend class Interface; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/look_at_point.hpp b/micras_nav/include/micras/nav/look_at_point.hpp index 20e98e3..deba404 100644 --- a/micras_nav/include/micras/nav/look_at_point.hpp +++ b/micras_nav/include/micras/nav/look_at_point.hpp @@ -24,7 +24,8 @@ class LookAtPoint { struct Config { PidController::Config linear_pid; PidController::Config angular_pid; - float tolerance{}; + float distance_tolerance; + float velocity_tolerance; }; /** @@ -45,6 +46,11 @@ class LookAtPoint { */ Twist action(const State& state, const Point& goal, float elapsed_time); + /** + * @brief Resets the PID controllers + */ + void reset(); + /** * @brief Checks if the robot has reached the goal * @@ -53,7 +59,7 @@ class LookAtPoint { * * @return True if the robot has reached the goal, false otherwise */ - bool finished(const Pose& pose, const Point& goal) const; + bool finished(const State& state, const Point& goal, bool stop = true) const; private: /** @@ -69,7 +75,12 @@ class LookAtPoint { /** * @brief Angular tolerance for the goal */ - float tolerance; + float distance_tolerance; + + /** + * @brief Velocity tolerance for the goal + */ + float velocity_tolerance; }; } // namespace micras::nav diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp new file mode 100644 index 0000000..f8a7f57 --- /dev/null +++ b/micras_nav/src/follow_wall.cpp @@ -0,0 +1,54 @@ +/** + * @file follow_wall.cpp + * + * @brief Nav FollowWall class implementation + * + * @date 10/2024 + */ + +#include "micras/nav/follow_wall.hpp" + +namespace micras::nav { +FollowWall::FollowWall(const proxy::WallSensors<4>& wall_sensors, const Config& config) : + wall_sensors{wall_sensors}, + pid{config.pid}, + filter{config.cutoff_frequency}, + can_follow_tolerance{config.can_follow_tolerance}, + base_left_reading{config.base_left_reading}, + base_right_reading{config.base_right_reading} { } + +float FollowWall::action(float elapsed_time) { + float error = this->get_left_value() - this->get_right_value(); + + if (this->last_diff == 0.0F) { + this->last_diff = error; + } + + float response = + this->pid.update(error, elapsed_time, this->filter.update((error - this->last_diff) / elapsed_time), true); + + this->last_diff = error; + return response; +} + +bool FollowWall::can_follow() { + return std::abs(this->get_left_value() + this->get_right_value()) < this->can_follow_tolerance; +} + +float FollowWall::get_left_value() const { + return this->wall_sensors.get_reading(1) - this->base_left_reading; +} + +float FollowWall::get_right_value() const { + return this->wall_sensors.get_reading(2) - this->base_right_reading; +} + +void FollowWall::reset() { + this->pid.reset(); +} + +void FollowWall::reset_base_readings() { + this->base_left_reading = this->wall_sensors.get_reading(1); + this->base_right_reading = this->wall_sensors.get_reading(2); +} +} // namespace micras::nav diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp index 83ac83d..b1813d5 100644 --- a/micras_nav/src/go_to_point.cpp +++ b/micras_nav/src/go_to_point.cpp @@ -12,24 +12,57 @@ #include "micras/nav/go_to_point.hpp" namespace micras::nav { -GoToPoint::GoToPoint(Config config) : +GoToPoint::GoToPoint( + const proxy::WallSensors<4>& wall_sensors, const Config& config, const FollowWall::Config& follow_wall_config +) : linear_pid(config.linear_pid), + stop_pid(config.stop_pid), angular_pid(config.angular_pid), + follow_wall{wall_sensors, follow_wall_config}, + cell_size{config.cell_size}, base_speed{config.base_speed}, linear_decay_damping{config.linear_decay_damping}, - tolerance{config.tolerance} { } + distance_tolerance{config.distance_tolerance}, + velocity_tolerance{config.velocity_tolerance} { } -Twist GoToPoint::action(const State& state, const Point& goal, float elapsed_time) { - float angular_error = core::assert_angle(state.pose.orientation - state.pose.position.angle_between(goal)); - float linear_target = core::decay(angular_error, this->linear_decay_damping) * this->base_speed; +Twist GoToPoint::action(const State& state, const Point& goal, float elapsed_time, bool stop) { + float linear_command; + float angular_command; + Point goal_distance = goal - state.pose.position; - float angular_command = this->angular_pid.update(angular_error, elapsed_time); - float linear_command = this->linear_pid.update(state.velocity.linear - linear_target, elapsed_time); + float linear_target{}; + + if (this->follow_wall.can_follow()) { + linear_target = this->base_speed; + angular_command = this->follow_wall.action(elapsed_time); + } else { + float angular_error = core::assert_angle(state.pose.orientation - std::atan2(this->cell_size, goal_distance.x)); + linear_target = core::decay(angular_error, this->linear_decay_damping) * this->base_speed; + angular_command = this->angular_pid.update(angular_error, elapsed_time, state.velocity.angular); + } + + if (stop and (std::abs(goal_distance.y) < this->cell_size / 2.0F)) { + linear_command = this->stop_pid.update(-goal_distance.y, elapsed_time, state.velocity.linear); + } else { + linear_command = this->linear_pid.update(state.velocity.linear - linear_target, elapsed_time); + } return {linear_command, angular_command}; } -bool GoToPoint::finished(const State& state, const Point& goal) const { - return std::hypot(goal.x - state.pose.position.x, goal.y - state.pose.position.y) <= this->tolerance; +bool GoToPoint::finished(const State& state, const Point& goal, bool stop) const { + return std::abs((goal - state.pose.position).y) <= this->distance_tolerance and + (not stop or std::abs(state.velocity.linear) <= this->velocity_tolerance); +} + +void GoToPoint::reset() { + this->follow_wall.reset(); + this->linear_pid.reset(); + this->stop_pid.reset(); + this->angular_pid.reset(); +} + +void GoToPoint::calibrate() { + this->follow_wall.reset_base_readings(); } } // namespace micras::nav diff --git a/micras_nav/src/look_at_point.cpp b/micras_nav/src/look_at_point.cpp index 0f6829f..027b735 100644 --- a/micras_nav/src/look_at_point.cpp +++ b/micras_nav/src/look_at_point.cpp @@ -11,17 +11,28 @@ namespace micras::nav { LookAtPoint::LookAtPoint(Config config) : - linear_pid(config.linear_pid), angular_pid{config.angular_pid}, tolerance{config.tolerance} { } + linear_pid(config.linear_pid), + angular_pid{config.angular_pid}, + distance_tolerance{config.distance_tolerance}, + velocity_tolerance{config.velocity_tolerance} { } Twist LookAtPoint::action(const State& state, const Point& goal, float elapsed_time) { - float angular_target = state.pose.position.angle_between(goal); + float angular_error = core::assert_angle(state.pose.orientation - state.pose.position.angle_between(goal)); + return { this->linear_pid.update(state.velocity.linear, elapsed_time), - this->angular_pid.update(core::assert_angle(state.pose.orientation - angular_target), elapsed_time) + this->angular_pid.update(angular_error, elapsed_time, state.velocity.angular) }; } -bool LookAtPoint::finished(const Pose& pose, const Point& goal) const { - return std::abs(core::assert_angle(pose.orientation - pose.position.angle_between(goal))) <= this->tolerance; +void LookAtPoint::reset() { + this->linear_pid.reset(); + this->angular_pid.reset(); +} + +bool LookAtPoint::finished(const State& state, const Point& goal, bool stop) const { + return std::abs(core::assert_angle(state.pose.orientation - state.pose.position.angle_between(goal))) <= + this->distance_tolerance and + (not stop or std::abs(state.velocity.angular) <= this->velocity_tolerance); } } // namespace micras::nav From 5c24590f7a7cf6466afe7b9b2877138f876d6c50 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 19:48:23 -0300 Subject: [PATCH 225/561] =?UTF-8?q?=F0=9F=9A=91=20Fix=20rotary=20sensors?= =?UTF-8?q?=20angle=20calculation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/rotary_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index c63c7ec..f6f9a7f 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -47,7 +47,7 @@ RotarySensor::RotarySensor(const Config& config) : } float RotarySensor::get_position() const { - return encoder.get_counter() * 2 * std::numbers::pi_v / this->resolution; + return encoder.get_counter() * 4.0F * std::numbers::pi_v / this->resolution; } uint16_t RotarySensor::read_register(uint16_t address) { From 965676ca12b33d8d7915de1e420890f4829f63cb Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 19:49:12 -0300 Subject: [PATCH 226/561] =?UTF-8?q?=E2=9C=A8=20Add=20reset=20to=20the=20od?= =?UTF-8?q?ometry=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/odometry.hpp | 14 ++++++++++++++ micras_nav/src/odometry.cpp | 10 ++++++++++ 2 files changed, 24 insertions(+) diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 338de65..7c82dca 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -50,6 +50,11 @@ class Odometry { */ void update(float elapsed_time); + /** + * @brief Reset the odometry + */ + void reset(); + /** * @brief Get the state of the robot * @@ -64,6 +69,13 @@ class Odometry { */ const State& get_imu_state() const; + /** + * @brief Set the state of the robot + * + * @param state New state of the robot + */ + void set_state(const State&); + private: /** * @brief Update a state based on the position variation @@ -144,6 +156,8 @@ class Odometry { * @brief Current state of the robot in space calculated using the IMU */ State imu_state; + + friend class Interface; }; } // namespace micras::nav diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 6cd8875..38754c0 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -50,6 +50,11 @@ void Odometry::update(float elapsed_time) { update_state(this->imu_state, linear_distance, angular_distance, linear_velocity, angular_velocity); } +void Odometry::reset() { + this->left_last_position = this->left_rotary_sensor.get_position(); + this->right_last_position = this->right_rotary_sensor.get_position(); +} + const nav::State& Odometry::get_state() const { return this->state; } @@ -57,4 +62,9 @@ const nav::State& Odometry::get_state() const { const nav::State& Odometry::get_imu_state() const { return this->imu_state; } + +void Odometry::set_state(const nav::State& state) { + this->state = state; + this->imu_state = state; +} } // namespace micras::nav From 2d63b40096abda89abc45569d7de070ddfda90b3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 19:51:34 -0300 Subject: [PATCH 227/561] =?UTF-8?q?=F0=9F=9A=91=20Fix=20angle=20to=20grid?= =?UTF-8?q?=20conversion?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/grid_pose.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index 5dad365..da68feb 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -12,7 +12,8 @@ namespace micras::nav { Side angle_to_grid(float angle) { - return static_cast(std::lround(2.0F * angle / std::numbers::pi_v) % 4); + int8_t grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); + return static_cast(grid_angle < 0 ? 4 + (grid_angle % 4) : grid_angle % 4); } Side GridPoint::direction(const GridPoint& next) const { From c2ac5dcb13d02998f34b5e076649f234d560df3c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 19:54:10 -0300 Subject: [PATCH 228/561] =?UTF-8?q?=F0=9F=9A=9A=20Rename=20distance=20sens?= =?UTF-8?q?or=20to=20wall=20sensors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/proxy/distance_sensors.hpp | 115 ----------- .../include/micras/proxy/wall_sensors.hpp | 178 ++++++++++++++++++ micras_proxy/src/distance_sensors.cpp | 57 ------ micras_proxy/src/wall_sensors.cpp | 120 ++++++++++++ 4 files changed, 298 insertions(+), 172 deletions(-) delete mode 100644 micras_proxy/include/micras/proxy/distance_sensors.hpp create mode 100644 micras_proxy/include/micras/proxy/wall_sensors.hpp delete mode 100644 micras_proxy/src/distance_sensors.cpp create mode 100644 micras_proxy/src/wall_sensors.cpp diff --git a/micras_proxy/include/micras/proxy/distance_sensors.hpp b/micras_proxy/include/micras/proxy/distance_sensors.hpp deleted file mode 100644 index fd0f2de..0000000 --- a/micras_proxy/include/micras/proxy/distance_sensors.hpp +++ /dev/null @@ -1,115 +0,0 @@ -/** - * @file distance_sensors.hpp - * - * @brief Proxy DistanceSensors class header - * - * @date 03/2024 - */ - -#ifndef MICRAS_PROXY_DISTANCE_SENSORS_HPP -#define MICRAS_PROXY_DISTANCE_SENSORS_HPP - -#include -#include - -#include "micras/core/butterworth_filter.hpp" -#include "micras/hal/adc_dma.hpp" -#include "micras/hal/pwm.hpp" - -namespace micras::proxy { -/** - * @brief Class for controlling DistanceSensors - */ -template -class DistanceSensors { -public: - /** - * @brief Configuration structure for distance sensors - */ - struct Config { - hal::AdcDma::Config adc; - hal::Pwm::Config led_0_pwm; - hal::Pwm::Config led_1_pwm; - float max_distance; - float filter_cutoff; - }; - - /** - * @brief Constructor for the DistanceSensors class - * - * @param config Configuration for the distance sensors - */ - explicit DistanceSensors(const Config& config); - - /** - * @brief Set the distance sensors led intensity - * - * @param intensity Intensity percentage of the infrared LED - */ - void set_led_intensity(float intensity); - - /** - * @brief Update the distance sensors readings - */ - void update(); - - /** - * @brief Get the distance from a sensor - * - * @param sensor_index Index of the sensor - * @return float Distance reading from the sensors - */ - float get_distance(uint8_t sensor_index) const; - - /** - * @brief Get the distance from a sensor - * - * @param sensor_index Index of the sensor - * @return float Raw reading from the distance sensor - */ - float get_distance_raw(uint8_t sensor_index) const; - - /** - * @brief Get the ADC reading from a sensor - * - * @param sensor_index Index of the sensor - * @return float ADC reading from the sensor from 0 to 1 - */ - float get_adc_reading(uint8_t sensor_index) const; - -private: - /** - * @brief ADC DMA handle - */ - hal::AdcDma adc; - - /** - * @brief PWM handle for the even infrared LEDs - */ - hal::Pwm led_0_pwm; - - /** - * @brief PWM handle for the odd infrared LEDs - */ - hal::Pwm led_1_pwm; - - /** - * @brief Buffer to store the ADC values - */ - std::array buffer; - - /** - * @brief Maximum distance reading in meters - */ - float max_distance; - - /** - * @brief Butterworth filter for the ADC readings - */ - std::array filters; -}; -} // namespace micras::proxy - -#include "../src/distance_sensors.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) - -#endif // MICRAS_PROXY_DISTANCE_SENSORS_HPP diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp new file mode 100644 index 0000000..109b8dc --- /dev/null +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -0,0 +1,178 @@ +/** + * @file wall_sensors.hpp + * + * @brief Proxy WallSensors class header + * + * @date 03/2024 + */ + +#ifndef MICRAS_PROXY_WALL_SENSORS_HPP +#define MICRAS_PROXY_WALL_SENSORS_HPP + +#include +#include + +#include "micras/core/types.hpp" +#include "micras/core/butterworth_filter.hpp" +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/pwm.hpp" + +namespace micras::proxy { +/** + * @brief Class for controlling WallSensors + */ +template +class WallSensors { +public: + /** + * @brief Configuration structure for wall sensors + */ + struct Config { + hal::AdcDma::Config adc; + hal::Pwm::Config led_0_pwm; + hal::Pwm::Config led_1_pwm; + float filter_cutoff; + float uncertainty; + std::array wall_threshold; + std::array free_threshold; + }; + + /** + * @brief Constructor for the WallSensors class + * + * @param config Configuration for the wall sensors + */ + explicit WallSensors(const Config& config); + + /** + * @brief Turn on the wall sensors IR LED + */ + void turn_on(); + + /** + * @brief Turn off the wall sensors IR LED + */ + void turn_off(); + + /** + * @brief Update the wall sensors readings + */ + void update(); + + /** + * @brief Get the observation from a sensor + * + * @param sensor_index Index of the sensor + * @return core::Observation Observation from the sensor + */ + core::Observation get_observation(uint8_t sensor_index) const; + + /** + * @brief Get the reading from a sensor + * + * @param sensor_index Index of the sensor + * @return float Reading from the sensor + */ + float get_reading(uint8_t sensor_index) const; + + /** + * @brief Get the ADC reading from a sensor + * + * @param sensor_index Index of the sensor + * @return float ADC reading from the sensor from 0 to 1 + */ + float get_adc_reading(uint8_t sensor_index) const; + + /** + * @brief Calibrate the wall sensors for a wall at the front + */ + void calibrate_front_wall(); + + /** + * @brief Calibrate the wall sensors for a wall at the left + */ + void calibrate_left_wall(); + + /** + * @brief Calibrate the wall sensors for a wall at the right + */ + void calibrate_right_wall(); + + /** + * @brief Calibrate the wall sensors for free space at the front + */ + void calibrate_front_free_space(); + + /** + * @brief Calibrate the wall sensors for free space at the left + */ + void calibrate_left_free_space(); + + /** + * @brief Calibrate the wall sensors for free space at the right + */ + void calibrate_right_free_space(); + + /** + * @brief Update the wall sensors thresholds + */ + void update_thresholds(); + +private: + /** + * @brief ADC DMA handle + */ + hal::AdcDma adc; + + /** + * @brief PWM handle for the even infrared LEDs + */ + hal::Pwm led_0_pwm; + + /** + * @brief PWM handle for the odd infrared LEDs + */ + hal::Pwm led_1_pwm; + + /** + * @brief Buffer to store the ADC values + */ + std::array buffer; + + /** + * @brief Butterworth filter for the ADC readings + */ + std::array filters; + + /** + * @brief Uncertainty of the wall sensors + */ + float uncertainty; + + /** + * @brief Measured wall value during calibration + */ + std::array wall_calibration_measure; + + /** + * @brief Measured free space value during calibration + */ + std::array free_space_calibration_measure; + + /** + * @brief Minimum reading value to identify a wall + */ + std::array wall_threshold; + + /** + * @brief Maximum reading value to identify a free space + */ + std::array free_space_threshold; + + friend class Interface; +}; +} // namespace micras::proxy + +#include "../src/wall_sensors.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) + +#endif // MICRAS_PROXY_WALL_SENSORS_HPP diff --git a/micras_proxy/src/distance_sensors.cpp b/micras_proxy/src/distance_sensors.cpp deleted file mode 100644 index 3c5e546..0000000 --- a/micras_proxy/src/distance_sensors.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/** - * @file distance_sensor.cpp - * - * @brief Proxy DistanceSensors class implementation - * - * @date 03/2024 - */ - -#ifndef MICRAS_PROXY_DISTANCE_SENSORS_CPP -#define MICRAS_PROXY_DISTANCE_SENSORS_CPP - -#include "micras/core/utils.hpp" -#include "micras/proxy/distance_sensors.hpp" - -namespace micras::proxy { -template -DistanceSensors::DistanceSensors(const Config& config) : - adc{config.adc}, - led_0_pwm{config.led_0_pwm}, - led_1_pwm{config.led_1_pwm}, - max_distance{config.max_distance}, - filters{core::make_array(config.filter_cutoff)} { - this->adc.start_dma(this->buffer); - this->led_0_pwm.set_duty_cycle(100.0F); - this->led_1_pwm.set_duty_cycle(100.0F); -} - -template -void DistanceSensors::set_led_intensity(float intensity) { - this->led_0_pwm.set_duty_cycle(intensity); - this->led_1_pwm.set_duty_cycle(intensity); -} - -template -void DistanceSensors::update() { - for (uint8_t i = 0; i < num_of_sensors; i++) { - this->filters[i].update(this->buffer[i]); - } -} - -template -float DistanceSensors::get_distance(uint8_t sensor_index) const { - return this->filters.at(sensor_index).get_last() * this->max_distance; -} - -template -float DistanceSensors::get_distance_raw(uint8_t sensor_index) const { - return this->get_adc_reading(sensor_index) * this->max_distance; -} - -template -float DistanceSensors::get_adc_reading(uint8_t sensor_index) const { - return static_cast(this->buffer.at(sensor_index)) / this->adc.get_max_reading(); -} -} // namespace micras::proxy - -#endif // MICRAS_PROXY_DISTANCE_SENSORS_CPP diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp new file mode 100644 index 0000000..690ee2b --- /dev/null +++ b/micras_proxy/src/wall_sensors.cpp @@ -0,0 +1,120 @@ +/** + * @file wall_sensor.cpp + * + * @brief Proxy WallSensors class implementation + * + * @date 03/2024 + */ + +#ifndef MICRAS_PROXY_WALL_SENSORS_CPP +#define MICRAS_PROXY_WALL_SENSORS_CPP + +#include "micras/core/utils.hpp" +#include "micras/proxy/wall_sensors.hpp" + +namespace micras::proxy { +template +WallSensors::WallSensors(const Config& config) : + adc{config.adc}, + led_0_pwm{config.led_0_pwm}, + led_1_pwm{config.led_1_pwm}, + filters{core::make_array(config.filter_cutoff)}, + uncertainty{config.uncertainty}, + wall_threshold{config.wall_threshold}, + free_space_threshold{config.free_threshold} { + this->adc.start_dma(this->buffer); + this->turn_off(); +} + +template +void WallSensors::turn_on() { + this->led_0_pwm.set_duty_cycle(100.0F); + this->led_1_pwm.set_duty_cycle(100.0F); +} + +template +void WallSensors::turn_off() { + this->led_0_pwm.set_duty_cycle(0.0F); + this->led_1_pwm.set_duty_cycle(0.0F); +} + +template +void WallSensors::update() { + for (uint8_t i = 0; i < num_of_sensors; i++) { + this->filters[i].update(this->get_adc_reading(i)); + } +} + +template +core::Observation WallSensors::get_observation(uint8_t sensor_index) const { + float reading = this->filters.at(sensor_index).get_last(); + + if (reading > this->wall_threshold.at(sensor_index)) { + return core::Observation::WALL; + } + + if (reading < this->free_space_threshold.at(sensor_index)) { + return core::Observation::FREE_SPACE; + } + + return core::Observation::UNKNOWN; +} + +template +float WallSensors::get_reading(uint8_t sensor_index) const { + return this->filters.at(sensor_index).get_last(); +} + +template +float WallSensors::get_adc_reading(uint8_t sensor_index) const { + return static_cast(this->buffer.at(sensor_index)) / this->adc.get_max_reading(); +} + +template +void WallSensors::calibrate_front_wall() { + this->wall_calibration_measure[0] = this->get_reading(0); + this->wall_calibration_measure[3] = this->get_reading(3); +} + +template +void WallSensors::calibrate_left_wall() { + this->wall_calibration_measure[1] = this->get_reading(1); +} + +template +void WallSensors::calibrate_right_wall() { + this->wall_calibration_measure[2] = this->get_reading(2); +} + +template +void WallSensors::calibrate_front_free_space() { + this->free_space_calibration_measure[0] = this->get_reading(0); + this->free_space_calibration_measure[3] = this->get_reading(3); +} + +template +void WallSensors::calibrate_left_free_space() { + this->free_space_calibration_measure[1] = this->get_reading(1); +} + +template +void WallSensors::calibrate_right_free_space() { + this->free_space_calibration_measure[2] = this->get_reading(2); +} + +template +void WallSensors::update_thresholds() { + for (uint8_t i = 0; i < num_of_sensors; i++) { + float mean_value = (this->wall_calibration_measure.at(i) + this->free_space_calibration_measure.at(i)) / 2.0F; + + float unknown_delta = this->uncertainty * + (this->wall_calibration_measure.at(i) - this->free_space_calibration_measure.at(i)) / + 2.0F; + + this->wall_threshold.at(i) = mean_value + unknown_delta; + this->free_space_threshold.at(i) = mean_value - unknown_delta; + } +} +} // namespace micras::proxy + +#endif // MICRAS_PROXY_WALL_SENSORS_CPP From 6eb58e6a78e1fe3aa17e6e70cc7cb3b28f77d1ac Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 19:54:52 -0300 Subject: [PATCH 229/561] =?UTF-8?q?=E2=9A=B0=EF=B8=8F=20Remove=20dead=20co?= =?UTF-8?q?de=20form=20locomotion=20acceleration=20ramp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/locomotion.hpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index 2249c0e..8c3f155 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -67,13 +67,6 @@ class Locomotion { */ void stop(); - /** - * @brief Update the locomotion state - * - * @param elapsed_time Time since the last update - */ - void update(float elapsed_time); - private: /** * @brief Left motor of the robot From 6efcaafcfd592a4164df4921391eb2e35b5bb6e6 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 20:56:03 -0300 Subject: [PATCH 230/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 1 + micras_core/include/micras/core/utils.hpp | 8 +-- micras_hal/stm32/src/timer.cpp | 4 +- micras_nav/include/micras/nav/follow_wall.hpp | 10 ++-- micras_nav/include/micras/nav/go_to_point.hpp | 10 ++-- .../include/micras/nav/look_at_point.hpp | 4 +- micras_nav/include/micras/nav/maze.hpp | 2 +- micras_nav/include/micras/nav/odometry.hpp | 54 ++----------------- micras_nav/src/go_to_point.cpp | 4 +- micras_nav/src/grid_pose.cpp | 2 +- micras_nav/src/maze.cpp | 2 +- micras_nav/src/odometry.cpp | 34 ++++++------ micras_nav/src/pid_controller.cpp | 3 ++ micras_proxy/include/micras/proxy/motor.hpp | 2 +- .../include/micras/proxy/wall_sensors.hpp | 2 +- 15 files changed, 49 insertions(+), 93 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index bf997e9..d684a6e 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -4,6 +4,7 @@ Checks: bugprone-*, -bugprone-easily-swappable-parameters, -bugprone-narrowing-conversions, + -bugprone-dynamic-static-initializers, clang-*, -clang-diagnostic-unused-command-line-argument, cppcoreguidelines-*, diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index 323380b..77fc87c 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -129,13 +129,13 @@ constexpr float assert_half_angle(float angle) { /** * @brief Check if two floating point numbers are near each other * - * @param a First number - * @param b Second number + * @param x First number + * @param y Second number * @param tolerance Tolerance * @return true If the numbers are near each other, false otherwise */ -constexpr bool is_near(float a, float b, float tolerance = 0.001) { - return std::abs(a - b) < tolerance; +constexpr bool is_near(float x, float y, float tolerance = 0.001) { + return std::abs(x - y) < tolerance; } } // namespace micras::core diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index 852097b..695b8df 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -46,9 +46,9 @@ uint32_t Timer::elapsed_time_us() const { if (counter < this->counter) { return std::numeric_limits::max() - this->counter + counter; - } else { - return counter - this->counter; } + + return counter - this->counter; } void Timer::sleep_ms(uint32_t time) { diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index b63fa23..60a0f49 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -24,10 +24,10 @@ class FollowWall { */ struct Config { PidController::Config pid; - float can_follow_tolerance; - float base_left_reading; - float base_right_reading; - float cutoff_frequency; + float can_follow_tolerance{}; + float base_left_reading{}; + float base_right_reading{}; + float cutoff_frequency{}; }; /** @@ -82,7 +82,7 @@ class FollowWall { /** * @brief Wall sensors of the robot */ - const proxy::WallSensors<4>& wall_sensors; + const proxy::WallSensors<4>& wall_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) /** * @brief PID controller for the wall following diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index 9b522f7..4ce0cd4 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -26,11 +26,11 @@ class GoToPoint { PidController::Config linear_pid; PidController::Config stop_pid; PidController::Config angular_pid; - float cell_size; - float base_speed; - float linear_decay_damping; - float distance_tolerance; - float velocity_tolerance; + float cell_size{}; + float base_speed{}; + float linear_decay_damping{}; + float distance_tolerance{}; + float velocity_tolerance{}; }; /** diff --git a/micras_nav/include/micras/nav/look_at_point.hpp b/micras_nav/include/micras/nav/look_at_point.hpp index deba404..875df29 100644 --- a/micras_nav/include/micras/nav/look_at_point.hpp +++ b/micras_nav/include/micras/nav/look_at_point.hpp @@ -24,8 +24,8 @@ class LookAtPoint { struct Config { PidController::Config linear_pid; PidController::Config angular_pid; - float distance_tolerance; - float velocity_tolerance; + float distance_tolerance{}; + float velocity_tolerance{}; }; /** diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index d0d2290..4d5d79a 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -32,7 +32,7 @@ class Maze { * @param start The start pose of the robot * @param goal The goal points in the maze */ - Maze(const GridPose& start, std::unordered_set goal); + Maze(const GridPose& start, const std::unordered_set& goal); /** * @brief Updates the maze walls with the current pose and new information diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 7c82dca..5bbc531 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -23,11 +23,12 @@ namespace micras::nav { */ class Odometry { public: + /** + * @brief Configuration for the odometry + */ struct Config { float linear_cutoff_frequency; - float angular_cutoff_frequency; float wheel_radius; - float wheel_separation; Pose initial_pose; }; @@ -62,46 +63,14 @@ class Odometry { */ const State& get_state() const; - /** - * @brief Get the state of the robot calculated using the IMU - * - * @return State current state of the robot in space - */ - const State& get_imu_state() const; - /** * @brief Set the state of the robot * * @param state New state of the robot */ - void set_state(const State&); + void set_state(const State& new_state); private: - /** - * @brief Update a state based on the position variation - * - * @param state State to be updated - * @param linear_distance Linear distance from last state - * @param angular_distance Angular distance from last state - * @param linear_velocity Measured linear velocity of the robot - * @param angular_velocity Measured angular velocity of the robot - */ - static constexpr void update_state( - State& state, float linear_distance, float angular_distance, float linear_velocity, float angular_velocity - ) { - float half_angle = angular_distance / 2; - float linear_diagonal = - angular_distance < 0.05F ? linear_distance : std::abs(std::sin(half_angle) * linear_distance / half_angle); - - state.pose.position.x += linear_diagonal * std::cos(state.pose.orientation + half_angle); - state.pose.position.y += linear_diagonal * std::sin(state.pose.orientation + half_angle); - - state.pose.orientation += angular_distance; - - state.velocity.linear = linear_velocity; - state.velocity.angular = angular_velocity; - } - /** * @brief Left rotary sensor */ @@ -122,11 +91,6 @@ class Odometry { */ float wheel_radius; - /** - * @brief Robot width - */ - float wheel_separation; - /** * @brief Last left rotary sensor position */ @@ -142,21 +106,11 @@ class Odometry { */ core::ButterworthFilter linear_filter; - /** - * @brief Angular velocity filter - */ - core::ButterworthFilter angular_filter; - /** * @brief Current state of the robot in space */ State state; - /** - * @brief Current state of the robot in space calculated using the IMU - */ - State imu_state; - friend class Interface; }; } // namespace micras::nav diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp index b1813d5..2b5c2f2 100644 --- a/micras_nav/src/go_to_point.cpp +++ b/micras_nav/src/go_to_point.cpp @@ -26,8 +26,8 @@ GoToPoint::GoToPoint( velocity_tolerance{config.velocity_tolerance} { } Twist GoToPoint::action(const State& state, const Point& goal, float elapsed_time, bool stop) { - float linear_command; - float angular_command; + float linear_command{}; + float angular_command{}; Point goal_distance = goal - state.pose.position; float linear_target{}; diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index da68feb..30205e9 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -12,7 +12,7 @@ namespace micras::nav { Side angle_to_grid(float angle) { - int8_t grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); + int32_t grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); return static_cast(grid_angle < 0 ? 4 + (grid_angle % 4) : grid_angle % 4); } diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 729af8f..6b58a9d 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -16,7 +16,7 @@ namespace micras::nav { template -Maze::Maze(const GridPose& start, std::unordered_set goal) : start{start}, goal{goal} { +Maze::Maze(const GridPose& start, const std::unordered_set& goal) : start{start}, goal{goal} { for (uint8_t row = 0; row < height; row++) { this->cells[row][0].wall_count[Side::LEFT] = 0xFFFF; this->cells[row][width - 1].wall_count[Side::RIGHT] = 0xFFFF; diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 38754c0..2e38849 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -21,13 +21,10 @@ Odometry::Odometry( right_rotary_sensor{right_rotary_sensor}, imu{imu}, wheel_radius{config.wheel_radius}, - wheel_separation{config.wheel_separation}, left_last_position{left_rotary_sensor.get_position()}, right_last_position{right_rotary_sensor.get_position()}, linear_filter{config.linear_cutoff_frequency}, - angular_filter{config.angular_cutoff_frequency}, - state{config.initial_pose, {0.0F, 0.0F}}, - imu_state{config.initial_pose, {0.0F, 0.0F}} { } + state{config.initial_pose, {0.0F, 0.0F}} { } void Odometry::update(float elapsed_time) { float left_position = this->left_rotary_sensor.get_position(); @@ -40,14 +37,20 @@ void Odometry::update(float elapsed_time) { this->right_last_position = right_position; float linear_distance = (left_distance + right_distance) / 2; - float angular_distance = (right_distance - left_distance) / this->wheel_separation; - float linear_velocity = this->linear_filter.update(linear_distance / elapsed_time); - float angular_velocity = this->angular_filter.update(angular_distance / elapsed_time); - update_state(this->state, linear_distance, angular_distance, linear_velocity, angular_velocity); - angular_velocity = this->imu.get_angular_velocity(proxy::Imu::Axis::Z); - angular_distance = angular_velocity * elapsed_time; - update_state(this->imu_state, linear_distance, angular_distance, linear_velocity, angular_velocity); + this->state.velocity.linear = this->linear_filter.update(linear_distance / elapsed_time); + this->state.velocity.angular = this->imu.get_angular_velocity(proxy::Imu::Axis::Z); + + float angular_distance = this->state.velocity.angular * elapsed_time; + + float half_angle = angular_distance / 2; + float linear_diagonal = + angular_distance < 0.05F ? linear_distance : std::abs(std::sin(half_angle) * linear_distance / half_angle); + + this->state.pose.position.x += linear_diagonal * std::cos(this->state.pose.orientation + half_angle); + this->state.pose.position.y += linear_diagonal * std::sin(this->state.pose.orientation + half_angle); + + this->state.pose.orientation += angular_distance; } void Odometry::reset() { @@ -59,12 +62,7 @@ const nav::State& Odometry::get_state() const { return this->state; } -const nav::State& Odometry::get_imu_state() const { - return this->imu_state; -} - -void Odometry::set_state(const nav::State& state) { - this->state = state; - this->imu_state = state; +void Odometry::set_state(const nav::State& new_state) { + this->state = new_state; } } // namespace micras::nav diff --git a/micras_nav/src/pid_controller.cpp b/micras_nav/src/pid_controller.cpp index c23ddbf..c67d26c 100644 --- a/micras_nav/src/pid_controller.cpp +++ b/micras_nav/src/pid_controller.cpp @@ -11,12 +11,15 @@ #include "micras/nav/pid_controller.hpp" +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) static volatile float test_error; static volatile float test_proportional; static volatile float test_integrative; static volatile float test_derivative; static volatile float test_response; +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) + namespace micras::nav { PidController::PidController(Config config) : kp{config.kp}, diff --git a/micras_proxy/include/micras/proxy/motor.hpp b/micras_proxy/include/micras/proxy/motor.hpp index 324a6c4..ac29361 100644 --- a/micras_proxy/include/micras/proxy/motor.hpp +++ b/micras_proxy/include/micras/proxy/motor.hpp @@ -32,7 +32,7 @@ class Motor { * * @param config Configuration for the motor driver */ - Motor(const Config& config); + explicit Motor(const Config& config); /** * @brief Set the command for the motor diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 109b8dc..69d3660 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -12,8 +12,8 @@ #include #include -#include "micras/core/types.hpp" #include "micras/core/butterworth_filter.hpp" +#include "micras/core/types.hpp" #include "micras/hal/adc_dma.hpp" #include "micras/hal/pwm.hpp" From 1cfc186dbcfb5deff62870d658ad538eb568b1cd Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 7 Oct 2024 21:02:44 -0300 Subject: [PATCH 231/561] =?UTF-8?q?=E2=9C=A8=20Add=20front=20and=20side=20?= =?UTF-8?q?alignment=20to=20the=20mapping=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/mapping.hpp | 115 ++++++++++++------- micras_nav/src/mapping.cpp | 128 +++++++++++++++++----- 2 files changed, 177 insertions(+), 66 deletions(-) diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index 94641e8..4b52bd5 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -13,7 +13,7 @@ #include "micras/nav/maze.hpp" #include "micras/nav/state.hpp" -#include "micras/proxy/distance_sensors.hpp" +#include "micras/proxy/wall_sensors.hpp" namespace micras::nav { /** @@ -29,15 +29,17 @@ class Mapping { * @brief Configuration for the Mapping class */ struct Config { - float wall_thickness; - float cell_size; - float wall_distance_threshold; - float free_distance_threshold; - float alignment_threshold; - Pose front_sensor_pose; - Pose side_sensor_pose; - - GridPose start; + float wall_thickness{}; + float cell_size{}; + float alignment_threshold{}; + Pose front_sensor_pose{}; + Pose side_sensor_pose{}; + float front_alignment_tolerance{}; + float side_alignment_tolerance{}; + std::array front_alignment_measure{}; + std::array side_alignment_measure{}; + + GridPose start{}; std::unordered_set goal{ {{width / 2, height / 2}, {(width - 1) / 2, height / 2}, @@ -47,7 +49,7 @@ class Mapping { }; /** - * @brief Type to identify the distance sensors + * @brief Type to identify the wall sensors */ enum Sensor : uint8_t { FRONT_LEFT, @@ -77,69 +79,88 @@ class Mapping { * @brief The next goal point */ Point point; + + /** + * @brief The orientation of the goal point + */ + Side side; }; /** * @brief Constructor for the Mapping class * - * @param distance_sensors The distance sensors + * @param wall_sensors The wall sensors * @param config The configuration for the mapping */ - Mapping(const proxy::DistanceSensors<4>& distance_sensors, Config config); + Mapping(const proxy::WallSensors<4>& wall_sensors, Config config); /** - * @brief Update the mapping of the maze using the current pose and distance sensors + * @brief Update the mapping of the maze using the current pose and wall sensors * * @param pose The current pose of the robot */ void update(const Pose& pose); /** - * @brief Get the action to take based on the current pose + * @brief Get the action to be taken based on the current pose * * @param pose The current pose of the robot - * @return The action to take + * @return The action to be taken */ Action get_action(const Pose& pose) const; -private: /** - * @brief Get the wall information for a sensor + * @brief Fix the pose based on the mapping information * - * @param sensor The sensor to get the wall information - * @return The wall information for the sensor + * @param pose The current pose of the robot + * @return The fixed pose */ - Information::Existence get_wall_information(Sensor sensor) const; + Pose correct_pose(const Pose& pose) const; /** - * @brief Distance sensors of the robot + * @brief Calibrate front alignment of the wall sensors */ - const proxy::DistanceSensors<4>& distance_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + void calibrate_front(); /** - * @brief The maze information the robot has + * @brief Calibrate side alignment of the wall sensors */ - Maze maze; + void calibrate_side(); /** - * @brief Thickness of the maze walls + * @brief Check if the robot is aligned with a wall at the front + * + * @return True if the robot is front aligned, false otherwise */ - float wall_thickness; + bool is_front_aligned() const; /** - * @brief Size of the cells in the maze + * @brief Check if the robot is aligned with a wall at the sides + * + * @return True if the robot is side aligned, false otherwise */ - float cell_size; + bool is_side_aligned() const; + +private: + /** + * @brief Wall sensors of the robot + */ + const proxy::WallSensors<4>& wall_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + + /** + * @brief The maze information the robot has + */ + Maze maze; /** - * @brief Threshold for the distance to consider a wall + * @brief Thickness of the maze walls */ - float wall_distance_threshold; + float wall_thickness; /** - * @brief Threshold for the distance to consider free space + * @brief Size of the cells in the maze */ - float free_distance_threshold; + float cell_size; /** * @brief Threshold for the robot alignment with the side walls @@ -147,24 +168,44 @@ class Mapping { float alignment_threshold; /** - * @brief Pose of the front distance sensors + * @brief Pose of the front wall sensors */ Pose front_sensor_pose; /** - * @brief Pose of the side distance sensors + * @brief Pose of the side wall sensors */ Pose side_sensor_pose; + /** + * @brief Region of the cell to consider the information of the front sensors to the current cell + */ + float front_sensors_region_division; + /** * @brief Region of the cell to consider the information of the side sensors to the current cell */ float side_sensors_region_division; /** - * @brief Region of the cell to consider the information of the front sensors to the current cell + * @brief Sensor readings tolerance for the front alignment of the robot */ - float front_sensors_region_division; + float front_alignment_tolerance; + + /** + * @brief Sensor readings tolerance for the side alignment of the robot + */ + float side_alignment_tolerance; + + /** + * @brief Sensor readings for the front alignment of the robot + */ + std::array front_alignment_measure{}; + + /** + * @brief Sensor readings for the side alignment of the robot + */ + std::array side_alignment_measure{}; }; } // namespace micras::nav diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index b79eeef..e1e41bc 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -15,21 +15,23 @@ namespace micras::nav { template -Mapping::Mapping(const proxy::DistanceSensors<4>& distance_sensors, Mapping::Config config) : - distance_sensors{distance_sensors}, +Mapping::Mapping(const proxy::WallSensors<4>& wall_sensors, Mapping::Config config) : + wall_sensors{wall_sensors}, maze{config.start, config.goal}, wall_thickness{config.wall_thickness}, cell_size{config.cell_size}, - wall_distance_threshold{config.wall_distance_threshold}, - free_distance_threshold{config.free_distance_threshold}, alignment_threshold{config.alignment_threshold}, front_sensor_pose{config.front_sensor_pose}, side_sensor_pose{config.side_sensor_pose}, + front_sensors_region_division{cell_size - wall_thickness / 2.0F - front_sensor_pose.position.y}, side_sensors_region_division{ cell_size - wall_thickness / 2.0F - side_sensor_pose.position.y + (side_sensor_pose.position.x + (wall_thickness - cell_size) / 2.0F) * std::tan(side_sensor_pose.orientation) }, - front_sensors_region_division{cell_size - wall_thickness / 2.0F - front_sensor_pose.position.y} { } + front_alignment_tolerance{config.front_alignment_tolerance}, + side_alignment_tolerance{config.side_alignment_tolerance}, + front_alignment_measure{config.front_alignment_measure}, + side_alignment_measure{config.side_alignment_measure} { } template void Mapping::update(const Pose& pose) { @@ -67,23 +69,23 @@ void Mapping::update(const Pose& pose) { Information information{}; if (cell_advance < this->front_sensors_region_division) { - if (this->get_wall_information(Sensor::FRONT_LEFT) == Information::WALL and - this->get_wall_information(Sensor::FRONT_RIGHT) == Information::WALL) { - information.front = Information::WALL; - } else if (this->get_wall_information(Sensor::FRONT_LEFT) == Information::FREE and - this->get_wall_information(Sensor::FRONT_RIGHT) == Information::FREE) { - information.front = Information::FREE; + if (this->wall_sensors.get_observation(Sensor::FRONT_LEFT) == core::Observation::WALL and + this->wall_sensors.get_observation(Sensor::FRONT_RIGHT) == core::Observation::WALL) { + information.front = core::Observation::WALL; + } else if (this->wall_sensors.get_observation(Sensor::FRONT_LEFT) == core::Observation::FREE_SPACE and + this->wall_sensors.get_observation(Sensor::FRONT_RIGHT) == core::Observation::FREE_SPACE) { + information.front = core::Observation::FREE_SPACE; } } if (cell_alignment < this->alignment_threshold * this->cell_size) { if (cell_advance < this->side_sensors_region_division) { - information.left = this->get_wall_information(Sensor::LEFT); - information.right = this->get_wall_information(Sensor::RIGHT); - } else if (information.front == Information::FREE and + information.left = this->wall_sensors.get_observation(Sensor::LEFT); + information.right = this->wall_sensors.get_observation(Sensor::RIGHT); + } else if (information.front != core::Observation::WALL and cell_advance > this->side_sensors_region_division + this->wall_thickness) { - information.front_left = this->get_wall_information(Sensor::LEFT); - information.front_right = this->get_wall_information(Sensor::RIGHT); + information.front_left = this->wall_sensors.get_observation(Sensor::LEFT); + information.front_right = this->wall_sensors.get_observation(Sensor::RIGHT); } } @@ -92,29 +94,97 @@ void Mapping::update(const Pose& pose) { template Mapping::Action Mapping::get_action(const Pose& pose) const { - Point current_goal = - Point::from_grid(this->maze.get_current_goal(pose.position.to_grid(this->cell_size)), this->cell_size); + GridPose current_grid_goal = this->maze.get_current_goal(pose.position.to_grid(this->cell_size)); + Point current_goal = Point::from_grid(current_grid_goal.position, this->cell_size); + Point current_goal_rotated = current_goal.rotate(current_grid_goal.orientation); - float angle_error = core::assert_angle(pose.orientation - pose.position.angle_between(current_goal)); - - if (std::abs(angle_error) > std::numbers::pi_v / 4.0F) { - return {Action::Type::LOOK_AT, current_goal}; + if (std::abs(core::assert_angle(pose.orientation - pose.position.angle_between(current_goal))) > + std::numbers::pi_v / 8.0F) { + return {Action::Type::LOOK_AT, current_goal_rotated, current_grid_goal.orientation}; } - return {Action::Type::GO_TO, current_goal}; + return {Action::Type::GO_TO, current_goal_rotated, current_grid_goal.orientation}; } template -Information::Existence Mapping::get_wall_information(Sensor sensor) const { - if (this->distance_sensors.get_distance(sensor) < this->wall_distance_threshold) { - return Information::WALL; +Pose Mapping::correct_pose(const Pose& pose) const { + float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); + + if (reliability < 50.0F) { + return pose; } - if (this->distance_sensors.get_distance(sensor) < this->free_distance_threshold) { - return Information::FREE; + Pose corrected_pose = pose; + Side side = angle_to_grid(pose.orientation); + + switch (side) { + case Side::RIGHT: + if (this->is_front_aligned()) { + corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; + } else if (this->is_side_aligned()) { + corrected_pose.orientation = 0.0F; + corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; + } + break; + case Side::UP: + if (this->is_front_aligned()) { + corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; + } else if (this->is_side_aligned()) { + corrected_pose.orientation = std::numbers::pi_v / 2.0F; + corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; + } + break; + case Side::LEFT: + if (this->is_front_aligned()) { + corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; + } else if (this->is_side_aligned()) { + corrected_pose.orientation = std::numbers::pi_v; + corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; + } + break; + case Side::DOWN: + if (this->is_front_aligned()) { + corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; + } else if (this->is_side_aligned()) { + corrected_pose.orientation = -std::numbers::pi_v / 2.0F; + corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; + } + break; } - return Information::UNKNOWN; + return corrected_pose; +} + +template +void Mapping::calibrate_front() { + this->front_alignment_measure[0] = this->wall_sensors.get_reading(0); + this->front_alignment_measure[1] = this->wall_sensors.get_reading(3); +} + +template +void Mapping::calibrate_side() { + this->side_alignment_measure[0] = this->wall_sensors.get_reading(1); + this->side_alignment_measure[1] = this->wall_sensors.get_reading(2); +} + +template +bool Mapping::is_front_aligned() const { + return core::is_near( + this->wall_sensors.get_reading(0), this->front_alignment_measure[0], this->front_alignment_tolerance + ) and + core::is_near( + this->wall_sensors.get_reading(3), this->front_alignment_measure[1], this->front_alignment_tolerance + ); +} + +template +bool Mapping::is_side_aligned() const { + return core::is_near( + this->wall_sensors.get_reading(1), this->side_alignment_measure[0], this->side_alignment_tolerance + ) and + core::is_near( + this->wall_sensors.get_reading(2), this->side_alignment_measure[1], this->side_alignment_tolerance + ); } } // namespace micras::nav From cf63bcb6465a2075be1de5b47e20855ab568d8a0 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 9 Oct 2024 05:24:21 -0300 Subject: [PATCH 232/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20can=5Ffollow=5Fwa?= =?UTF-8?q?ll=20to=20maze?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/utils.hpp | 2 +- micras_nav/include/micras/nav/follow_wall.hpp | 7 ------- micras_nav/include/micras/nav/go_to_point.hpp | 2 +- micras_nav/include/micras/nav/mapping.hpp | 12 ++++++++++- micras_nav/include/micras/nav/maze.hpp | 9 +++++++++ micras_nav/src/follow_wall.cpp | 4 ---- micras_nav/src/go_to_point.cpp | 4 ++-- micras_nav/src/mapping.cpp | 3 ++- micras_nav/src/maze.cpp | 20 +++++++++++++++++++ 9 files changed, 46 insertions(+), 17 deletions(-) diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index 77fc87c..1219ee2 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -135,7 +135,7 @@ constexpr float assert_half_angle(float angle) { * @return true If the numbers are near each other, false otherwise */ constexpr bool is_near(float x, float y, float tolerance = 0.001) { - return std::abs(x - y) < tolerance; + return std::abs(x - y) <= tolerance; } } // namespace micras::core diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 60a0f49..83d9e2d 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -47,13 +47,6 @@ class FollowWall { */ float action(float elapsed_time); - /** - * @brief Checks if the robot can follow the wall - * - * @return True if the robot can follow the wall, false otherwise - */ - bool can_follow(); - /** * @brief Resets the PID controller */ diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index 4ce0cd4..efb5edb 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -54,7 +54,7 @@ class GoToPoint { * * @return The command to go to the point */ - Twist action(const State& state, const Point& goal, float elapsed_time, bool stop = true); + Twist action(const State& state, const Point& goal, bool can_follow_wall, float elapsed_time, bool stop = true); /** * @brief Checks if the robot has reached the goal diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index 4b52bd5..63bb63f 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -36,6 +36,7 @@ class Mapping { Pose side_sensor_pose{}; float front_alignment_tolerance{}; float side_alignment_tolerance{}; + float can_follow_wall_tolerance{}; std::array front_alignment_measure{}; std::array side_alignment_measure{}; @@ -115,7 +116,7 @@ class Mapping { * @param pose The current pose of the robot * @return The fixed pose */ - Pose correct_pose(const Pose& pose) const; + Pose correct_pose(const Pose& pose, bool can_follow_wall) const; /** * @brief Calibrate front alignment of the wall sensors @@ -141,6 +142,13 @@ class Mapping { */ bool is_side_aligned() const; + /** + * @brief Check if the robot can follow a wall + * + * @return True if the robot can follow a wall, false otherwise + */ + bool can_follow_wall(const Pose& pose) const; + private: /** * @brief Wall sensors of the robot @@ -197,6 +205,8 @@ class Mapping { */ float side_alignment_tolerance; + float can_follow_wall_tolerance; + /** * @brief Sensor readings for the front alignment of the robot */ diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 4d5d79a..f6ef338 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -51,6 +51,15 @@ class Maze { */ GridPose get_current_goal(const GridPoint& position, bool force_costmap = false) const; + /** + * @brief Checks whether the robot can follow a wall + * + * @param pose The pose of the robot + * @param front_cell Whether to check the cell at the front of the robot + * @return True if the robot can follow the wall, false otherwise + */ + bool can_follow_wall(const GridPose& pose, bool front_cell) const; + private: /** * @brief Type to store the information of a cell in the maze diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index f8a7f57..a7c25af 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -31,10 +31,6 @@ float FollowWall::action(float elapsed_time) { return response; } -bool FollowWall::can_follow() { - return std::abs(this->get_left_value() + this->get_right_value()) < this->can_follow_tolerance; -} - float FollowWall::get_left_value() const { return this->wall_sensors.get_reading(1) - this->base_left_reading; } diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp index 2b5c2f2..1e6d88a 100644 --- a/micras_nav/src/go_to_point.cpp +++ b/micras_nav/src/go_to_point.cpp @@ -25,14 +25,14 @@ GoToPoint::GoToPoint( distance_tolerance{config.distance_tolerance}, velocity_tolerance{config.velocity_tolerance} { } -Twist GoToPoint::action(const State& state, const Point& goal, float elapsed_time, bool stop) { +Twist GoToPoint::action(const State& state, const Point& goal, bool can_follow_wall, float elapsed_time, bool stop) { float linear_command{}; float angular_command{}; Point goal_distance = goal - state.pose.position; float linear_target{}; - if (this->follow_wall.can_follow()) { + if (can_follow_wall) { linear_target = this->base_speed; angular_command = this->follow_wall.action(elapsed_time); } else { diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index e1e41bc..76ceb6f 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -30,6 +30,7 @@ Mapping::Mapping(const proxy::WallSensors<4>& wall_sensors, Mappi }, front_alignment_tolerance{config.front_alignment_tolerance}, side_alignment_tolerance{config.side_alignment_tolerance}, + can_follow_wall_tolerance{config.can_follow_wall_tolerance}, front_alignment_measure{config.front_alignment_measure}, side_alignment_measure{config.side_alignment_measure} { } @@ -107,7 +108,7 @@ Mapping::Action Mapping::get_action(const Pose& po } template -Pose Mapping::correct_pose(const Pose& pose) const { +Pose Mapping::correct_pose(const Pose& pose, bool can_follow_wall) const { float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); if (reliability < 50.0F) { diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 6b58a9d..ca7610f 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -143,6 +143,26 @@ bool Maze::has_wall(const GridPose& pose) const { this->get_cell(pose.position).free_count[pose.orientation]; } +template +bool Maze::can_follow_wall(const GridPose& pose, bool front_cell) const { + if (pose.position.x >= width or pose.position.y >= height) { + return false; + } + + if (not front_cell) { + return this->has_wall(pose.turned_left()) and this->has_wall(pose.turned_right()); + } + + GridPose front_pose = pose.front(); + + if (front_pose.position.x >= width or front_pose.position.y >= height) { + return false; + } + + return not this->has_wall(pose) and this->has_wall(front_pose.turned_left()) and + this->has_wall(front_pose.turned_right()); +} + template void Maze::calculate_costmap() { std::array, height> visited{}; From ceeb2c0c9e931d2f046217fe64efe971674fccb2 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 9 Oct 2024 05:24:58 -0300 Subject: [PATCH 233/561] =?UTF-8?q?=E2=9C=A8=20Add=20orientation=20alignme?= =?UTF-8?q?nt=20to=20mapping?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/mapping.hpp | 8 +++ micras_nav/src/mapping.cpp | 70 +++++++++++++++++++---- 2 files changed, 66 insertions(+), 12 deletions(-) diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index 63bb63f..d314ee1 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -36,6 +36,7 @@ class Mapping { Pose side_sensor_pose{}; float front_alignment_tolerance{}; float side_alignment_tolerance{}; + float orientation_alignment_tolerance{}; float can_follow_wall_tolerance{}; std::array front_alignment_measure{}; std::array side_alignment_measure{}; @@ -142,6 +143,8 @@ class Mapping { */ bool is_side_aligned() const; + bool is_orientation_aligned() const; + /** * @brief Check if the robot can follow a wall * @@ -205,6 +208,11 @@ class Mapping { */ float side_alignment_tolerance; + /** + * @brief Sensor readings for the front alignment of the robot + */ + float orientation_alignment_tolerance; + float can_follow_wall_tolerance; /** diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 76ceb6f..0508f07 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -122,33 +122,49 @@ Pose Mapping::correct_pose(const Pose& pose, bool can_follow_wall case Side::RIGHT: if (this->is_front_aligned()) { corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; - } else if (this->is_side_aligned()) { - corrected_pose.orientation = 0.0F; - corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; + } else if (can_follow_wall) { + if (this->is_side_aligned()) { + corrected_pose.orientation = 0.0F; + corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; + } else if (this->is_orientation_aligned()) { + corrected_pose.orientation = 0.0F; + } } break; case Side::UP: if (this->is_front_aligned()) { corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; - } else if (this->is_side_aligned()) { - corrected_pose.orientation = std::numbers::pi_v / 2.0F; - corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; + } else if (can_follow_wall) { + if (this->is_side_aligned()) { + corrected_pose.orientation = std::numbers::pi_v / 2.0F; + corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; + } else if (this->is_orientation_aligned()) { + corrected_pose.orientation = std::numbers::pi_v / 2.0F; + } } break; case Side::LEFT: if (this->is_front_aligned()) { corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; - } else if (this->is_side_aligned()) { - corrected_pose.orientation = std::numbers::pi_v; - corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; + } else if (can_follow_wall) { + if (this->is_side_aligned()) { + corrected_pose.orientation = std::numbers::pi_v; + corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; + } else if (this->is_orientation_aligned()) { + corrected_pose.orientation = std::numbers::pi_v; + } } break; case Side::DOWN: if (this->is_front_aligned()) { corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; - } else if (this->is_side_aligned()) { - corrected_pose.orientation = -std::numbers::pi_v / 2.0F; - corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; + } else if (can_follow_wall) { + if (this->is_side_aligned()) { + corrected_pose.orientation = -std::numbers::pi_v / 2.0F; + corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; + } else if (this->is_orientation_aligned()) { + corrected_pose.orientation = -std::numbers::pi_v / 2.0F; + } } break; } @@ -187,6 +203,36 @@ bool Mapping::is_side_aligned() const { this->wall_sensors.get_reading(2), this->side_alignment_measure[1], this->side_alignment_tolerance ); } + +template +bool Mapping::is_orientation_aligned() const { + return std::abs( + this->wall_sensors.get_reading(1) - this->side_alignment_measure[0] + this->wall_sensors.get_reading(2) - + this->side_alignment_measure[1] + ) <= this->orientation_alignment_tolerance; +} + +template +bool Mapping::can_follow_wall(const Pose& pose) const { + nav::Point cell_position = pose.to_cell(cell_size); + + if (std::abs( + this->wall_sensors.get_reading(1) - this->side_alignment_measure[0] + this->wall_sensors.get_reading(2) - + this->side_alignment_measure[1] + ) > this->can_follow_wall_tolerance) { + return false; + } + + if (cell_position.y < this->side_sensors_region_division - 2 * this->wall_thickness) { + return this->maze.can_follow_wall(pose.to_grid(this->cell_size), false); + } + + if (cell_position.y > this->side_sensors_region_division) { + return this->maze.can_follow_wall(pose.to_grid(this->cell_size), true); + } + + return false; +} } // namespace micras::nav #endif // MICRAS_NAV_MAPPING_CPP From 260b2d170ad60bf0bd9dc44d3fb7d0c4e7478243 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 9 Oct 2024 05:25:53 -0300 Subject: [PATCH 234/561] =?UTF-8?q?=E2=9C=A8=20Add=20route=20optimization?= =?UTF-8?q?=20to=20maze?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 5 +++++ micras_nav/src/maze.cpp | 27 +++++++++++++++++++++++--- 2 files changed, 29 insertions(+), 3 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index f6ef338..94b38b7 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -75,6 +75,11 @@ class Maze { */ void calculate_costmap(); + /** + * @brief Improves the route to the goal + */ + void optimize_route(); + /** * @brief Returns the cell at the given position * diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index ca7610f..4c18297 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -45,6 +45,7 @@ void Maze::update(const GridPose& pose, Information information) } else if (pose == start.turned_back()) { this->exploring = false; this->returning = false; + this->optimize_route(); } if (not this->exploring) { @@ -80,9 +81,13 @@ GridPose Maze::get_current_goal(const GridPoint& position, bool f if (not force_costmap and (not this->exploring or this->returning) and this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { - return (this->returning ? std::prev(this->best_route.find(current_cost)) : - std::next(this->best_route.find(current_cost))) - ->second; + auto current = this->best_route.find(current_cost); + + if (this->returning) { + return std::prev(current)->second.turned_back(); + } + + return {std::next(current)->second.position, current->second.orientation}; } GridPoint next_position = position; @@ -212,6 +217,22 @@ void Maze::calculate_costmap() { this->best_route.try_emplace(this->get_cell(current_pose.position).cost, current_pose); } } + +template +void Maze::optimize_route() { + for (auto it = this->best_route.begin(); it != this->best_route.end();) { + auto next = std::next(it); + + if (next != this->best_route.end()) { + if (it->second.orientation == next->second.orientation) { + this->best_route.erase(next); + continue; + } + } + + it++; + } +} } // namespace micras::nav #endif // MICRAS_NAV_MAZE_CPP From ce366f7e9cc0b0970c84abce6dc49fb013a2b8f1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 9 Oct 2024 05:27:04 -0300 Subject: [PATCH 235/561] =?UTF-8?q?=F0=9F=94=A7=20Change=20imu=20configura?= =?UTF-8?q?tion?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/imu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index aa9f8bf..673922c 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -19,7 +19,7 @@ Imu::Imu(const Config& config) : mdps_to_radps * 4.375F * (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale))) }, - xl_factor{mg_to_mps2 * (0.61F * (1 << static_cast(config.accelerometer_scale)))} { + xl_factor{mg_to_mps2 * (0.061F * (1 << static_cast(config.accelerometer_scale)))} { this->dev_ctx.read_reg = platform_read; this->dev_ctx.write_reg = platform_write; this->dev_ctx.handle = &this->spi; @@ -46,7 +46,7 @@ Imu::Imu(const Config& config) : lsm6dsv_gy_full_scale_set(&(this->dev_ctx), config.gyroscope_scale); lsm6dsv_xl_full_scale_set(&(this->dev_ctx), config.accelerometer_scale); - lsm6dsv_filt_settling_mask_set(&dev_ctx, {.drdy = 1, .ois_drdy = 0, .irq_xl = 1, .irq_g = 1}); + lsm6dsv_filt_settling_mask_set(&dev_ctx, {.drdy = 1, .ois_drdy = 0, .irq_xl = 0, .irq_g = 0}); lsm6dsv_filt_gy_lp1_set(&(this->dev_ctx), PROPERTY_ENABLE); lsm6dsv_filt_gy_lp1_bandwidth_set(&(this->dev_ctx), config.gyroscope_filter); From 9af5460db3277130d86a66e82e5282056161abdf Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 9 Oct 2024 05:33:09 -0300 Subject: [PATCH 236/561] =?UTF-8?q?=E2=9C=A8=20Add=20to=5Fcell=20function?= =?UTF-8?q?=20to=20Pose=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/state.hpp | 8 +++++ micras_nav/src/mapping.cpp | 47 +++++-------------------- micras_nav/src/state.cpp | 28 +++++++++++++++ 3 files changed, 45 insertions(+), 38 deletions(-) diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index 0b77a83..8fbde92 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -104,6 +104,14 @@ struct Pose { */ GridPose to_grid(float cell_size) const; + /** + * @brief Converts the pose to a cell reference + * + * @param cell_size The size of the grid cells + * @return The point inside the cell reference + */ + Point to_cell(float cell_size) const; + /** * @brief The position of the pose */ diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 0508f07..9748e3f 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -25,7 +25,7 @@ Mapping::Mapping(const proxy::WallSensors<4>& wall_sensors, Mappi side_sensor_pose{config.side_sensor_pose}, front_sensors_region_division{cell_size - wall_thickness / 2.0F - front_sensor_pose.position.y}, side_sensors_region_division{ - cell_size - wall_thickness / 2.0F - side_sensor_pose.position.y + + cell_size + wall_thickness - side_sensor_pose.position.y + (side_sensor_pose.position.x + (wall_thickness - cell_size) / 2.0F) * std::tan(side_sensor_pose.orientation) }, front_alignment_tolerance{config.front_alignment_tolerance}, @@ -38,38 +38,14 @@ template void Mapping::update(const Pose& pose) { float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); - if (reliability < 50.0F) { + if (reliability < 60.0F) { return; } - Point cell_position = pose.position % this->cell_size; - Side side = angle_to_grid(pose.orientation); - - float cell_advance = 0.0F; - float cell_alignment = 0.0F; - - switch (side) { - case Side::RIGHT: - cell_advance = cell_position.x; - cell_alignment = std::abs(this->cell_size / 2.0F - cell_position.y); - break; - case Side::UP: - cell_advance = cell_position.y; - cell_alignment = std::abs(this->cell_size / 2.0F - cell_position.x); - break; - case Side::LEFT: - cell_advance = this->cell_size - cell_position.x; - cell_alignment = std::abs(this->cell_size / 2.0F - cell_position.y); - break; - case Side::DOWN: - cell_advance = this->cell_size - cell_position.y; - cell_alignment = std::abs(this->cell_size / 2.0F - cell_position.x); - break; - } - Information information{}; + nav::Point cell_position = pose.to_cell(cell_size); - if (cell_advance < this->front_sensors_region_division) { + if (cell_position.y < this->front_sensors_region_division) { if (this->wall_sensors.get_observation(Sensor::FRONT_LEFT) == core::Observation::WALL and this->wall_sensors.get_observation(Sensor::FRONT_RIGHT) == core::Observation::WALL) { information.front = core::Observation::WALL; @@ -79,18 +55,13 @@ void Mapping::update(const Pose& pose) { } } - if (cell_alignment < this->alignment_threshold * this->cell_size) { - if (cell_advance < this->side_sensors_region_division) { - information.left = this->wall_sensors.get_observation(Sensor::LEFT); - information.right = this->wall_sensors.get_observation(Sensor::RIGHT); - } else if (information.front != core::Observation::WALL and - cell_advance > this->side_sensors_region_division + this->wall_thickness) { - information.front_left = this->wall_sensors.get_observation(Sensor::LEFT); - information.front_right = this->wall_sensors.get_observation(Sensor::RIGHT); - } + if (cell_position.x < this->alignment_threshold * this->cell_size and + information.front != core::Observation::WALL and cell_position.y > this->side_sensors_region_division) { + information.front_left = this->wall_sensors.get_observation(Sensor::LEFT); + information.front_right = this->wall_sensors.get_observation(Sensor::RIGHT); } - this->maze.update({pose.position.to_grid(this->cell_size), side}, information); + this->maze.update(pose.to_grid(this->cell_size), information); } template diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 6c8327e..9ed2249 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -58,4 +58,32 @@ Point Point::from_grid(const GridPoint& grid_point, float cell_size) { GridPose Pose::to_grid(float cell_size) const { return {this->position.to_grid(cell_size), angle_to_grid(this->orientation)}; }; + +Point Pose::to_cell(float cell_size) const { + Point cell_position = this->position % cell_size; + + float cell_advance = 0.0F; + float cell_alignment = 0.0F; + + switch (angle_to_grid(this->orientation)) { + case Side::RIGHT: + cell_advance = cell_position.x; + cell_alignment = std::abs(cell_size / 2.0F - cell_position.y); + break; + case Side::UP: + cell_advance = cell_position.y; + cell_alignment = std::abs(cell_size / 2.0F - cell_position.x); + break; + case Side::LEFT: + cell_advance = cell_size - cell_position.x; + cell_alignment = std::abs(cell_size / 2.0F - cell_position.y); + break; + case Side::DOWN: + cell_advance = cell_size - cell_position.y; + cell_alignment = std::abs(cell_size / 2.0F - cell_position.x); + break; + } + + return {cell_alignment, cell_advance}; +} } // namespace micras::nav From fff1985f0cfe86ddff60e0472c3e0d12edbe0e3e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 9 Oct 2024 05:38:09 -0300 Subject: [PATCH 237/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Increase=20can=5Ff?= =?UTF-8?q?ollow=5Fwall=20tolerance?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/mapping.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 9748e3f..fef963c 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -25,7 +25,7 @@ Mapping::Mapping(const proxy::WallSensors<4>& wall_sensors, Mappi side_sensor_pose{config.side_sensor_pose}, front_sensors_region_division{cell_size - wall_thickness / 2.0F - front_sensor_pose.position.y}, side_sensors_region_division{ - cell_size + wall_thickness - side_sensor_pose.position.y + + cell_size + wall_thickness / 2 - side_sensor_pose.position.y + (side_sensor_pose.position.x + (wall_thickness - cell_size) / 2.0F) * std::tan(side_sensor_pose.orientation) }, front_alignment_tolerance{config.front_alignment_tolerance}, @@ -194,15 +194,11 @@ bool Mapping::can_follow_wall(const Pose& pose) const { return false; } - if (cell_position.y < this->side_sensors_region_division - 2 * this->wall_thickness) { + if (cell_position.y < this->side_sensors_region_division - this->wall_thickness / 2.0F) { return this->maze.can_follow_wall(pose.to_grid(this->cell_size), false); } - if (cell_position.y > this->side_sensors_region_division) { - return this->maze.can_follow_wall(pose.to_grid(this->cell_size), true); - } - - return false; + return this->maze.can_follow_wall(pose.to_grid(this->cell_size), true); } } // namespace micras::nav From 1e3542a341461f8ff2acfae9262ebea206c5cca4 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 9 Oct 2024 06:18:15 -0300 Subject: [PATCH 238/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20maz=20bug=20of=20r?= =?UTF-8?q?obot=20not=20returning?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 4c18297..4a171c6 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -84,10 +84,10 @@ GridPose Maze::get_current_goal(const GridPoint& position, bool f auto current = this->best_route.find(current_cost); if (this->returning) { - return std::prev(current)->second.turned_back(); + return {std::prev(current)->second.position, current->second.turned_back().orientation}; } - return {std::next(current)->second.position, current->second.orientation}; + return std::next(current)->second; } GridPoint next_position = position; @@ -220,12 +220,12 @@ void Maze::calculate_costmap() { template void Maze::optimize_route() { - for (auto it = this->best_route.begin(); it != this->best_route.end();) { + for (auto it = std::next(this->best_route.begin()); it != this->best_route.end();) { auto next = std::next(it); if (next != this->best_route.end()) { if (it->second.orientation == next->second.orientation) { - this->best_route.erase(next); + it = this->best_route.erase(it); continue; } } From 511fcdd9fed41dd6f02120639df7f30d73e1f455 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 9 Oct 2024 19:01:48 -0300 Subject: [PATCH 239/561] =?UTF-8?q?=E2=9C=A8=20Add=20new=20FollowWall=20ty?= =?UTF-8?q?pes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/types.hpp | 11 ++ micras_nav/include/micras/nav/follow_wall.hpp | 9 +- micras_nav/include/micras/nav/go_to_point.hpp | 6 +- micras_nav/include/micras/nav/mapping.hpp | 39 ++++--- micras_nav/include/micras/nav/maze.hpp | 6 +- micras_nav/src/follow_wall.cpp | 25 ++++- micras_nav/src/go_to_point.cpp | 8 +- micras_nav/src/mapping.cpp | 104 +++++++----------- micras_nav/src/maze.cpp | 35 ++++-- 9 files changed, 139 insertions(+), 104 deletions(-) diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index d42e802..df2a242 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -12,6 +12,17 @@ enum Observation : uint8_t { FREE_SPACE = 1, WALL = 2 }; + +/** + * @brief Type of wall following the robot is able ot do + */ +enum FollowWallType : uint8_t { + NONE = 0, + FRONT = 1, + LEFT = 2, + RIGHT = 3, + PARALLEL = 4 +}; } // namespace micras::core #endif // MICRAS_CORE_TYPES_HPP diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 83d9e2d..461294c 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -24,7 +24,6 @@ class FollowWall { */ struct Config { PidController::Config pid; - float can_follow_tolerance{}; float base_left_reading{}; float base_right_reading{}; float cutoff_frequency{}; @@ -41,11 +40,12 @@ class FollowWall { /** * @brief Update the PID controller and return the response * + * @param follow_wall_type The type of wall following to perform * @param elapsed_time The time elapsed since the last update * * @return The response of the PID controller */ - float action(float elapsed_time); + float action(core::FollowWallType follow_wall_type, float elapsed_time); /** * @brief Resets the PID controller @@ -92,11 +92,6 @@ class FollowWall { */ float last_diff{}; - /** - * @brief Tolerance for the robot to follow the wall - */ - float can_follow_tolerance; - /** * @brief Base reading of the left wall sensor when aligned with the wall */ diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index efb5edb..31351d6 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -49,12 +49,16 @@ class GoToPoint { * * @param state The current state of the robot * @param goal The goal point + * @param follow_wall_type The type of wall following to perform * @param elapsed_time The time elapsed since the last update * @param stop Whether to stop at the goal * * @return The command to go to the point */ - Twist action(const State& state, const Point& goal, bool can_follow_wall, float elapsed_time, bool stop = true); + Twist action( + const State& state, const Point& goal, core::FollowWallType follow_wall_type, float elapsed_time, + bool stop = true + ); /** * @brief Checks if the robot has reached the goal diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index d314ee1..fc19f54 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -37,7 +37,6 @@ class Mapping { float front_alignment_tolerance{}; float side_alignment_tolerance{}; float orientation_alignment_tolerance{}; - float can_follow_wall_tolerance{}; std::array front_alignment_measure{}; std::array side_alignment_measure{}; @@ -115,9 +114,10 @@ class Mapping { * @brief Fix the pose based on the mapping information * * @param pose The current pose of the robot + * @param follow_wall_type The type of wall following being performed * @return The fixed pose */ - Pose correct_pose(const Pose& pose, bool can_follow_wall) const; + Pose correct_pose(const Pose& pose, core::FollowWallType follow_wall_type) const; /** * @brief Calibrate front alignment of the wall sensors @@ -130,27 +130,40 @@ class Mapping { void calibrate_side(); /** - * @brief Check if the robot is aligned with a wall at the front + * @brief Check if the robot is at a distance from a wall at the front * - * @return True if the robot is front aligned, false otherwise + * @return True if the robot is aligned, false otherwise */ - bool is_front_aligned() const; + bool is_distance_front_aligned() const; /** - * @brief Check if the robot is aligned with a wall at the sides + * @brief Check if the robot orientation is aligned with walls at the front * - * @return True if the robot is side aligned, false otherwise + * @return True if the robot is aligned, false otherwise */ - bool is_side_aligned() const; + bool is_orientation_front_aligned() const; - bool is_orientation_aligned() const; + /** + * @brief Check if the robot is at a distance from a wall at the sides + * + * @return True if the robot is aligned, false otherwise + */ + bool is_distance_side_aligned() const; /** - * @brief Check if the robot can follow a wall + * @brief Check if the robot orientation is aligned with walls at the sides * - * @return True if the robot can follow a wall, false otherwise + * @return True if the robot is aligned, false otherwise */ - bool can_follow_wall(const Pose& pose) const; + bool is_orientation_side_aligned() const; + + /** + * @brief Get the type of wall following the robot can do + * + * @param pose The current pose of the robot + * @return The type of wall following + */ + core::FollowWallType get_follow_wall_type(const Pose& pose) const; private: /** @@ -213,8 +226,6 @@ class Mapping { */ float orientation_alignment_tolerance; - float can_follow_wall_tolerance; - /** * @brief Sensor readings for the front alignment of the robot */ diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 94b38b7..5709ad6 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -52,13 +52,13 @@ class Maze { GridPose get_current_goal(const GridPoint& position, bool force_costmap = false) const; /** - * @brief Checks whether the robot can follow a wall + * @brief Checks the type of wall following the robot can do * * @param pose The pose of the robot * @param front_cell Whether to check the cell at the front of the robot - * @return True if the robot can follow the wall, false otherwise + * @return The type of wall following */ - bool can_follow_wall(const GridPose& pose, bool front_cell) const; + core::FollowWallType get_follow_wall_type(const GridPose& pose, bool front_cell) const; private: /** diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index a7c25af..0d55b81 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -13,12 +13,31 @@ FollowWall::FollowWall(const proxy::WallSensors<4>& wall_sensors, const Config& wall_sensors{wall_sensors}, pid{config.pid}, filter{config.cutoff_frequency}, - can_follow_tolerance{config.can_follow_tolerance}, base_left_reading{config.base_left_reading}, base_right_reading{config.base_right_reading} { } -float FollowWall::action(float elapsed_time) { - float error = this->get_left_value() - this->get_right_value(); +float FollowWall::action(core::FollowWallType follow_wall_type, float elapsed_time) { + float error{}; + + switch (follow_wall_type) { + case core::FollowWallType::FRONT: + error = this->get_right_value() - this->get_left_value(); + break; + + case core::FollowWallType::LEFT: + error = 2.0F * this->get_left_value(); + break; + + case core::FollowWallType::RIGHT: + error = 2.0F * this->get_right_value(); + break; + + case core::FollowWallType::PARALLEL: + error = this->get_left_value() - this->get_right_value(); + break; + default: + return 0.0F; + } if (this->last_diff == 0.0F) { this->last_diff = error; diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp index 1e6d88a..7c8d403 100644 --- a/micras_nav/src/go_to_point.cpp +++ b/micras_nav/src/go_to_point.cpp @@ -25,16 +25,18 @@ GoToPoint::GoToPoint( distance_tolerance{config.distance_tolerance}, velocity_tolerance{config.velocity_tolerance} { } -Twist GoToPoint::action(const State& state, const Point& goal, bool can_follow_wall, float elapsed_time, bool stop) { +Twist GoToPoint::action( + const State& state, const Point& goal, core::FollowWallType follow_wall_type, float elapsed_time, bool stop +) { float linear_command{}; float angular_command{}; Point goal_distance = goal - state.pose.position; float linear_target{}; - if (can_follow_wall) { + if (follow_wall_type != core::FollowWallType::NONE) { linear_target = this->base_speed; - angular_command = this->follow_wall.action(elapsed_time); + angular_command = this->follow_wall.action(follow_wall_type, elapsed_time); } else { float angular_error = core::assert_angle(state.pose.orientation - std::atan2(this->cell_size, goal_distance.x)); linear_target = core::decay(angular_error, this->linear_decay_damping) * this->base_speed; diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index fef963c..a6f25ae 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -30,7 +30,7 @@ Mapping::Mapping(const proxy::WallSensors<4>& wall_sensors, Mappi }, front_alignment_tolerance{config.front_alignment_tolerance}, side_alignment_tolerance{config.side_alignment_tolerance}, - can_follow_wall_tolerance{config.can_follow_wall_tolerance}, + orientation_alignment_tolerance{config.orientation_alignment_tolerance}, front_alignment_measure{config.front_alignment_measure}, side_alignment_measure{config.side_alignment_measure} { } @@ -79,67 +79,42 @@ Mapping::Action Mapping::get_action(const Pose& po } template -Pose Mapping::correct_pose(const Pose& pose, bool can_follow_wall) const { +Pose Mapping::correct_pose(const Pose& pose, core::FollowWallType follow_wall_type) const { float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); if (reliability < 50.0F) { return pose; } - Pose corrected_pose = pose; - Side side = angle_to_grid(pose.orientation); - - switch (side) { - case Side::RIGHT: - if (this->is_front_aligned()) { - corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; - } else if (can_follow_wall) { - if (this->is_side_aligned()) { - corrected_pose.orientation = 0.0F; - corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; - } else if (this->is_orientation_aligned()) { - corrected_pose.orientation = 0.0F; - } - } - break; - case Side::UP: - if (this->is_front_aligned()) { - corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; - } else if (can_follow_wall) { - if (this->is_side_aligned()) { - corrected_pose.orientation = std::numbers::pi_v / 2.0F; - corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; - } else if (this->is_orientation_aligned()) { - corrected_pose.orientation = std::numbers::pi_v / 2.0F; - } + Point cell_position = pose.to_cell(this->cell_size); + Point pose_correction{}; + Pose corrected_pose = pose; + Side direction = angle_to_grid(pose.orientation); + + switch (follow_wall_type) { + case core::FollowWallType::FRONT: + if (this->is_distance_front_aligned()) { + pose_correction.y = cell_position.y - this->cell_size / 2.0F; } - break; - case Side::LEFT: - if (this->is_front_aligned()) { - corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; - } else if (can_follow_wall) { - if (this->is_side_aligned()) { - corrected_pose.orientation = std::numbers::pi_v; - corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; - } else if (this->is_orientation_aligned()) { - corrected_pose.orientation = std::numbers::pi_v; - } + + if (this->is_orientation_front_aligned()) { + corrected_pose.orientation = static_cast(direction) * std::numbers::pi_v / 2.0F; } break; - case Side::DOWN: - if (this->is_front_aligned()) { - corrected_pose.position.y -= std::fmod(pose.position.y, this->cell_size) - this->cell_size / 2.0F; - } else if (can_follow_wall) { - if (this->is_side_aligned()) { - corrected_pose.orientation = -std::numbers::pi_v / 2.0F; - corrected_pose.position.x -= std::fmod(pose.position.x, this->cell_size) - this->cell_size / 2.0F; - } else if (this->is_orientation_aligned()) { - corrected_pose.orientation = -std::numbers::pi_v / 2.0F; - } + case core::FollowWallType::PARALLEL: + if (this->is_distance_side_aligned()) { + pose_correction.x = cell_position.x; + corrected_pose.orientation = static_cast(direction) * std::numbers::pi_v / 2.0F; + } else if (this->is_orientation_side_aligned()) { + corrected_pose.orientation = static_cast(direction) * std::numbers::pi_v / 2.0F; } break; + default: + return pose; } + corrected_pose.position = corrected_pose.position - pose_correction.rotate(direction); + return corrected_pose; } @@ -156,7 +131,7 @@ void Mapping::calibrate_side() { } template -bool Mapping::is_front_aligned() const { +bool Mapping::is_distance_front_aligned() const { return core::is_near( this->wall_sensors.get_reading(0), this->front_alignment_measure[0], this->front_alignment_tolerance ) and @@ -166,7 +141,15 @@ bool Mapping::is_front_aligned() const { } template -bool Mapping::is_side_aligned() const { +bool Mapping::is_orientation_front_aligned() const { + return std::abs( + (this->wall_sensors.get_reading(1) - this->side_alignment_measure[0]) - + (this->wall_sensors.get_reading(2) - this->side_alignment_measure[1]) + ) <= this->orientation_alignment_tolerance; +} + +template +bool Mapping::is_distance_side_aligned() const { return core::is_near( this->wall_sensors.get_reading(1), this->side_alignment_measure[0], this->side_alignment_tolerance ) and @@ -176,29 +159,22 @@ bool Mapping::is_side_aligned() const { } template -bool Mapping::is_orientation_aligned() const { +bool Mapping::is_orientation_side_aligned() const { return std::abs( - this->wall_sensors.get_reading(1) - this->side_alignment_measure[0] + this->wall_sensors.get_reading(2) - - this->side_alignment_measure[1] + (this->wall_sensors.get_reading(1) - this->side_alignment_measure[0]) + + (this->wall_sensors.get_reading(2) - this->side_alignment_measure[1]) ) <= this->orientation_alignment_tolerance; } template -bool Mapping::can_follow_wall(const Pose& pose) const { +core::FollowWallType Mapping::get_follow_wall_type(const Pose& pose) const { nav::Point cell_position = pose.to_cell(cell_size); - if (std::abs( - this->wall_sensors.get_reading(1) - this->side_alignment_measure[0] + this->wall_sensors.get_reading(2) - - this->side_alignment_measure[1] - ) > this->can_follow_wall_tolerance) { - return false; - } - if (cell_position.y < this->side_sensors_region_division - this->wall_thickness / 2.0F) { - return this->maze.can_follow_wall(pose.to_grid(this->cell_size), false); + return this->maze.get_follow_wall_type(pose.to_grid(this->cell_size), false); } - return this->maze.can_follow_wall(pose.to_grid(this->cell_size), true); + return this->maze.get_follow_wall_type(pose.to_grid(this->cell_size), true); } } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 4a171c6..f291603 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -149,23 +149,40 @@ bool Maze::has_wall(const GridPose& pose) const { } template -bool Maze::can_follow_wall(const GridPose& pose, bool front_cell) const { +core::FollowWallType Maze::get_follow_wall_type(const GridPose& pose, bool front_cell) const { if (pose.position.x >= width or pose.position.y >= height) { - return false; + return core::FollowWallType::NONE; } - if (not front_cell) { - return this->has_wall(pose.turned_left()) and this->has_wall(pose.turned_right()); + GridPose check_pose{}; + + if (front_cell) { + if (this->has_wall(pose)) { + return core::FollowWallType::FRONT; + } + + check_pose = pose.front(); + + if (check_pose.position.x >= width or check_pose.position.y >= height) { + return core::FollowWallType::NONE; + } + } else { + check_pose = pose; } - GridPose front_pose = pose.front(); + if (this->has_wall(check_pose.turned_left()) and this->has_wall(check_pose.turned_right())) { + return core::FollowWallType::PARALLEL; + } - if (front_pose.position.x >= width or front_pose.position.y >= height) { - return false; + if (this->has_wall(check_pose.turned_left())) { + return core::FollowWallType::LEFT; + } + + if (this->has_wall(check_pose.turned_right())) { + return core::FollowWallType::RIGHT; } - return not this->has_wall(pose) and this->has_wall(front_pose.turned_left()) and - this->has_wall(front_pose.turned_right()); + return core::FollowWallType::NONE; } template From adc9036d83ee488040878baf444fb789a79450bd Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 10 Oct 2024 00:12:12 -0300 Subject: [PATCH 240/561] =?UTF-8?q?=F0=9F=A5=85=20Fix=20mapping=20errors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/mapping.hpp | 38 +++++++++----- micras_nav/src/follow_wall.cpp | 2 +- micras_nav/src/mapping.cpp | 60 ++++++++++++++--------- micras_nav/src/state.cpp | 8 +-- 4 files changed, 66 insertions(+), 42 deletions(-) diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index fc19f54..cdbbe4a 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -34,11 +34,13 @@ class Mapping { float alignment_threshold{}; Pose front_sensor_pose{}; Pose side_sensor_pose{}; - float front_alignment_tolerance{}; - float side_alignment_tolerance{}; - float orientation_alignment_tolerance{}; - std::array front_alignment_measure{}; - std::array side_alignment_measure{}; + float front_distance_alignment_tolerance{}; + float side_distance_alignment_tolerance{}; + float front_orientation_alignment_tolerance{}; + float side_orientation_alignment_tolerance{}; + std::array front_distance_reading{}; + std::array front_orientation_reading{}; + std::array side_distance_reading{}; GridPose start{}; std::unordered_set goal{ @@ -214,27 +216,37 @@ class Mapping { /** * @brief Sensor readings tolerance for the front alignment of the robot */ - float front_alignment_tolerance; + float front_distance_alignment_tolerance; /** * @brief Sensor readings tolerance for the side alignment of the robot */ - float side_alignment_tolerance; + float side_distance_alignment_tolerance; /** - * @brief Sensor readings for the front alignment of the robot + * @brief Sensor readings for the front alignment of the robot at the front */ - float orientation_alignment_tolerance; + float front_orientation_alignment_tolerance; /** - * @brief Sensor readings for the front alignment of the robot + * @brief Sensor readings for the front alignment of the robot with walls at the side */ - std::array front_alignment_measure{}; + float side_orientation_alignment_tolerance; /** - * @brief Sensor readings for the side alignment of the robot + * @brief Sensor readings when the front of the robot is distance aligned */ - std::array side_alignment_measure{}; + std::array front_distance_reading{}; + + /** + * @brief Sensor readings when the front of the robot is orientation aligned + */ + std::array front_orientation_reading{}; + + /** + * @brief Sensor readings when the sides of the robot are distance aligned + */ + std::array side_distance_reading{}; }; } // namespace micras::nav diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 0d55b81..e7a9342 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -29,7 +29,7 @@ float FollowWall::action(core::FollowWallType follow_wall_type, float elapsed_ti break; case core::FollowWallType::RIGHT: - error = 2.0F * this->get_right_value(); + error = -2.0F * this->get_right_value(); break; case core::FollowWallType::PARALLEL: diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index a6f25ae..184d37c 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -28,11 +28,13 @@ Mapping::Mapping(const proxy::WallSensors<4>& wall_sensors, Mappi cell_size + wall_thickness / 2 - side_sensor_pose.position.y + (side_sensor_pose.position.x + (wall_thickness - cell_size) / 2.0F) * std::tan(side_sensor_pose.orientation) }, - front_alignment_tolerance{config.front_alignment_tolerance}, - side_alignment_tolerance{config.side_alignment_tolerance}, - orientation_alignment_tolerance{config.orientation_alignment_tolerance}, - front_alignment_measure{config.front_alignment_measure}, - side_alignment_measure{config.side_alignment_measure} { } + front_distance_alignment_tolerance{config.front_distance_alignment_tolerance}, + side_distance_alignment_tolerance{config.side_distance_alignment_tolerance}, + front_orientation_alignment_tolerance{config.front_orientation_alignment_tolerance}, + side_orientation_alignment_tolerance{config.side_orientation_alignment_tolerance}, + front_distance_reading{config.front_distance_reading}, + front_orientation_reading{config.front_orientation_reading}, + side_distance_reading{config.side_distance_reading} { } template void Mapping::update(const Pose& pose) { @@ -55,7 +57,7 @@ void Mapping::update(const Pose& pose) { } } - if (cell_position.x < this->alignment_threshold * this->cell_size and + if (std::abs(cell_position.x) < this->alignment_threshold * this->cell_size and information.front != core::Observation::WALL and cell_position.y > this->side_sensors_region_division) { information.front_left = this->wall_sensors.get_observation(Sensor::LEFT); information.front_right = this->wall_sensors.get_observation(Sensor::RIGHT); @@ -98,72 +100,82 @@ Pose Mapping::correct_pose(const Pose& pose, core::FollowWallType } if (this->is_orientation_front_aligned()) { - corrected_pose.orientation = static_cast(direction) * std::numbers::pi_v / 2.0F; + corrected_pose.orientation = + core::assert_angle(static_cast(direction) * std::numbers::pi_v / 2.0F); } break; case core::FollowWallType::PARALLEL: if (this->is_distance_side_aligned()) { pose_correction.x = cell_position.x; - corrected_pose.orientation = static_cast(direction) * std::numbers::pi_v / 2.0F; + corrected_pose.orientation = + core::assert_angle(static_cast(direction) * std::numbers::pi_v / 2.0F); } else if (this->is_orientation_side_aligned()) { - corrected_pose.orientation = static_cast(direction) * std::numbers::pi_v / 2.0F; + corrected_pose.orientation = + core::assert_angle(static_cast(direction) * std::numbers::pi_v / 2.0F); } break; default: return pose; } - corrected_pose.position = corrected_pose.position - pose_correction.rotate(direction); + pose_correction = pose_correction.rotate(static_cast((6 - direction) % 4)); + corrected_pose.position = corrected_pose.position - pose_correction; return corrected_pose; } template void Mapping::calibrate_front() { - this->front_alignment_measure[0] = this->wall_sensors.get_reading(0); - this->front_alignment_measure[1] = this->wall_sensors.get_reading(3); + this->front_distance_reading[0] = this->wall_sensors.get_reading(0); + front_orientation_reading[0] = this->wall_sensors.get_reading(1); + front_orientation_reading[1] = this->wall_sensors.get_reading(2); + this->front_distance_reading[1] = this->wall_sensors.get_reading(3); } template void Mapping::calibrate_side() { - this->side_alignment_measure[0] = this->wall_sensors.get_reading(1); - this->side_alignment_measure[1] = this->wall_sensors.get_reading(2); + this->side_distance_reading[0] = this->wall_sensors.get_reading(1); + this->side_distance_reading[1] = this->wall_sensors.get_reading(2); } template bool Mapping::is_distance_front_aligned() const { return core::is_near( - this->wall_sensors.get_reading(0), this->front_alignment_measure[0], this->front_alignment_tolerance + this->wall_sensors.get_reading(0), this->front_distance_reading[0], + this->front_distance_alignment_tolerance ) and core::is_near( - this->wall_sensors.get_reading(3), this->front_alignment_measure[1], this->front_alignment_tolerance + this->wall_sensors.get_reading(3), this->front_distance_reading[1], + this->front_distance_alignment_tolerance ); } template bool Mapping::is_orientation_front_aligned() const { return std::abs( - (this->wall_sensors.get_reading(1) - this->side_alignment_measure[0]) - - (this->wall_sensors.get_reading(2) - this->side_alignment_measure[1]) - ) <= this->orientation_alignment_tolerance; + (this->wall_sensors.get_reading(1) - this->front_orientation_reading[0]) + + (this->wall_sensors.get_reading(2) - this->front_orientation_reading[1]) + ) <= this->front_orientation_alignment_tolerance; } template bool Mapping::is_distance_side_aligned() const { return core::is_near( - this->wall_sensors.get_reading(1), this->side_alignment_measure[0], this->side_alignment_tolerance + this->wall_sensors.get_reading(1), this->side_distance_reading[0], + this->side_distance_alignment_tolerance ) and core::is_near( - this->wall_sensors.get_reading(2), this->side_alignment_measure[1], this->side_alignment_tolerance + this->wall_sensors.get_reading(2), this->side_distance_reading[1], + this->side_distance_alignment_tolerance ); } template bool Mapping::is_orientation_side_aligned() const { return std::abs( - (this->wall_sensors.get_reading(1) - this->side_alignment_measure[0]) + - (this->wall_sensors.get_reading(2) - this->side_alignment_measure[1]) - ) <= this->orientation_alignment_tolerance; + (this->wall_sensors.get_reading(1) - this->side_distance_reading[0]) + + (this->wall_sensors.get_reading(2) - this->side_distance_reading[1]) + ) <= this->side_orientation_alignment_tolerance; } template diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 9ed2249..b509d9b 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -68,19 +68,19 @@ Point Pose::to_cell(float cell_size) const { switch (angle_to_grid(this->orientation)) { case Side::RIGHT: cell_advance = cell_position.x; - cell_alignment = std::abs(cell_size / 2.0F - cell_position.y); + cell_alignment = cell_size / 2.0F - cell_position.y; break; case Side::UP: cell_advance = cell_position.y; - cell_alignment = std::abs(cell_size / 2.0F - cell_position.x); + cell_alignment = cell_position.x - cell_size / 2.0F; break; case Side::LEFT: cell_advance = cell_size - cell_position.x; - cell_alignment = std::abs(cell_size / 2.0F - cell_position.y); + cell_alignment = cell_position.y - cell_size / 2.0F; break; case Side::DOWN: cell_advance = cell_size - cell_position.y; - cell_alignment = std::abs(cell_size / 2.0F - cell_position.x); + cell_alignment = cell_size / 2.0F - cell_position.x; break; } From 60135c201d6ef373b5939fb1927d8ed3363503f8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 13 Oct 2024 02:27:34 -0300 Subject: [PATCH 241/561] =?UTF-8?q?=E2=9C=A8Sincronize=20wall=20sensor=20r?= =?UTF-8?q?eading=20with=20ir=20led=20activation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/wall_sensors.hpp | 2 +- micras_proxy/src/wall_sensors.cpp | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 69d3660..2a3e484 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -137,7 +137,7 @@ class WallSensors { /** * @brief Buffer to store the ADC values */ - std::array buffer; + std::array buffer; /** * @brief Butterworth filter for the ADC readings diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index 690ee2b..e3f7532 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -28,8 +28,8 @@ WallSensors::WallSensors(const Config& config) : template void WallSensors::turn_on() { - this->led_0_pwm.set_duty_cycle(100.0F); - this->led_1_pwm.set_duty_cycle(100.0F); + this->led_0_pwm.set_duty_cycle(50.0F); + this->led_1_pwm.set_duty_cycle(50.0F); } template @@ -67,7 +67,9 @@ float WallSensors::get_reading(uint8_t sensor_index) const { template float WallSensors::get_adc_reading(uint8_t sensor_index) const { - return static_cast(this->buffer.at(sensor_index)) / this->adc.get_max_reading(); + return static_cast(std::abs(this->buffer.at(sensor_index) - this->buffer.at(sensor_index + num_of_sensors)) + ) / + this->adc.get_max_reading(); } template From 19237e796eabb340cd0e33948b5cf7316aa5a828 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 4 Dec 2024 18:30:58 -0300 Subject: [PATCH 242/561] =?UTF-8?q?=E2=9A=B0=EF=B8=8F=20Remove=20extra=20i?= =?UTF-8?q?nfo=20from=20doxygen=20file=20headers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/types.hpp | 4 ++++ micras_core/include/micras/core/utils.hpp | 6 +----- micras_core/src/butterworth_filter.cpp | 4 ++++ micras_hal/include/micras/hal/adc_dma.hpp | 6 +----- micras_hal/include/micras/hal/crc.hpp | 6 +----- micras_hal/include/micras/hal/encoder.hpp | 6 +----- micras_hal/include/micras/hal/flash.hpp | 6 +----- micras_hal/include/micras/hal/gpio.hpp | 6 +----- micras_hal/include/micras/hal/mcu.hpp | 6 +----- micras_hal/include/micras/hal/pwm.hpp | 6 +----- micras_hal/include/micras/hal/pwm_dma.hpp | 6 +----- micras_hal/include/micras/hal/spi.hpp | 6 +----- micras_hal/include/micras/hal/timer.hpp | 6 +----- micras_hal/stm32/src/adc_dma.cpp | 6 +----- micras_hal/stm32/src/crc.cpp | 6 +----- micras_hal/stm32/src/encoder.cpp | 6 +----- micras_hal/stm32/src/flash.cpp | 6 +----- micras_hal/stm32/src/gpio.cpp | 6 +----- micras_hal/stm32/src/mcu.cpp | 4 +--- micras_hal/stm32/src/pwm.cpp | 6 +----- micras_hal/stm32/src/pwm_dma.cpp | 6 +----- micras_hal/stm32/src/spi.cpp | 6 +----- micras_hal/stm32/src/timer.cpp | 6 +----- micras_nav/include/micras/nav/follow_wall.hpp | 6 +----- micras_nav/include/micras/nav/go_to_point.hpp | 6 +----- micras_nav/include/micras/nav/grid_pose.hpp | 6 +----- micras_nav/include/micras/nav/look_at_point.hpp | 6 +----- micras_nav/include/micras/nav/mapping.hpp | 6 +----- micras_nav/include/micras/nav/maze.hpp | 6 +----- micras_nav/include/micras/nav/odometry.hpp | 6 +----- micras_nav/include/micras/nav/pid_controller.hpp | 6 +----- micras_nav/include/micras/nav/state.hpp | 6 +----- micras_nav/src/follow_wall.cpp | 6 +----- micras_nav/src/go_to_point.cpp | 6 +----- micras_nav/src/grid_pose.cpp | 6 +----- micras_nav/src/look_at_point.cpp | 6 +----- micras_nav/src/mapping.cpp | 6 +----- micras_nav/src/maze.cpp | 6 +----- micras_nav/src/odometry.cpp | 6 +----- micras_nav/src/pid_controller.cpp | 6 +----- micras_nav/src/state.cpp | 6 +----- micras_proxy/include/micras/proxy/argb.hpp | 6 +----- micras_proxy/include/micras/proxy/battery.hpp | 6 +----- micras_proxy/include/micras/proxy/button.hpp | 6 +----- micras_proxy/include/micras/proxy/buzzer.hpp | 6 +----- micras_proxy/include/micras/proxy/dip_switch.hpp | 6 +----- micras_proxy/include/micras/proxy/fan.hpp | 6 +----- micras_proxy/include/micras/proxy/imu.hpp | 6 +----- micras_proxy/include/micras/proxy/led.hpp | 6 +----- micras_proxy/include/micras/proxy/locomotion.hpp | 6 +----- micras_proxy/include/micras/proxy/motor.hpp | 6 +----- micras_proxy/include/micras/proxy/rotary_sensor.hpp | 6 +----- micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp | 7 +------ .../include/micras/proxy/serializable_interface.hpp | 6 +----- micras_proxy/include/micras/proxy/storage.hpp | 6 +----- micras_proxy/include/micras/proxy/torque_sensors.hpp | 6 +----- micras_proxy/include/micras/proxy/wall_sensors.hpp | 6 +----- micras_proxy/src/argb.cpp | 6 +----- micras_proxy/src/battery.cpp | 6 +----- micras_proxy/src/button.cpp | 6 +----- micras_proxy/src/buzzer.cpp | 6 +----- micras_proxy/src/dip_switch.cpp | 6 +----- micras_proxy/src/fan.cpp | 6 +----- micras_proxy/src/imu.cpp | 6 +----- micras_proxy/src/led.cpp | 6 +----- micras_proxy/src/locomotion.cpp | 6 +----- micras_proxy/src/motor.cpp | 6 +----- micras_proxy/src/rotary_sensor.cpp | 6 +----- micras_proxy/src/storage.cpp | 6 +----- micras_proxy/src/torque_sensors.cpp | 6 +----- micras_proxy/src/wall_sensors.cpp | 6 +----- 71 files changed, 77 insertions(+), 344 deletions(-) diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index df2a242..fb69e08 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -1,3 +1,7 @@ +/** + * @file + */ + #ifndef MICRAS_CORE_TYPES_HPP #define MICRAS_CORE_TYPES_HPP diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index 1219ee2..fef7ff2 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -1,9 +1,5 @@ /** - * @file utils.hpp - * - * @brief Core Utils functions - * - * @date 09/2024 + * @file */ #ifndef MICRAS_CORE_UTILS_HPP diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index 3e783e6..f27ff3c 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -1,3 +1,7 @@ +/** + * @file + */ + #include #include diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index a32f3bc..77d3e66 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -1,9 +1,5 @@ /** - * @file adc_dma.hpp - * - * @brief ADC DMA HAL header - * - * @date 03/2024 + * @file */ #ifndef MICRAS_HAL_ADC_DMA_HPP diff --git a/micras_hal/include/micras/hal/crc.hpp b/micras_hal/include/micras/hal/crc.hpp index bc1e6cd..648c942 100644 --- a/micras_hal/include/micras/hal/crc.hpp +++ b/micras_hal/include/micras/hal/crc.hpp @@ -1,9 +1,5 @@ /** - * @file crc.hpp - * - * @brief STM32 CRC HAL wrapper - * - * @date 03/2024 + * @file */ #ifndef MICRAS_HAL_CRC_HPP diff --git a/micras_hal/include/micras/hal/encoder.hpp b/micras_hal/include/micras/hal/encoder.hpp index 8e01ec4..bbad892 100644 --- a/micras_hal/include/micras/hal/encoder.hpp +++ b/micras_hal/include/micras/hal/encoder.hpp @@ -1,9 +1,5 @@ /** - * @file encoder.hpp - * - * @brief STM32 encoder HAL wrapper - * - * @date 03/2024 + * @file */ #ifndef MICRAS_HAL_ENCODER_HPP diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp index 9fac40f..859253f 100644 --- a/micras_hal/include/micras/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -1,9 +1,5 @@ /** - * @file flash.hpp - * - * @brief STM32 flash HAL wrapper - * - * @date 03/2024 + * @file */ #ifndef MICRAS_HAL_FLASH_HPP diff --git a/micras_hal/include/micras/hal/gpio.hpp b/micras_hal/include/micras/hal/gpio.hpp index bd350a8..261220e 100644 --- a/micras_hal/include/micras/hal/gpio.hpp +++ b/micras_hal/include/micras/hal/gpio.hpp @@ -1,9 +1,5 @@ /** - * @file gpio.hpp - * - * @brief HAL GPIO class header - * - * @date 03/2024 + * @file */ #ifndef MICRAS_HAL_GPIO_HPP diff --git a/micras_hal/include/micras/hal/mcu.hpp b/micras_hal/include/micras/hal/mcu.hpp index 38e6e1f..8a9c19d 100644 --- a/micras_hal/include/micras/hal/mcu.hpp +++ b/micras_hal/include/micras/hal/mcu.hpp @@ -1,9 +1,5 @@ /** - * @file mcu.hpp - * - * @brief MCU related - * - * @date 03/2024 + * @file */ #ifndef MICRAS_HAL_MCU_HPP diff --git a/micras_hal/include/micras/hal/pwm.hpp b/micras_hal/include/micras/hal/pwm.hpp index fed77a2..6b95854 100644 --- a/micras_hal/include/micras/hal/pwm.hpp +++ b/micras_hal/include/micras/hal/pwm.hpp @@ -1,9 +1,5 @@ /** - * @file pwm.hpp - * - * @brief STM32 PWM HAL wrapper - * - * @date 03/2024 + * @file */ #ifndef MICRAS_HAL_PWM_HPP diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp index 3eb652b..cc1c632 100644 --- a/micras_hal/include/micras/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -1,9 +1,5 @@ /** - * @file pwm_dma.hpp - * - * @brief STM32 PWM DMA HAL wrapper - * - * @date 03/2024 + * @file */ #ifndef MICRAS_HAL_PWM_DMA_HPP diff --git a/micras_hal/include/micras/hal/spi.hpp b/micras_hal/include/micras/hal/spi.hpp index 9102bb6..ce1ae35 100644 --- a/micras_hal/include/micras/hal/spi.hpp +++ b/micras_hal/include/micras/hal/spi.hpp @@ -1,9 +1,5 @@ /** - * @file spi.hpp - * - * @brief STM32 SPI HAL wrapper - * - * @date 03/2024 + * @file */ #ifndef MICRAS_HAL_SPI_HPP diff --git a/micras_hal/include/micras/hal/timer.hpp b/micras_hal/include/micras/hal/timer.hpp index ab5b6c0..8c031bf 100644 --- a/micras_hal/include/micras/hal/timer.hpp +++ b/micras_hal/include/micras/hal/timer.hpp @@ -1,9 +1,5 @@ /** - * @file timer.hpp - * - * @brief STM32 Timer HAL wrapper - * - * @date 03/2024 + * @file */ #ifndef MICRAS_HAL_TIMER_HPP diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 4b75ea5..2525c34 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -1,9 +1,5 @@ /** - * @file adc_dma.cpp - * - * @brief STM32 ADC DMA HAL wrapper - * - * @date 03/2024 + * @file */ #include diff --git a/micras_hal/stm32/src/crc.cpp b/micras_hal/stm32/src/crc.cpp index 0d32810..84c9980 100644 --- a/micras_hal/stm32/src/crc.cpp +++ b/micras_hal/stm32/src/crc.cpp @@ -1,9 +1,5 @@ /** - * @file crc.cpp - * - * @brief STM32 CRC HAL wrapper - * - * @date 03/2024 + * @file */ #include "micras/hal/crc.hpp" diff --git a/micras_hal/stm32/src/encoder.cpp b/micras_hal/stm32/src/encoder.cpp index 25e6da7..315817b 100644 --- a/micras_hal/stm32/src/encoder.cpp +++ b/micras_hal/stm32/src/encoder.cpp @@ -1,9 +1,5 @@ /** - * @file encoder.cpp - * - * @brief STM32 encoder HAL wrapper - * - * @date 03/2024 + * @file */ #include "micras/hal/encoder.hpp" diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index 0f7ed0f..fce95a4 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -1,9 +1,5 @@ /** - * @file flash.cpp - * - * @brief STM32 flash HAL wrapper - * - * @date 03/2024 + * @file */ #include diff --git a/micras_hal/stm32/src/gpio.cpp b/micras_hal/stm32/src/gpio.cpp index 900e924..1d2b542 100644 --- a/micras_hal/stm32/src/gpio.cpp +++ b/micras_hal/stm32/src/gpio.cpp @@ -1,9 +1,5 @@ /** - * @file gpio.cpp - * - * @brief HAL GPIO class source - * - * @date 03/2024 + * @file */ #include "micras/hal/gpio.hpp" diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp index 90751f3..beeaed6 100644 --- a/micras_hal/stm32/src/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -1,7 +1,5 @@ /** - * @file mcu.cpp - * - * @brief MCU related + * @file */ #include diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index 775a6d2..6b2164c 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -1,9 +1,5 @@ /** - * @file pwm.cpp - * - * @brief STM32 PWM HAL wrapper - * - * @date 03/2024 + * @file */ #include diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index 1ecdd70..fc9caac 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -1,9 +1,5 @@ /** - * @file pwm_dma.cpp - * - * @brief STM32 PWM DMA HAL wrapper - * - * @date 03/2024 + * @file */ #include diff --git a/micras_hal/stm32/src/spi.cpp b/micras_hal/stm32/src/spi.cpp index 1e647e9..8c90cda 100644 --- a/micras_hal/stm32/src/spi.cpp +++ b/micras_hal/stm32/src/spi.cpp @@ -1,9 +1,5 @@ /** - * @file spi.cpp - * - * @brief Proxy SPI Switch class source - * - * @date 03/2024 + * @file */ #include "micras/hal/spi.hpp" diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index 695b8df..2993db5 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -1,9 +1,5 @@ /** - * @file timer.cpp - * - * @brief STM32 TIM HAL wrapper - * - * @date 03/2024 + * @file */ #include diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 461294c..47b73e2 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -1,9 +1,5 @@ /** - * @file follow_wall.hpp - * - * @brief Nav FollowWall class declaration - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_FOLLOW_WALL diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index 31351d6..5b40473 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -1,9 +1,5 @@ /** - * @file go_to_point.hpp - * - * @brief Nav GoToPoint class declaration - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_GO_TO_POINT_HPP diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index eff3d6e..3634678 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -1,9 +1,5 @@ /** - * @file grid_pose.hpp - * - * @brief Types for grid-based navigation - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_GRID_POSE_HPP diff --git a/micras_nav/include/micras/nav/look_at_point.hpp b/micras_nav/include/micras/nav/look_at_point.hpp index 875df29..53f2e4b 100644 --- a/micras_nav/include/micras/nav/look_at_point.hpp +++ b/micras_nav/include/micras/nav/look_at_point.hpp @@ -1,9 +1,5 @@ /** - * @file look_at_point.hpp - * - * @brief Nav LookAtPoint class declaration - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_LOOK_AT_POINT_HPP diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index cdbbe4a..d2b85df 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -1,9 +1,5 @@ /** - * @file mapping.hpp - * - * @brief Nav Mapping class declaration - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_MAPPING_HPP diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 5709ad6..d56e856 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -1,9 +1,5 @@ /** - * @file maze.hpp - * - * @brief Nav Maze class declaration - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_MAZE_HPP diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 5bbc531..bb8d0a9 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -1,9 +1,5 @@ /** - * @file odometry.hpp - * - * @brief Nav Odometry class declaration - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_ODOMETRY_HPP diff --git a/micras_nav/include/micras/nav/pid_controller.hpp b/micras_nav/include/micras/nav/pid_controller.hpp index 8756353..a056b16 100644 --- a/micras_nav/include/micras/nav/pid_controller.hpp +++ b/micras_nav/include/micras/nav/pid_controller.hpp @@ -1,9 +1,5 @@ /** - * @file pid_controller.hpp - * - * @brief Nav Pid Controller class declaration - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_PID_CONTROLLER_HPP diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index 8fbde92..0f58a41 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -1,9 +1,5 @@ /** - * @file state.hpp - * - * @brief Types for robot state representation - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_POSE_HPP diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index e7a9342..b270519 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -1,9 +1,5 @@ /** - * @file follow_wall.cpp - * - * @brief Nav FollowWall class implementation - * - * @date 10/2024 + * @file */ #include "micras/nav/follow_wall.hpp" diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp index 7c8d403..be46759 100644 --- a/micras_nav/src/go_to_point.cpp +++ b/micras_nav/src/go_to_point.cpp @@ -1,9 +1,5 @@ /** - * @file go_to_point.cpp - * - * @brief Nav GoToPoint class implementation - * - * @date 10/2024 + * @file */ #include diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index 30205e9..dae0f78 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -1,9 +1,5 @@ /** - * @file grid_pose.cpp - * - * @brief Types for grid-based navigation - * - * @date 10/2024 + * @file */ #include diff --git a/micras_nav/src/look_at_point.cpp b/micras_nav/src/look_at_point.cpp index 027b735..77fcb29 100644 --- a/micras_nav/src/look_at_point.cpp +++ b/micras_nav/src/look_at_point.cpp @@ -1,9 +1,5 @@ /** - * @file look_at_point.cpp - * - * @brief Nav LookAtPoint class implementation - * - * @date 10/2024 + * @file */ #include "micras/core/utils.hpp" diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 184d37c..d9f9f3c 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -1,9 +1,5 @@ /** - * @file mapping.cpp - * - * @brief Nav Mapping class implementation - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_MAPPING_CPP diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index f291603..73edc76 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -1,9 +1,5 @@ /** - * @file maze.cpp - * - * @brief Nav Maze class implementation - * - * @date 10/2024 + * @file */ #ifndef MICRAS_NAV_MAZE_CPP diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 2e38849..bdd71ca 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -1,9 +1,5 @@ /** - * @file odometry.cpp - * - * @brief Nav Odometry class implementation - * - * @date 04/2024 + * @file */ #include diff --git a/micras_nav/src/pid_controller.cpp b/micras_nav/src/pid_controller.cpp index c67d26c..ae8e60a 100644 --- a/micras_nav/src/pid_controller.cpp +++ b/micras_nav/src/pid_controller.cpp @@ -1,9 +1,5 @@ /** - * @file pid_controller.cpp - * - * @brief Nav Pid Controller class implementation - * - * @date 10/2024 + * @file */ #include diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index b509d9b..62fce84 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -1,9 +1,5 @@ /** - * @file state.cpp - * - * @brief Types for robot state representation - * - * @date 10/2024 + * @file */ #include diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index 9cafdd0..f4f1fbf 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -1,9 +1,5 @@ /** - * @file argb.hpp - * - * @brief Proxy Argb class declaration - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_ARGB_HPP diff --git a/micras_proxy/include/micras/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp index 1eabdb9..2c931e5 100644 --- a/micras_proxy/include/micras/proxy/battery.hpp +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -1,9 +1,5 @@ /** - * @file battery.hpp - * - * @brief Proxy Battery class declaration - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_BATTERY_HPP diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index fb24178..e3440d6 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -1,9 +1,5 @@ /** - * @file button.hpp - * - * @brief Proxy Button class header - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_BUTTON_HPP diff --git a/micras_proxy/include/micras/proxy/buzzer.hpp b/micras_proxy/include/micras/proxy/buzzer.hpp index 08cad67..a498d60 100644 --- a/micras_proxy/include/micras/proxy/buzzer.hpp +++ b/micras_proxy/include/micras/proxy/buzzer.hpp @@ -1,9 +1,5 @@ /** - * @file buzzer.hpp - * - * @brief Proxy Buzzer class declaration - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_BUZZER_HPP diff --git a/micras_proxy/include/micras/proxy/dip_switch.hpp b/micras_proxy/include/micras/proxy/dip_switch.hpp index db3f724..d370b5e 100644 --- a/micras_proxy/include/micras/proxy/dip_switch.hpp +++ b/micras_proxy/include/micras/proxy/dip_switch.hpp @@ -1,9 +1,5 @@ /** - * @file dip_switch.hpp - * - * @brief Proxy Dip Switch class header - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_DIP_SWITCH_HPP diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index d9ce2ad..277587d 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -1,9 +1,5 @@ /** - * @file fan.hpp - * - * @brief Proxy Fan class declaration - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_FAN_HPP diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index bb1cf77..21c7950 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -1,9 +1,5 @@ /** - * @file imu.hpp - * - * @brief STM32 IMU HAL wrapper - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_IMU_HPP diff --git a/micras_proxy/include/micras/proxy/led.hpp b/micras_proxy/include/micras/proxy/led.hpp index 03c3664..868b9cc 100644 --- a/micras_proxy/include/micras/proxy/led.hpp +++ b/micras_proxy/include/micras/proxy/led.hpp @@ -1,9 +1,5 @@ /** - * @file led.hpp - * - * @brief Proxy Led class header - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_LED_HPP diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index 8c3f155..ec99675 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -1,9 +1,5 @@ /** - * @file locomotion.hpp - * - * @brief Proxy Locomotion class declaration - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_LOCOMOTION_HPP diff --git a/micras_proxy/include/micras/proxy/motor.hpp b/micras_proxy/include/micras/proxy/motor.hpp index ac29361..34e93f1 100644 --- a/micras_proxy/include/micras/proxy/motor.hpp +++ b/micras_proxy/include/micras/proxy/motor.hpp @@ -1,9 +1,5 @@ /** - * @file motor.hpp - * - * @brief Proxy Motor class declaration - * - * @date 10/2024 + * @file */ #ifndef MICRAS_PROXY_MOTOR_HPP diff --git a/micras_proxy/include/micras/proxy/rotary_sensor.hpp b/micras_proxy/include/micras/proxy/rotary_sensor.hpp index ac78214..3aaee90 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor.hpp @@ -1,9 +1,5 @@ /** - * @file rotary_sensor.hpp - * - * @brief STM32 rotary sensor HAL wrapper - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_ROTARY_SENSOR_HPP diff --git a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp index d1b1947..3ce466a 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp @@ -1,10 +1,5 @@ - /** - * @file rotary_sensor_reg.hpp - * - * @brief AS5047U rotary sensor registers definition - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_ROTARY_SENSOR_REG_HPP diff --git a/micras_proxy/include/micras/proxy/serializable_interface.hpp b/micras_proxy/include/micras/proxy/serializable_interface.hpp index 889f77f..e94c7c5 100644 --- a/micras_proxy/include/micras/proxy/serializable_interface.hpp +++ b/micras_proxy/include/micras/proxy/serializable_interface.hpp @@ -1,9 +1,5 @@ /** - * @file serializable_interface.hpp - * - * @brief Serializable interface for all classes that need to be serialized - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index b70e27e..d147ad9 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -1,9 +1,5 @@ /** - * @file storage.hpp - * - * @brief Proxy Storage class declaration - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_STORAGE_HPP diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 02c4b04..a70149a 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -1,9 +1,5 @@ /** - * @file torque_sensors.hpp - * - * @brief Proxy TorqueSensors class header - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_TORQUE_SENSORS_HPP diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 2a3e484..4fe05e2 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -1,9 +1,5 @@ /** - * @file wall_sensors.hpp - * - * @brief Proxy WallSensors class header - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_WALL_SENSORS_HPP diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index 80e7486..04fc410 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -1,9 +1,5 @@ /** - * @file argb.cpp - * - * @brief Proxy Argb class implementation - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_ARGB_CPP diff --git a/micras_proxy/src/battery.cpp b/micras_proxy/src/battery.cpp index c0a27ef..0e42075 100644 --- a/micras_proxy/src/battery.cpp +++ b/micras_proxy/src/battery.cpp @@ -1,9 +1,5 @@ /** - * @file battery.cpp - * - * @brief Proxy Battery class implementation - * - * @date 03/2024 + * @file */ #include "micras/core/utils.hpp" diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp index 30d6c46..3d4da1c 100644 --- a/micras_proxy/src/button.cpp +++ b/micras_proxy/src/button.cpp @@ -1,9 +1,5 @@ /** - * @file button.cpp - * - * @brief Proxy Button class source - * - * @date 03/2024 + * @file */ #include "micras/proxy/button.hpp" diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp index 2388c7c..8e330ea 100644 --- a/micras_proxy/src/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -1,9 +1,5 @@ /** - * @file buzzer.cpp - * - * @brief Proxy Buzzer class implementation - * - * @date 03/2024 + * @file */ #include "micras/proxy/buzzer.hpp" diff --git a/micras_proxy/src/dip_switch.cpp b/micras_proxy/src/dip_switch.cpp index fe78da7..d42c47b 100644 --- a/micras_proxy/src/dip_switch.cpp +++ b/micras_proxy/src/dip_switch.cpp @@ -1,9 +1,5 @@ /** - * @file dip_switch.cpp - * - * @brief Proxy DIP Switch class source - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_DIP_SWITCH_CPP diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp index 6a16293..34bc3df 100644 --- a/micras_proxy/src/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -1,9 +1,5 @@ /** - * @file fan.cpp - * - * @brief Proxy Fan class source - * - * @date 03/2024 + * @file */ #include "micras/core/utils.hpp" diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 673922c..3f6e2b5 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -1,9 +1,5 @@ /** - * @file imu.cpp - * - * @brief Proxy Imu class source - * - * @date 03/2024 + * @file */ #include diff --git a/micras_proxy/src/led.cpp b/micras_proxy/src/led.cpp index a40d2ae..62fcbdd 100644 --- a/micras_proxy/src/led.cpp +++ b/micras_proxy/src/led.cpp @@ -1,9 +1,5 @@ /** - * @file led.cpp - * - * @brief Proxy Led class source - * - * @date 03/2024 + * @file */ #include "micras/proxy/led.hpp" diff --git a/micras_proxy/src/locomotion.cpp b/micras_proxy/src/locomotion.cpp index a5bf2d2..b828a5a 100644 --- a/micras_proxy/src/locomotion.cpp +++ b/micras_proxy/src/locomotion.cpp @@ -1,9 +1,5 @@ /** - * @file locomotion.cpp - * - * @brief Proxy Locomotion class source - * - * @date 03/2024 + * @file */ #include diff --git a/micras_proxy/src/motor.cpp b/micras_proxy/src/motor.cpp index 1761d7c..ba1b7b5 100644 --- a/micras_proxy/src/motor.cpp +++ b/micras_proxy/src/motor.cpp @@ -1,9 +1,5 @@ /** - * @file motor.cpp - * - * @brief Proxy Motor class source - * - * @date 10/2024 + * @file */ #include diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index f6f9a7f..eb7eaa7 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -1,9 +1,5 @@ /** - * @file rotary_sensor.cpp - * - * @brief Proxy RotarySensor class source - * - * @date 03/2024 + * @file */ #include diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 6b7c38c..4c9232c 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -1,9 +1,5 @@ /** - * @file storage.cpp - * - * @brief Proxy Storage class source - * - * @date 03/2024 + * @file */ #include diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index 3f9b82d..80cb9c4 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -1,9 +1,5 @@ /** - * @file torque_sensors.cpp - * - * @brief Proxy TorqueSensors class implementation - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_TORQUE_SENSORS_CPP diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index e3f7532..1c3ee9a 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -1,9 +1,5 @@ /** - * @file wall_sensor.cpp - * - * @brief Proxy WallSensors class implementation - * - * @date 03/2024 + * @file */ #ifndef MICRAS_PROXY_WALL_SENSORS_CPP From d83d82e4c0262064884b5c558beeffafad952ad9 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 5 Dec 2024 20:03:56 -0300 Subject: [PATCH 243/561] =?UTF-8?q?=E2=9C=A8=20Add=20set=5Fcolors=20functi?= =?UTF-8?q?on=20to=20argb=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/argb.hpp | 7 +++++++ micras_proxy/src/argb.cpp | 13 +++++++++++++ 2 files changed, 20 insertions(+) diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index f4f1fbf..c6e5d13 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -66,6 +66,13 @@ class Argb { */ void set_color(const Color& color); + /** + * @brief Set the colors of all ARGBs + * + * @param colors The colors to set the ARGBs to + */ + void set_colors(const std::array& colors); + /** * @brief Turn off the ARGB at the specified index * diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index 04fc410..885ef43 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -40,6 +40,19 @@ void Argb::set_color(const Color& color) { this->pwm.start_dma(this->buffer); } +template +void Argb::set_colors(const std::array& colors) { + if (this->pwm.is_busy()) { + return; + } + + for (uint8_t i = 0; i < num_of_leds; i++) { + this->encode_color(colors[i] * this->brightness, i); + } + + this->pwm.start_dma(this->buffer); +} + template void Argb::turn_off(uint8_t index) { this->set_color(index, {0, 0, 0}); From ae5c0dd74172ae612ba40d0aa788c24c685e5bfa Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 5 Dec 2024 21:51:54 -0300 Subject: [PATCH 244/561] =?UTF-8?q?=E2=9C=A8=20Add=20transition=20function?= =?UTF-8?q?=20to=20utils?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/utils.hpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index fef7ff2..ba32cd3 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -42,16 +42,18 @@ constexpr T move_towards(T value, T target, T step) { } /** - * @brief Decay a value towards zero with a damping factor + * @brief Varies a value from start to end smoothly * * @tparam T Type of the value * @param value Current value - * @param damping Damping factor + * @param resistance Value resistance to variation + * @param start Start value + * @param end End value * @return T New value */ template -constexpr T decay(T value, T damping) { - return damping / (damping + value * value); +constexpr T transition(T value, T start, T end, T resistance) { + return end - (end - start) * resistance / (resistance + value * value); } /** From 9a1c6d65c363ea4dfd2fee389d2dcda35106d8ce Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 5 Dec 2024 21:54:39 -0300 Subject: [PATCH 245/561] =?UTF-8?q?=E2=9C=A8=20Add=20calibration=20functio?= =?UTF-8?q?n=20to=20torque=20sensors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/torque_sensors.hpp | 5 +++++ micras_proxy/src/torque_sensors.cpp | 8 +++----- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index a70149a..3bcbae9 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -35,6 +35,11 @@ class TorqueSensors { */ explicit TorqueSensors(const Config& config); + /** + * @brief Calibrate the torque sensors + */ + void calibrate(); + /** * @brief Update the torque sensors readings */ diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index 80cb9c4..6bca6ab 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -17,12 +17,10 @@ TorqueSensors::TorqueSensors(const Config& config) : max_torque{config.max_torque}, filters{core::make_array(config.filter_cutoff)} { this->adc.start_dma(this->buffer); +} - for (uint8_t i = 0; i < 50; i++) { - hal::Timer::sleep_ms(2); - this->update(); - } - +template +void TorqueSensors::calibrate() { for (uint8_t i = 0; i < num_of_sensors; i++) { this->base_reading[i] = this->filters[i].get_last(); } From bb6c4d3a94252a851b0049873cfd22330c952f73 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 5 Dec 2024 22:28:59 -0300 Subject: [PATCH 246/561] =?UTF-8?q?=F0=9F=92=A1=20Fix=20motor=20proxy=20cl?= =?UTF-8?q?ass=20comment?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/motor.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/micras_proxy/include/micras/proxy/motor.hpp b/micras_proxy/include/micras/proxy/motor.hpp index 34e93f1..8bf7117 100644 --- a/micras_proxy/include/micras/proxy/motor.hpp +++ b/micras_proxy/include/micras/proxy/motor.hpp @@ -7,10 +7,10 @@ #include "micras/hal/pwm.hpp" -namespace micras::proxy { /** - * @brief Class for controlling a motor driver - */ - +namespace micras::proxy { +/** + * @brief Class for controlling a motor driver + */ class Motor { public: /** From b84b630def1da9a3f3d791777773a30f63a85c91 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 5 Dec 2024 22:29:59 -0300 Subject: [PATCH 247/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20rotary=20sensor=20?= =?UTF-8?q?position=20calculation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/rotary_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index eb7eaa7..a00e178 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -43,7 +43,7 @@ RotarySensor::RotarySensor(const Config& config) : } float RotarySensor::get_position() const { - return encoder.get_counter() * 4.0F * std::numbers::pi_v / this->resolution; + return encoder.get_counter() * 2.0F * std::numbers::pi_v / this->resolution; } uint16_t RotarySensor::read_register(uint16_t address) { From 49a91f14a192f48c19f7e3d7a9a72c37f5fe3c1d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 5 Dec 2024 22:31:29 -0300 Subject: [PATCH 248/561] =?UTF-8?q?=E2=9C=A8=20Add=20calibration=20functio?= =?UTF-8?q?n=20to=20imu=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/imu.hpp | 32 +++++++++++------------ micras_proxy/src/imu.cpp | 10 ++++++- 2 files changed, 25 insertions(+), 17 deletions(-) diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 21c7950..19d1c07 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -10,6 +10,7 @@ #include #include +#include "micras/core/butterworth_filter.hpp" #include "micras/hal/spi.hpp" namespace micras::proxy { @@ -75,6 +76,11 @@ class Imu { */ float get_linear_acceleration(Axis axis) const; + /** + * @brief Define the base reading to be removed from the IMU value + */ + void calibrate(); + private: /** * @brief Read data from the IMU @@ -100,22 +106,6 @@ class Imu { */ static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len); - /** - * @brief Function to convert raw data to orientation - * - * @param sflp Raw data from the IMU - */ - void update_orientation(const uint16_t sflp[3]); // NOLINT(*-avoid-c-arrays) - - /** - * @brief Function to convert half precision float to single precision float - * - * @param n Half precision float - * - * @return float Single precision float - */ - static float half_to_float(uint16_t x); - /** * @brief Conversion constants */ @@ -151,6 +141,16 @@ class Imu { * @brief Accelerometer conversion factor */ float xl_factor; + + /** + * @brief Gyroscope Butterworth filter for the calibration + */ + core::ButterworthFilter calibration_filter{5.0F}; + + /** + * @brief Flag to check if the IMU is calibrated + */ + bool calibrated{}; }; } // namespace micras::proxy diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 3f6e2b5..d9e5a0e 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -76,6 +76,10 @@ void Imu::update() { this->angular_velocity[1] = raw_data[1] * gy_factor; this->angular_velocity[2] = raw_data[2] * gy_factor; } + + if (not this->calibrated) { + this->calibration_filter.update(this->angular_velocity[2]); + } } float Imu::get_angular_velocity(Axis axis) const { @@ -87,7 +91,7 @@ float Imu::get_angular_velocity(Axis axis) const { return this->angular_velocity[1]; case Axis::Z: - return this->angular_velocity[2]; + return this->angular_velocity[2] - this->calibration_filter.get_last(); default: return 0.0F; @@ -134,4 +138,8 @@ int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint return 0; } + +void Imu::calibrate() { + this->calibrated = true; +} } // namespace micras::proxy From 9a8c96ef6882db506f2da6442c177b2d8e628d04 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 5 Dec 2024 22:32:51 -0300 Subject: [PATCH 249/561] =?UTF-8?q?=E2=9C=A8=20Add=20Objective=20type?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/types.hpp | 9 ++++++++- micras_nav/include/micras/nav/grid_pose.hpp | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index fb69e08..c456f06 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -25,7 +25,14 @@ enum FollowWallType : uint8_t { FRONT = 1, LEFT = 2, RIGHT = 3, - PARALLEL = 4 + PARALLEL = 4, + BACK = 5, +}; + +enum Objective : uint8_t { + EXPLORE = 0, + RETURN = 1, + SOLVE = 2 }; } // namespace micras::core diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index 3634678..6139054 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -12,7 +12,7 @@ namespace micras::nav { /** - * @brief Possible directions in the grid + * @brief Possible sides in the grid */ enum Side : uint8_t { RIGHT = 0, From dab669b93685fb3fbda7c20d47c11434091eea2c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 5 Dec 2024 22:33:35 -0300 Subject: [PATCH 250/561] =?UTF-8?q?=E2=9C=A8=20Add=20Direction=20type=20an?= =?UTF-8?q?d=20move=5Ftowards=20function?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/state.hpp | 25 ++++++++++++++++++++++++- micras_nav/src/state.cpp | 25 ++++++++++++++++++++----- 2 files changed, 44 insertions(+), 6 deletions(-) diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index 0f58a41..6220aac 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -8,6 +8,20 @@ #include "micras/nav/grid_pose.hpp" namespace micras::nav { +/** + * @brief Possible directions in the maze + */ +enum Direction : uint8_t { + EAST = 0, + NORTHEAST = 1, + NORTH = 2, + NORTHWEST = 3, + WEST = 4, + SOUTHWEST = 5, + SOUTH = 6, + SOUTHEAST = 7, +}; + /** * @brief Type to store a point in 2D space */ @@ -42,7 +56,16 @@ struct Point { * @param angle The angle to rotate the point by * @return The rotated point */ - Point rotate(Side angle); + Point rotate(Direction angle); + + /** + * @brief Moves the point towards another point by a given distance + * + * @param other The point to move towards + * @param distance The distance to move + * @return The point after moving towards the other point + */ + Point move_towards(const Point& other, float distance) const; /** * @brief Subtracts a point from another diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 62fce84..52bf837 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -20,21 +20,36 @@ GridPoint Point::to_grid(float cell_size) const { return {static_cast(this->x / cell_size), static_cast(this->y / cell_size)}; } -Point Point::rotate(Side angle) { +Point Point::rotate(Direction angle) { + constexpr float sqrt2_2 = std::numbers::sqrt2_v / 2.0F; + switch (angle) { - case Side::RIGHT: + case Direction::EAST: return {-this->y, this->x}; - case Side::UP: + case Direction::NORTHEAST: + return {sqrt2_2 * (this->x - this->y), sqrt2_2 * (this->x + this->y)}; + case Direction::NORTH: return *this; - case Side::LEFT: + case Direction::NORTHWEST: + return {sqrt2_2 * (this->x + this->y), sqrt2_2 * (-this->x + this->y)}; + case Direction::WEST: return {this->y, -this->x}; - case Side::DOWN: + case Direction::SOUTHWEST: + return {sqrt2_2 * (-this->x + this->y), sqrt2_2 * (-this->x - this->y)}; + case Direction::SOUTH: return {-this->x, -this->y}; + case Direction::SOUTHEAST: + return {sqrt2_2 * (-this->x - this->y), sqrt2_2 * (this->x - this->y)}; } return *this; } +Point Point::move_towards(const Point& other, float distance) const { + float angle = this->angle_between(other); + return {this->x + distance * std::cos(angle), this->y + distance * std::sin(angle)}; +} + Point Point::operator-(const Point& other) const { return {this->x - other.x, this->y - other.y}; } From d36123af59d608aea554b092adb543966e65b638 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 5 Dec 2024 22:35:00 -0300 Subject: [PATCH 251/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Remove=20linear=20?= =?UTF-8?q?pid=20from=20go=5Fto=5Fpoint?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/go_to_point.hpp | 26 +++++++++---------- micras_nav/src/go_to_point.cpp | 25 ++++++++++-------- 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index 5b40473..00bd42c 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -19,12 +19,12 @@ class GoToPoint { * @brief Configuration for the GoToPoint class */ struct Config { - PidController::Config linear_pid; PidController::Config stop_pid; PidController::Config angular_pid; float cell_size{}; - float base_speed{}; - float linear_decay_damping{}; + float min_linear_command{}; + float max_linear_command{}; + float deceleration_factor{}; float distance_tolerance{}; float velocity_tolerance{}; }; @@ -78,11 +78,6 @@ class GoToPoint { void calibrate(); private: - /** - * @brief PID controller for the linear velocity - */ - PidController linear_pid; - /** * @brief PID controller for stoppping at the goal */ @@ -104,14 +99,19 @@ class GoToPoint { float cell_size; /** - * @brief Base linear speed for the robot + * @brief Minimum linear command */ - float base_speed; + float min_linear_command; /** - * @brief Damping factor for the linear decay + * @brief Maximum linear command */ - float linear_decay_damping; + float max_linear_command; + + /** + * @brief Deceleration factor for the linear command transition + */ + float deceleration_factor; /** * @brief Linear tolerance for the goal point @@ -122,8 +122,6 @@ class GoToPoint { * @brief Velocity tolerance at the goal point */ float velocity_tolerance; - - friend class Interface; }; } // namespace micras::nav diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp index be46759..97365fb 100644 --- a/micras_nav/src/go_to_point.cpp +++ b/micras_nav/src/go_to_point.cpp @@ -11,13 +11,13 @@ namespace micras::nav { GoToPoint::GoToPoint( const proxy::WallSensors<4>& wall_sensors, const Config& config, const FollowWall::Config& follow_wall_config ) : - linear_pid(config.linear_pid), stop_pid(config.stop_pid), angular_pid(config.angular_pid), follow_wall{wall_sensors, follow_wall_config}, cell_size{config.cell_size}, - base_speed{config.base_speed}, - linear_decay_damping{config.linear_decay_damping}, + min_linear_command{config.min_linear_command}, + max_linear_command{config.max_linear_command}, + deceleration_factor{config.deceleration_factor}, distance_tolerance{config.distance_tolerance}, velocity_tolerance{config.velocity_tolerance} { } @@ -28,23 +28,27 @@ Twist GoToPoint::action( float angular_command{}; Point goal_distance = goal - state.pose.position; - float linear_target{}; - if (follow_wall_type != core::FollowWallType::NONE) { - linear_target = this->base_speed; angular_command = this->follow_wall.action(follow_wall_type, elapsed_time); } else { float angular_error = core::assert_angle(state.pose.orientation - std::atan2(this->cell_size, goal_distance.x)); - linear_target = core::decay(angular_error, this->linear_decay_damping) * this->base_speed; angular_command = this->angular_pid.update(angular_error, elapsed_time, state.velocity.angular); } - if (stop and (std::abs(goal_distance.y) < this->cell_size / 2.0F)) { - linear_command = this->stop_pid.update(-goal_distance.y, elapsed_time, state.velocity.linear); + if (stop) { + if (std::abs(goal_distance.y) < this->cell_size) { + linear_command = this->stop_pid.update(-goal_distance.y, elapsed_time, state.velocity.linear); + } else { + linear_command = + core::transition(goal_distance.y, 0.0F, this->max_linear_command, this->deceleration_factor); + } } else { - linear_command = this->linear_pid.update(state.velocity.linear - linear_target, elapsed_time); + linear_command = core::transition( + goal_distance.y, this->min_linear_command, this->max_linear_command, this->deceleration_factor + ); } + angular_command *= linear_command / 50.0F; return {linear_command, angular_command}; } @@ -55,7 +59,6 @@ bool GoToPoint::finished(const State& state, const Point& goal, bool stop) const void GoToPoint::reset() { this->follow_wall.reset(); - this->linear_pid.reset(); this->stop_pid.reset(); this->angular_pid.reset(); } From d68f17c70244273be1773411a0ab2bb1899a0235 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 6 Dec 2024 01:00:29 -0300 Subject: [PATCH 252/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20mapping?= =?UTF-8?q?=20and=20control=20logic?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/mapping.hpp | 60 +++++-- micras_nav/include/micras/nav/maze.hpp | 84 ++++++---- micras_nav/src/mapping.cpp | 178 ++++++++++++++++++-- micras_nav/src/maze.cpp | 190 ++++++++++++---------- 4 files changed, 373 insertions(+), 139 deletions(-) diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index d2b85df..ebb1cd5 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -6,9 +6,11 @@ #define MICRAS_NAV_MAPPING_HPP #include +#include #include "micras/nav/maze.hpp" #include "micras/nav/state.hpp" +#include "micras/proxy/serializable_interface.hpp" #include "micras/proxy/wall_sensors.hpp" namespace micras::nav { @@ -19,7 +21,7 @@ namespace micras::nav { * @tparam height The height of the maze */ template -class Mapping { +class Mapping : public proxy::ISerializable { public: /** * @brief Configuration for the Mapping class @@ -27,7 +29,6 @@ class Mapping { struct Config { float wall_thickness{}; float cell_size{}; - float alignment_threshold{}; Pose front_sensor_pose{}; Pose side_sensor_pose{}; float front_distance_alignment_tolerance{}; @@ -62,11 +63,14 @@ class Mapping { */ struct Action { /** - * @brief TPossible types of actions + * @brief Possible types of actions */ enum Type : uint8_t { LOOK_AT, - GO_TO + GO_TO, + ALIGN_BACK, + FINISHED, + ERROR, }; /** @@ -82,7 +86,7 @@ class Mapping { /** * @brief The orientation of the goal point */ - Side side; + Direction direction; }; /** @@ -104,9 +108,10 @@ class Mapping { * @brief Get the action to be taken based on the current pose * * @param pose The current pose of the robot + * @param objective The curent objective of the robot * @return The action to be taken */ - Action get_action(const Pose& pose) const; + Action get_action(const Pose& pose, core::Objective objective); /** * @brief Fix the pose based on the mapping information @@ -163,6 +168,34 @@ class Mapping { */ core::FollowWallType get_follow_wall_type(const Pose& pose) const; + /** + * @brief Serialize the best route to the goal + * + * @return The serialized data + */ + std::vector serialize() const override; + + /** + * @brief Deserialize the best route to the goal + * + * @param buffer The serialized data + * @param size The size of the serialized data + */ + void deserialize(const uint8_t* buffer, uint16_t size) override; + + /** + * @brief Check if the robot can align its back with a wall + * + * @param pose The current pose of the robot + * @return True if the robot can align its back, false otherwise + */ + bool can_align_back(const Pose& pose) const; + + /** + * @brief Adds diagonal movements to the best route + */ + void diagonalize_best_route(); + private: /** * @brief Wall sensors of the robot @@ -184,11 +217,6 @@ class Mapping { */ float cell_size; - /** - * @brief Threshold for the robot alignment with the side walls - */ - float alignment_threshold; - /** * @brief Pose of the front wall sensors */ @@ -243,6 +271,16 @@ class Mapping { * @brief Sensor readings when the sides of the robot are distance aligned */ std::array side_distance_reading{}; + + /** + * @brief List of points and directions that represent the best route to the goal + */ + std::list> best_route; + + /** + * @brief Current iterator of the best route + */ + std::list>::iterator best_route_iterator{best_route.begin()}; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index d56e856..cdecccd 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -39,13 +39,20 @@ class Maze { void update(const GridPose& pose, Information information); /** - * @brief Returns the next point the robot should go to + * @brief Returns the next point the robot should go to when exploring * * @param position The current position of the robot - * @param force_costmap Whether to force the use of the update of the costmap - * @return The next point the robot should go to + * @return The next point the robot should go to when exploring */ - GridPose get_current_goal(const GridPoint& position, bool force_costmap = false) const; + GridPose get_current_exploration_goal(const GridPoint& position) const; + + /** + * @brief Returns the next point the robot should go to when returning + * + * @param position The current position of the robot + * @return The next point the robot should go to when returning + */ + GridPose get_current_returning_goal(const GridPoint& position) const; /** * @brief Checks the type of wall following the robot can do @@ -54,7 +61,48 @@ class Maze { * @param front_cell Whether to check the cell at the front of the robot * @return The type of wall following */ - core::FollowWallType get_follow_wall_type(const GridPose& pose, bool front_cell) const; + core::FollowWallType get_follow_wall_type(const GridPose& pose) const; + + /** + * @brief Checks whether the robot has finished the maze + * + * @param position The current position of the robot + * @return True if the robot has finished the maze, false otherwise + */ + bool finished(const GridPoint& position) const; + + /** + * @brief Checks whether the robot has returned to the start + * + * @param position The current position of the robot + * @return True if the robot has returned to the start, false otherwise + */ + bool returned(const GridPoint& position) const; + + /** + * @brief Calculates the best route to the goal using the current costmap + */ + void calculate_best_route(); + + /** + * @brief Improves the route to the goal + */ + void optimize_route(); + + /** + * @brief Returns the best route to the goal + * + * @return The best route to the goal + */ + const std::map>& get_best_route() const; + + /** + * @brief Checks whether there is a wall at the front of a given pose + * + * @param pose The pose to check + * @return True if there is a wall, false otherwise + */ + bool has_wall(const GridPose& pose) const; private: /** @@ -71,11 +119,6 @@ class Maze { */ void calculate_costmap(); - /** - * @brief Improves the route to the goal - */ - void optimize_route(); - /** * @brief Returns the cell at the given position * @@ -97,16 +140,9 @@ class Maze { * * @param pose The pose of the robot * @param wall Whether there is a wall in front of the robot + * @return True if the information is new, false otherwise */ - void update_wall(const GridPose& pose, bool wall); - - /** - * @brief Checks whether there is a wall at the front of a given pose - * - * @param pose The pose to check - * @return True if there is a wall, false otherwise - */ - bool has_wall(const GridPose& pose) const; + bool update_wall(const GridPose& pose, bool wall); /** * @brief Cells matrix representing the maze @@ -123,16 +159,6 @@ class Maze { */ std::unordered_set goal; - /** - * @brief Whether the robot is returning to the start - */ - bool returning{}; - - /** - * @brief Whether the robot is exploring the maze - */ - bool exploring{true}; - /** * @brief Current best found route to the goal */ diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index d9f9f3c..47184f8 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -16,12 +16,11 @@ Mapping::Mapping(const proxy::WallSensors<4>& wall_sensors, Mappi maze{config.start, config.goal}, wall_thickness{config.wall_thickness}, cell_size{config.cell_size}, - alignment_threshold{config.alignment_threshold}, front_sensor_pose{config.front_sensor_pose}, side_sensor_pose{config.side_sensor_pose}, front_sensors_region_division{cell_size - wall_thickness / 2.0F - front_sensor_pose.position.y}, side_sensors_region_division{ - cell_size + wall_thickness / 2 - side_sensor_pose.position.y + + cell_size - side_sensor_pose.position.y + (side_sensor_pose.position.x + (wall_thickness - cell_size) / 2.0F) * std::tan(side_sensor_pose.orientation) }, front_distance_alignment_tolerance{config.front_distance_alignment_tolerance}, @@ -36,7 +35,7 @@ template void Mapping::update(const Pose& pose) { float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); - if (reliability < 60.0F) { + if (reliability < 30.0F) { return; } @@ -53,8 +52,8 @@ void Mapping::update(const Pose& pose) { } } - if (std::abs(cell_position.x) < this->alignment_threshold * this->cell_size and - information.front != core::Observation::WALL and cell_position.y > this->side_sensors_region_division) { + if (information.front != core::Observation::WALL and + cell_position.y > this->side_sensors_region_division + 2 * this->wall_thickness) { information.front_left = this->wall_sensors.get_observation(Sensor::LEFT); information.front_right = this->wall_sensors.get_observation(Sensor::RIGHT); } @@ -63,24 +62,64 @@ void Mapping::update(const Pose& pose) { } template -Mapping::Action Mapping::get_action(const Pose& pose) const { - GridPose current_grid_goal = this->maze.get_current_goal(pose.position.to_grid(this->cell_size)); - Point current_goal = Point::from_grid(current_grid_goal.position, this->cell_size); - Point current_goal_rotated = current_goal.rotate(current_grid_goal.orientation); +Mapping::Action Mapping::get_action(const Pose& pose, core::Objective objective) { + GridPoint grid_position = pose.position.to_grid(this->cell_size); + GridPose current_grid_goal{}; + + Point current_goal{}; + Direction orientation{}; + + switch (objective) { + case core::Objective::EXPLORE: + if (this->maze.finished(grid_position)) { + return {Action::Type::FINISHED, pose.position, Direction::EAST}; + } + + current_grid_goal = this->maze.get_current_exploration_goal(grid_position); + current_goal = Point::from_grid(current_grid_goal.position, this->cell_size); + orientation = static_cast(2 * current_grid_goal.orientation); + break; + case core::Objective::RETURN: + if (this->maze.returned(grid_position)) { + this->maze.optimize_route(); + return {Action::Type::FINISHED, pose.position, Direction::EAST}; + } + + this->maze.calculate_best_route(); + current_grid_goal = this->maze.get_current_returning_goal(grid_position); + current_goal = Point::from_grid(current_grid_goal.position, this->cell_size); + orientation = static_cast(2 * current_grid_goal.orientation); + break; + case core::Objective::SOLVE: + if (this->maze.finished(grid_position)) { + return {Action::Type::FINISHED, pose.position, Direction::EAST}; + } + + this->best_route_iterator++; + current_goal = this->best_route_iterator->first; + orientation = this->best_route_iterator->second; + break; + } + + Point current_goal_rotated = current_goal.rotate(orientation); if (std::abs(core::assert_angle(pose.orientation - pose.position.angle_between(current_goal))) > - std::numbers::pi_v / 8.0F) { - return {Action::Type::LOOK_AT, current_goal_rotated, current_grid_goal.orientation}; + 0.375F * std::numbers::pi_v) { + if (objective == core::Objective::SOLVE) { + this->best_route_iterator--; + } + + return {Action::Type::LOOK_AT, current_goal_rotated, orientation}; } - return {Action::Type::GO_TO, current_goal_rotated, current_grid_goal.orientation}; + return {Action::Type::GO_TO, current_goal_rotated, orientation}; } template Pose Mapping::correct_pose(const Pose& pose, core::FollowWallType follow_wall_type) const { float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); - if (reliability < 50.0F) { + if (reliability < 20.0F) { return pose; } @@ -90,6 +129,12 @@ Pose Mapping::correct_pose(const Pose& pose, core::FollowWallType Side direction = angle_to_grid(pose.orientation); switch (follow_wall_type) { + case core::FollowWallType::BACK: + pose_correction.y = cell_position.y - (0.04F + this->wall_thickness / 2); + corrected_pose.orientation = + core::assert_angle(static_cast(direction) * std::numbers::pi_v / 2.0F); + break; + case core::FollowWallType::FRONT: if (this->is_distance_front_aligned()) { pose_correction.y = cell_position.y - this->cell_size / 2.0F; @@ -103,6 +148,8 @@ Pose Mapping::correct_pose(const Pose& pose, core::FollowWallType case core::FollowWallType::PARALLEL: if (this->is_distance_side_aligned()) { pose_correction.x = cell_position.x; + pose_correction.y = cell_position.y - std::hypot(cell_position.x, cell_position.y); + corrected_pose.orientation = core::assert_angle(static_cast(direction) * std::numbers::pi_v / 2.0F); } else if (this->is_orientation_side_aligned()) { @@ -114,7 +161,7 @@ Pose Mapping::correct_pose(const Pose& pose, core::FollowWallType return pose; } - pose_correction = pose_correction.rotate(static_cast((6 - direction) % 4)); + pose_correction = pose_correction.rotate(static_cast((12 - 2 * direction) % 8)); corrected_pose.position = corrected_pose.position - pose_correction; return corrected_pose; @@ -178,11 +225,108 @@ template core::FollowWallType Mapping::get_follow_wall_type(const Pose& pose) const { nav::Point cell_position = pose.to_cell(cell_size); - if (cell_position.y < this->side_sensors_region_division - this->wall_thickness / 2.0F) { - return this->maze.get_follow_wall_type(pose.to_grid(this->cell_size), false); + core::FollowWallType follow_wall_type = this->maze.get_follow_wall_type(pose.to_grid(this->cell_size)); + + if (follow_wall_type == core::FollowWallType::FRONT and cell_position.y > this->side_sensors_region_division) { + return follow_wall_type; + } + + if (cell_position.y < this->side_sensors_region_division and follow_wall_type != core::FollowWallType::FRONT) { + return follow_wall_type; + } + + bool can_follow_left = this->wall_sensors.get_observation(Sensor::LEFT) == core::Observation::WALL; + bool can_follow_right = this->wall_sensors.get_observation(Sensor::RIGHT) == core::Observation::WALL; + + if (can_follow_left and can_follow_right) { + return core::FollowWallType::PARALLEL; + } + + if (can_follow_left) { + return core::FollowWallType::LEFT; + } + + if (can_follow_right) { + return core::FollowWallType::RIGHT; + } + + return core::FollowWallType::NONE; +} + +template +std::vector Mapping::serialize() const { + std::vector buffer; + buffer.reserve(3 * this->maze.get_best_route().size()); + + for (const auto& [cost, grid_pose] : this->maze.get_best_route()) { + buffer.emplace_back(grid_pose.position.x); + buffer.emplace_back(grid_pose.position.y); + buffer.emplace_back(grid_pose.orientation); + } + + return buffer; +} + +template +void Mapping::deserialize(const uint8_t* buffer, uint16_t size) { + this->best_route.clear(); + + for (uint32_t i = 0; i < size; i += 3) { + this->best_route.emplace_back( + Point::from_grid({buffer[i], buffer[i + 1]}, this->cell_size), static_cast(2 * buffer[i + 2]) + ); + } +} + +template +bool Mapping::can_align_back(const Pose& pose) const { + return this->maze.has_wall(pose.to_grid(this->cell_size).turned_back()); +} + +template +void Mapping::diagonalize_best_route() { + for (auto it = std::next(this->best_route.begin()); it != this->best_route.end(); it++) { + auto next_it = std::next(it); + + if (next_it == this->best_route.end()) { + break; + } + + auto diagonal_direction = static_cast((next_it->second + it->second) / 2); + + if (std::abs(next_it->second - it->second) > 2) { + diagonal_direction = Direction::SOUTHEAST; + } + + this->best_route.insert(it, {it->first.move_towards(std::prev(it)->first, this->cell_size / 2), it->second}); + this->best_route.insert(it, {it->first.move_towards(next_it->first, this->cell_size / 2), diagonal_direction}); + } + + uint8_t pos = 1; + + for (auto it = std::next(this->best_route.begin()); it != this->best_route.end(); pos++) { + if (pos % 3 == 0) { + it = this->best_route.erase(it); + } else { + it++; + } } - return this->maze.get_follow_wall_type(pose.to_grid(this->cell_size), true); + for (auto it = std::next(this->best_route.begin()); it != this->best_route.end();) { + auto next_it = std::next(it); + + if (next_it == this->best_route.end()) { + break; + } + + if (it->second == next_it->second) { + it = this->best_route.erase(it); + } else if (it->first == next_it->first) { + this->best_route.erase(next_it); + } else { + it++; + } + } } } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 73edc76..7809b55 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -27,6 +27,26 @@ Maze::Maze(const GridPose& start, const std::unordered_setget_cell(right_pose.position).wall_count[right_pose.turned_back().orientation] = 0xFFFF; + // Hardcoded walls at the end region + if (width == 16 and height == 16) { + this->cells.at(7).at(7).wall_count[Side::DOWN] = 0xFFFF; + this->cells.at(7).at(7).wall_count[Side::LEFT] = 0xFFFF; + this->cells.at(6).at(7).wall_count[Side::UP] = 0xFFFF; + this->cells.at(7).at(6).wall_count[Side::RIGHT] = 0xFFFF; + this->cells.at(7).at(8).wall_count[Side::DOWN] = 0xFFFF; + this->cells.at(6).at(8).wall_count[Side::UP] = 0xFFFF; + this->cells.at(7).at(8).free_count[Side::RIGHT] = 0xFFFF; + this->cells.at(7).at(9).free_count[Side::LEFT] = 0xFFFF; + this->cells.at(8).at(7).wall_count[Side::LEFT] = 0xFFFF; + this->cells.at(8).at(7).wall_count[Side::UP] = 0xFFFF; + this->cells.at(8).at(6).wall_count[Side::RIGHT] = 0xFFFF; + this->cells.at(9).at(7).wall_count[Side::DOWN] = 0xFFFF; + this->cells.at(8).at(8).wall_count[Side::UP] = 0xFFFF; + this->cells.at(8).at(8).wall_count[Side::RIGHT] = 0xFFFF; + this->cells.at(9).at(8).wall_count[Side::DOWN] = 0xFFFF; + this->cells.at(8).at(9).wall_count[Side::LEFT] = 0xFFFF; + } + for (const auto& position : this->goal) { this->get_cell(position).cost = 0; } @@ -36,56 +56,38 @@ Maze::Maze(const GridPose& start, const std::unordered_set void Maze::update(const GridPose& pose, Information information) { - if (this->goal.contains(pose.position)) { - this->returning = true; - } else if (pose == start.turned_back()) { - this->exploring = false; - this->returning = false; - this->optimize_route(); - } - - if (not this->exploring) { - return; - } + bool new_information{}; if (information.left != core::Observation::UNKNOWN) { - this->update_wall(pose.turned_left(), information.left == core::Observation::WALL); + new_information |= this->update_wall(pose.turned_left(), information.left == core::Observation::WALL); } if (information.front_left != core::Observation::UNKNOWN) { - this->update_wall(pose.front().turned_left(), information.front_left == core::Observation::WALL); + new_information |= + this->update_wall(pose.front().turned_left(), information.front_left == core::Observation::WALL); } if (information.front != core::Observation::UNKNOWN) { - this->update_wall(pose, information.front == core::Observation::WALL); + new_information |= this->update_wall(pose, information.front == core::Observation::WALL); } if (information.front_right != core::Observation::UNKNOWN) { - this->update_wall(pose.front().turned_right(), information.front_right == core::Observation::WALL); + new_information |= + this->update_wall(pose.front().turned_right(), information.front_right == core::Observation::WALL); } if (information.right != core::Observation::UNKNOWN) { - this->update_wall(pose.turned_right(), information.right == core::Observation::WALL); + new_information |= this->update_wall(pose.turned_right(), information.right == core::Observation::WALL); } - this->calculate_costmap(); + if (new_information) { + this->calculate_costmap(); + } } template -GridPose Maze::get_current_goal(const GridPoint& position, bool force_costmap) const { - uint16_t current_cost = this->get_cell(position).cost; - - if (not force_costmap and (not this->exploring or this->returning) and this->best_route.contains(current_cost) and - this->best_route.at(current_cost).position == position) { - auto current = this->best_route.find(current_cost); - - if (this->returning) { - return {std::prev(current)->second.position, current->second.turned_back().orientation}; - } - - return std::next(current)->second; - } - +GridPose Maze::get_current_exploration_goal(const GridPoint& position) const { + uint16_t current_cost = this->get_cell(position).cost; GridPoint next_position = position; Side next_side{}; @@ -93,7 +95,7 @@ GridPose Maze::get_current_goal(const GridPoint& position, bool f Side side = static_cast(i); GridPoint front_position = position + side; - if (not this->has_wall({position, side}) and this->get_cell(front_position).cost <= current_cost) { + if (not this->has_wall({position, side}) and this->get_cell(front_position).cost < current_cost) { current_cost = this->get_cell(front_position).cost; next_position = front_position; next_side = side; @@ -103,6 +105,18 @@ GridPose Maze::get_current_goal(const GridPoint& position, bool f return {next_position, next_side}; } +template +GridPose Maze::get_current_returning_goal(const GridPoint& position) const { + uint16_t current_cost = this->get_cell(position).cost; + + if (this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { + auto current = this->best_route.find(current_cost); + return {std::prev(current)->second.position, current->second.turned_back().orientation}; + } + + return get_current_exploration_goal(position); +} + template const Maze::Cell& Maze::get_cell(const GridPoint& position) const { return this->cells.at(position.y).at(position.x); @@ -114,28 +128,38 @@ Maze::Cell& Maze::get_cell(const GridPoint& positi } template -void Maze::update_wall(const GridPose& pose, bool wall) { +bool Maze::update_wall(const GridPose& pose, bool wall) { if (pose.position.x >= width or pose.position.y >= height) { - return; + return false; } + bool new_information = this->has_wall(pose); + if (wall) { this->get_cell(pose.position).wall_count[pose.orientation]++; } else { this->get_cell(pose.position).free_count[pose.orientation]++; } + new_information ^= this->has_wall(pose); + GridPose front_pose = pose.front(); if (front_pose.position.x >= width or front_pose.position.y >= height) { - return; + return new_information; } + bool new_front_information = this->has_wall(front_pose); + if (wall) { this->get_cell(front_pose.position).wall_count[pose.turned_back().orientation]++; } else { this->get_cell(front_pose.position).free_count[pose.turned_back().orientation]++; } + + new_front_information ^= this->has_wall(front_pose); + + return new_information or new_front_information; } template @@ -145,42 +169,73 @@ bool Maze::has_wall(const GridPose& pose) const { } template -core::FollowWallType Maze::get_follow_wall_type(const GridPose& pose, bool front_cell) const { +core::FollowWallType Maze::get_follow_wall_type(const GridPose& pose) const { if (pose.position.x >= width or pose.position.y >= height) { return core::FollowWallType::NONE; } - GridPose check_pose{}; - - if (front_cell) { - if (this->has_wall(pose)) { - return core::FollowWallType::FRONT; - } - - check_pose = pose.front(); - - if (check_pose.position.x >= width or check_pose.position.y >= height) { - return core::FollowWallType::NONE; - } - } else { - check_pose = pose; + if (this->has_wall(pose)) { + return core::FollowWallType::FRONT; } - if (this->has_wall(check_pose.turned_left()) and this->has_wall(check_pose.turned_right())) { + if (this->has_wall(pose.turned_left()) and this->has_wall(pose.turned_right())) { return core::FollowWallType::PARALLEL; } - if (this->has_wall(check_pose.turned_left())) { + if (this->has_wall(pose.turned_left())) { return core::FollowWallType::LEFT; } - if (this->has_wall(check_pose.turned_right())) { + if (this->has_wall(pose.turned_right())) { return core::FollowWallType::RIGHT; } return core::FollowWallType::NONE; } +template +bool Maze::finished(const GridPoint& position) const { + return this->goal.contains(position); +} + +template +bool Maze::returned(const GridPoint& position) const { + return this->start.position == position; +} + +template +void Maze::calculate_best_route() { + GridPose current_pose = this->start; + this->best_route.clear(); + this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start); + + while (not this->goal.contains(current_pose.position)) { + current_pose = this->get_current_exploration_goal(current_pose.position); + this->best_route.try_emplace(this->get_cell(current_pose.position).cost, current_pose); + } +} + +template +void Maze::optimize_route() { + for (auto it = std::next(this->best_route.begin()); it != this->best_route.end();) { + auto next = std::next(it); + + if (next != this->best_route.end()) { + if (it->second.orientation == next->second.orientation) { + it = this->best_route.erase(it); + continue; + } + } + + it++; + } +} + +template +const std::map>& Maze::get_best_route() const { + return this->best_route; +} + template void Maze::calculate_costmap() { std::array, height> visited{}; @@ -216,35 +271,6 @@ void Maze::calculate_costmap() { } } } - - if (not this->returning) { - return; - } - - GridPose current_pose = this->start; - this->best_route.clear(); - this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start); - - while (not this->goal.contains(current_pose.position)) { - current_pose = this->get_current_goal(current_pose.position, true); - this->best_route.try_emplace(this->get_cell(current_pose.position).cost, current_pose); - } -} - -template -void Maze::optimize_route() { - for (auto it = std::next(this->best_route.begin()); it != this->best_route.end();) { - auto next = std::next(it); - - if (next != this->best_route.end()) { - if (it->second.orientation == next->second.orientation) { - it = this->best_route.erase(it); - continue; - } - } - - it++; - } } } // namespace micras::nav From bb896ed54d584585a031cfef83b40eadae503993 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 8 Dec 2024 23:31:51 -0300 Subject: [PATCH 253/561] =?UTF-8?q?=F0=9F=93=9D=20Fix=20doxygen=20document?= =?UTF-8?q?ation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/types.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index c456f06..ce432e3 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -18,7 +18,7 @@ enum Observation : uint8_t { }; /** - * @brief Type of wall following the robot is able ot do + * @brief Type of wall following the robot is able to do */ enum FollowWallType : uint8_t { NONE = 0, From 4a87fc3f2bf11d2cdce0b94443a5f28cef10fbdb Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 27 Dec 2024 20:10:09 -0300 Subject: [PATCH 254/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Add=20T=20prefix?= =?UTF-8?q?=20to=20template=20classes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 4 +-- micras_nav/include/micras/nav/go_to_point.hpp | 4 +-- micras_nav/include/micras/nav/mapping.hpp | 10 +++--- micras_nav/include/micras/nav/maze.hpp | 4 +-- micras_nav/src/follow_wall.cpp | 2 +- micras_nav/src/go_to_point.cpp | 2 +- micras_nav/src/mapping.cpp | 30 +++++++++--------- micras_nav/src/maze.cpp | 31 ++++++++++--------- micras_proxy/include/micras/proxy/argb.hpp | 4 +-- .../include/micras/proxy/dip_switch.hpp | 4 +-- ...lizable_interface.hpp => serializable.hpp} | 6 ++-- micras_proxy/include/micras/proxy/storage.hpp | 2 +- .../include/micras/proxy/torque_sensors.hpp | 4 +-- .../include/micras/proxy/wall_sensors.hpp | 4 +-- micras_proxy/src/argb.cpp | 14 ++++----- micras_proxy/src/dip_switch.cpp | 6 ++-- micras_proxy/src/torque_sensors.cpp | 16 +++++----- micras_proxy/src/wall_sensors.cpp | 28 ++++++++--------- 18 files changed, 88 insertions(+), 87 deletions(-) rename micras_proxy/include/micras/proxy/{serializable_interface.hpp => serializable.hpp} (87%) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 47b73e2..f9e1591 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -31,7 +31,7 @@ class FollowWall { * @param wall_sensors The wall sensors of the robot * @param config The configuration for the FollowWall class */ - FollowWall(const proxy::WallSensors<4>& wall_sensors, const Config& config); + FollowWall(const proxy::TWallSensors<4>& wall_sensors, const Config& config); /** * @brief Update the PID controller and return the response @@ -71,7 +71,7 @@ class FollowWall { /** * @brief Wall sensors of the robot */ - const proxy::WallSensors<4>& wall_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + const proxy::TWallSensors<4>& wall_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) /** * @brief PID controller for the wall following diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index 00bd42c..dfefe60 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -36,8 +36,8 @@ class GoToPoint { * @param config The configuration for the GoToPoint class * @param follow_wall_config The configuration for the FollowWall class */ - explicit GoToPoint( - const proxy::WallSensors<4>& wall_sensors, const Config& config, const FollowWall::Config& follow_wall_config + GoToPoint( + const proxy::TWallSensors<4>& wall_sensors, const Config& config, const FollowWall::Config& follow_wall_config ); /** diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index ebb1cd5..a3edb5b 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -10,7 +10,7 @@ #include "micras/nav/maze.hpp" #include "micras/nav/state.hpp" -#include "micras/proxy/serializable_interface.hpp" +#include "micras/proxy/serializable.hpp" #include "micras/proxy/wall_sensors.hpp" namespace micras::nav { @@ -21,7 +21,7 @@ namespace micras::nav { * @tparam height The height of the maze */ template -class Mapping : public proxy::ISerializable { +class TMapping : public proxy::ISerializable { public: /** * @brief Configuration for the Mapping class @@ -95,7 +95,7 @@ class Mapping : public proxy::ISerializable { * @param wall_sensors The wall sensors * @param config The configuration for the mapping */ - Mapping(const proxy::WallSensors<4>& wall_sensors, Config config); + TMapping(const proxy::TWallSensors<4>& wall_sensors, Config config); /** * @brief Update the mapping of the maze using the current pose and wall sensors @@ -200,12 +200,12 @@ class Mapping : public proxy::ISerializable { /** * @brief Wall sensors of the robot */ - const proxy::WallSensors<4>& wall_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + const proxy::TWallSensors<4>& wall_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) /** * @brief The maze information the robot has */ - Maze maze; + TMaze maze; /** * @brief Thickness of the maze walls diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index cdecccd..8ad1af8 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -20,7 +20,7 @@ namespace micras::nav { * @tparam height The height of the maze */ template -class Maze { +class TMaze { public: /** * @brief Construct a new Maze object @@ -28,7 +28,7 @@ class Maze { * @param start The start pose of the robot * @param goal The goal points in the maze */ - Maze(const GridPose& start, const std::unordered_set& goal); + TMaze(const GridPose& start, const std::unordered_set& goal); /** * @brief Updates the maze walls with the current pose and new information diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index b270519..54b6ed7 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -5,7 +5,7 @@ #include "micras/nav/follow_wall.hpp" namespace micras::nav { -FollowWall::FollowWall(const proxy::WallSensors<4>& wall_sensors, const Config& config) : +FollowWall::FollowWall(const proxy::TWallSensors<4>& wall_sensors, const Config& config) : wall_sensors{wall_sensors}, pid{config.pid}, filter{config.cutoff_frequency}, diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp index 97365fb..2f3d924 100644 --- a/micras_nav/src/go_to_point.cpp +++ b/micras_nav/src/go_to_point.cpp @@ -9,7 +9,7 @@ namespace micras::nav { GoToPoint::GoToPoint( - const proxy::WallSensors<4>& wall_sensors, const Config& config, const FollowWall::Config& follow_wall_config + const proxy::TWallSensors<4>& wall_sensors, const Config& config, const FollowWall::Config& follow_wall_config ) : stop_pid(config.stop_pid), angular_pid(config.angular_pid), diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 47184f8..7c3f79a 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -11,7 +11,7 @@ namespace micras::nav { template -Mapping::Mapping(const proxy::WallSensors<4>& wall_sensors, Mapping::Config config) : +TMapping::TMapping(const proxy::TWallSensors<4>& wall_sensors, TMapping::Config config) : wall_sensors{wall_sensors}, maze{config.start, config.goal}, wall_thickness{config.wall_thickness}, @@ -32,7 +32,7 @@ Mapping::Mapping(const proxy::WallSensors<4>& wall_sensors, Mappi side_distance_reading{config.side_distance_reading} { } template -void Mapping::update(const Pose& pose) { +void TMapping::update(const Pose& pose) { float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); if (reliability < 30.0F) { @@ -62,7 +62,7 @@ void Mapping::update(const Pose& pose) { } template -Mapping::Action Mapping::get_action(const Pose& pose, core::Objective objective) { +TMapping::Action TMapping::get_action(const Pose& pose, core::Objective objective) { GridPoint grid_position = pose.position.to_grid(this->cell_size); GridPose current_grid_goal{}; @@ -116,7 +116,7 @@ Mapping::Action Mapping::get_action(const Pose& po } template -Pose Mapping::correct_pose(const Pose& pose, core::FollowWallType follow_wall_type) const { +Pose TMapping::correct_pose(const Pose& pose, core::FollowWallType follow_wall_type) const { float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); if (reliability < 20.0F) { @@ -168,7 +168,7 @@ Pose Mapping::correct_pose(const Pose& pose, core::FollowWallType } template -void Mapping::calibrate_front() { +void TMapping::calibrate_front() { this->front_distance_reading[0] = this->wall_sensors.get_reading(0); front_orientation_reading[0] = this->wall_sensors.get_reading(1); front_orientation_reading[1] = this->wall_sensors.get_reading(2); @@ -176,13 +176,13 @@ void Mapping::calibrate_front() { } template -void Mapping::calibrate_side() { +void TMapping::calibrate_side() { this->side_distance_reading[0] = this->wall_sensors.get_reading(1); this->side_distance_reading[1] = this->wall_sensors.get_reading(2); } template -bool Mapping::is_distance_front_aligned() const { +bool TMapping::is_distance_front_aligned() const { return core::is_near( this->wall_sensors.get_reading(0), this->front_distance_reading[0], this->front_distance_alignment_tolerance @@ -194,7 +194,7 @@ bool Mapping::is_distance_front_aligned() const { } template -bool Mapping::is_orientation_front_aligned() const { +bool TMapping::is_orientation_front_aligned() const { return std::abs( (this->wall_sensors.get_reading(1) - this->front_orientation_reading[0]) + (this->wall_sensors.get_reading(2) - this->front_orientation_reading[1]) @@ -202,7 +202,7 @@ bool Mapping::is_orientation_front_aligned() const { } template -bool Mapping::is_distance_side_aligned() const { +bool TMapping::is_distance_side_aligned() const { return core::is_near( this->wall_sensors.get_reading(1), this->side_distance_reading[0], this->side_distance_alignment_tolerance @@ -214,7 +214,7 @@ bool Mapping::is_distance_side_aligned() const { } template -bool Mapping::is_orientation_side_aligned() const { +bool TMapping::is_orientation_side_aligned() const { return std::abs( (this->wall_sensors.get_reading(1) - this->side_distance_reading[0]) + (this->wall_sensors.get_reading(2) - this->side_distance_reading[1]) @@ -222,7 +222,7 @@ bool Mapping::is_orientation_side_aligned() const { } template -core::FollowWallType Mapping::get_follow_wall_type(const Pose& pose) const { +core::FollowWallType TMapping::get_follow_wall_type(const Pose& pose) const { nav::Point cell_position = pose.to_cell(cell_size); core::FollowWallType follow_wall_type = this->maze.get_follow_wall_type(pose.to_grid(this->cell_size)); @@ -254,7 +254,7 @@ core::FollowWallType Mapping::get_follow_wall_type(const Pose& po } template -std::vector Mapping::serialize() const { +std::vector TMapping::serialize() const { std::vector buffer; buffer.reserve(3 * this->maze.get_best_route().size()); @@ -268,7 +268,7 @@ std::vector Mapping::serialize() const { } template -void Mapping::deserialize(const uint8_t* buffer, uint16_t size) { +void TMapping::deserialize(const uint8_t* buffer, uint16_t size) { this->best_route.clear(); for (uint32_t i = 0; i < size; i += 3) { @@ -279,12 +279,12 @@ void Mapping::deserialize(const uint8_t* buffer, uint16_t size) { } template -bool Mapping::can_align_back(const Pose& pose) const { +bool TMapping::can_align_back(const Pose& pose) const { return this->maze.has_wall(pose.to_grid(this->cell_size).turned_back()); } template -void Mapping::diagonalize_best_route() { +void TMapping::diagonalize_best_route() { for (auto it = std::next(this->best_route.begin()); it != this->best_route.end(); it++) { auto next_it = std::next(it); diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 7809b55..a852baa 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -12,7 +12,8 @@ namespace micras::nav { template -Maze::Maze(const GridPose& start, const std::unordered_set& goal) : start{start}, goal{goal} { +TMaze::TMaze(const GridPose& start, const std::unordered_set& goal) : + start{start}, goal{goal} { for (uint8_t row = 0; row < height; row++) { this->cells[row][0].wall_count[Side::LEFT] = 0xFFFF; this->cells[row][width - 1].wall_count[Side::RIGHT] = 0xFFFF; @@ -55,7 +56,7 @@ Maze::Maze(const GridPose& start, const std::unordered_set -void Maze::update(const GridPose& pose, Information information) { +void TMaze::update(const GridPose& pose, Information information) { bool new_information{}; if (information.left != core::Observation::UNKNOWN) { @@ -86,7 +87,7 @@ void Maze::update(const GridPose& pose, Information information) } template -GridPose Maze::get_current_exploration_goal(const GridPoint& position) const { +GridPose TMaze::get_current_exploration_goal(const GridPoint& position) const { uint16_t current_cost = this->get_cell(position).cost; GridPoint next_position = position; Side next_side{}; @@ -106,7 +107,7 @@ GridPose Maze::get_current_exploration_goal(const GridPoint& posi } template -GridPose Maze::get_current_returning_goal(const GridPoint& position) const { +GridPose TMaze::get_current_returning_goal(const GridPoint& position) const { uint16_t current_cost = this->get_cell(position).cost; if (this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { @@ -118,17 +119,17 @@ GridPose Maze::get_current_returning_goal(const GridPoint& positi } template -const Maze::Cell& Maze::get_cell(const GridPoint& position) const { +const TMaze::Cell& TMaze::get_cell(const GridPoint& position) const { return this->cells.at(position.y).at(position.x); } template -Maze::Cell& Maze::get_cell(const GridPoint& position) { +TMaze::Cell& TMaze::get_cell(const GridPoint& position) { return this->cells.at(position.y).at(position.x); } template -bool Maze::update_wall(const GridPose& pose, bool wall) { +bool TMaze::update_wall(const GridPose& pose, bool wall) { if (pose.position.x >= width or pose.position.y >= height) { return false; } @@ -163,13 +164,13 @@ bool Maze::update_wall(const GridPose& pose, bool wall) { } template -bool Maze::has_wall(const GridPose& pose) const { +bool TMaze::has_wall(const GridPose& pose) const { return this->get_cell(pose.position).wall_count[pose.orientation] > this->get_cell(pose.position).free_count[pose.orientation]; } template -core::FollowWallType Maze::get_follow_wall_type(const GridPose& pose) const { +core::FollowWallType TMaze::get_follow_wall_type(const GridPose& pose) const { if (pose.position.x >= width or pose.position.y >= height) { return core::FollowWallType::NONE; } @@ -194,17 +195,17 @@ core::FollowWallType Maze::get_follow_wall_type(const GridPose& p } template -bool Maze::finished(const GridPoint& position) const { +bool TMaze::finished(const GridPoint& position) const { return this->goal.contains(position); } template -bool Maze::returned(const GridPoint& position) const { +bool TMaze::returned(const GridPoint& position) const { return this->start.position == position; } template -void Maze::calculate_best_route() { +void TMaze::calculate_best_route() { GridPose current_pose = this->start; this->best_route.clear(); this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start); @@ -216,7 +217,7 @@ void Maze::calculate_best_route() { } template -void Maze::optimize_route() { +void TMaze::optimize_route() { for (auto it = std::next(this->best_route.begin()); it != this->best_route.end();) { auto next = std::next(it); @@ -232,12 +233,12 @@ void Maze::optimize_route() { } template -const std::map>& Maze::get_best_route() const { +const std::map>& TMaze::get_best_route() const { return this->best_route; } template -void Maze::calculate_costmap() { +void TMaze::calculate_costmap() { std::array, height> visited{}; std::array, height> origin{}; std::queue queue; diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index c6e5d13..eab9b52 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -15,7 +15,7 @@ namespace micras::proxy { * @brief Class for controlling an addressable RGB LED */ template -class Argb { +class TArgb { public: /** * @brief Configuration structure for the addressable RGB LED @@ -49,7 +49,7 @@ class Argb { * * @param config Configuration for the addressable RGB LED */ - explicit Argb(const Config& config); + explicit TArgb(const Config& config); /** * @brief Set the color of the ARGB at the specified index diff --git a/micras_proxy/include/micras/proxy/dip_switch.hpp b/micras_proxy/include/micras/proxy/dip_switch.hpp index d370b5e..6c4c2d7 100644 --- a/micras_proxy/include/micras/proxy/dip_switch.hpp +++ b/micras_proxy/include/micras/proxy/dip_switch.hpp @@ -15,7 +15,7 @@ namespace micras::proxy { * @brief Class for acquiring a dip switch data */ template -class DipSwitch { +class TDipSwitch { public: /** * @brief Configuration struct for DipSwitch @@ -29,7 +29,7 @@ class DipSwitch { * * @param config Configuration struct for DipSwitch */ - explicit DipSwitch(const Config& config); + explicit TDipSwitch(const Config& config); /** * @brief Get the state of a switch diff --git a/micras_proxy/include/micras/proxy/serializable_interface.hpp b/micras_proxy/include/micras/proxy/serializable.hpp similarity index 87% rename from micras_proxy/include/micras/proxy/serializable_interface.hpp rename to micras_proxy/include/micras/proxy/serializable.hpp index e94c7c5..a82431c 100644 --- a/micras_proxy/include/micras/proxy/serializable_interface.hpp +++ b/micras_proxy/include/micras/proxy/serializable.hpp @@ -2,8 +2,8 @@ * @file */ -#ifndef MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP -#define MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP +#ifndef MICRAS_PROXY_SERIALIZABLE_HPP +#define MICRAS_PROXY_SERIALIZABLE_HPP #include #include @@ -50,4 +50,4 @@ class ISerializable { }; } // namespace micras::proxy -#endif // MICRAS_PROXY_SERIALIZABLE_INTERFACE_HPP +#endif // MICRAS_PROXY_SERIALIZABLE_HPP diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index d147ad9..60d90ab 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -11,7 +11,7 @@ #include #include -#include "serializable_interface.hpp" +#include "serializable.hpp" namespace micras::proxy { template diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 3bcbae9..3ddb32f 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -16,7 +16,7 @@ namespace micras::proxy { * @brief Class for acquiring torque sensors data */ template -class TorqueSensors { +class TTorqueSensors { public: /** * @brief Configuration structure for torque sensors @@ -33,7 +33,7 @@ class TorqueSensors { * * @param config Configuration for the torque sensors */ - explicit TorqueSensors(const Config& config); + explicit TTorqueSensors(const Config& config); /** * @brief Calibrate the torque sensors diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 4fe05e2..12d2b65 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -18,7 +18,7 @@ namespace micras::proxy { * @brief Class for controlling WallSensors */ template -class WallSensors { +class TWallSensors { public: /** * @brief Configuration structure for wall sensors @@ -38,7 +38,7 @@ class WallSensors { * * @param config Configuration for the wall sensors */ - explicit WallSensors(const Config& config); + explicit TWallSensors(const Config& config); /** * @brief Turn on the wall sensors IR LED diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index 885ef43..cc201ce 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -9,7 +9,7 @@ namespace micras::proxy { template -Argb::Argb(const Config& config) : +TArgb::TArgb(const Config& config) : pwm{config.pwm}, low_bit{pwm.get_compare(config.low_duty_cycle)}, high_bit{pwm.get_compare(config.high_duty_cycle)}, @@ -18,7 +18,7 @@ Argb::Argb(const Config& config) : } template -void Argb::set_color(const Color& color, uint8_t index) { +void TArgb::set_color(const Color& color, uint8_t index) { if (index >= num_of_leds or this->pwm.is_busy()) { return; } @@ -28,7 +28,7 @@ void Argb::set_color(const Color& color, uint8_t index) { } template -void Argb::set_color(const Color& color) { +void TArgb::set_color(const Color& color) { if (this->pwm.is_busy()) { return; } @@ -41,7 +41,7 @@ void Argb::set_color(const Color& color) { } template -void Argb::set_colors(const std::array& colors) { +void TArgb::set_colors(const std::array& colors) { if (this->pwm.is_busy()) { return; } @@ -54,17 +54,17 @@ void Argb::set_colors(const std::array& colors) } template -void Argb::turn_off(uint8_t index) { +void TArgb::turn_off(uint8_t index) { this->set_color(index, {0, 0, 0}); } template -void Argb::turn_off() { +void TArgb::turn_off() { this->set_color({0, 0, 0}); } template -void Argb::encode_color(const Color& color, uint8_t index) { +void TArgb::encode_color(const Color& color, uint8_t index) { uint32_t data = (color.green << (2 * bits_per_color)) | (color.red << bits_per_color) | color.blue; for (uint32_t i = bits_per_led * index, j = bits_per_led - 1; i < bits_per_led * (index + 1U); i++, j--) { diff --git a/micras_proxy/src/dip_switch.cpp b/micras_proxy/src/dip_switch.cpp index d42c47b..977c336 100644 --- a/micras_proxy/src/dip_switch.cpp +++ b/micras_proxy/src/dip_switch.cpp @@ -11,16 +11,16 @@ namespace micras::proxy { template -DipSwitch::DipSwitch(const Config& config) : +TDipSwitch::TDipSwitch(const Config& config) : gpio_array{core::make_array(config.gpio_array)} { } template -bool DipSwitch::get_switch_state(uint8_t switch_index) const { +bool TDipSwitch::get_switch_state(uint8_t switch_index) const { return gpio_array.at(switch_index).read(); } template -uint8_t DipSwitch::get_switches_value() const { +uint8_t TDipSwitch::get_switches_value() const { uint8_t switches_value = 0; for (uint8_t i = 0; i < num_of_sensors; i++) { diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index 6bca6ab..d7f86b2 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -11,7 +11,7 @@ namespace micras::proxy { template -TorqueSensors::TorqueSensors(const Config& config) : +TTorqueSensors::TTorqueSensors(const Config& config) : adc{config.adc}, max_current{hal::AdcDma::reference_voltage / config.shunt_resistor}, max_torque{config.max_torque}, @@ -20,42 +20,42 @@ TorqueSensors::TorqueSensors(const Config& config) : } template -void TorqueSensors::calibrate() { +void TTorqueSensors::calibrate() { for (uint8_t i = 0; i < num_of_sensors; i++) { this->base_reading[i] = this->filters[i].get_last(); } } template -void TorqueSensors::update() { +void TTorqueSensors::update() { for (uint8_t i = 0; i < num_of_sensors; i++) { this->filters[i].update(this->get_adc_reading(i)); } } template -float TorqueSensors::get_torque(uint8_t sensor_index) const { +float TTorqueSensors::get_torque(uint8_t sensor_index) const { return this->filters.at(sensor_index).get_last() * this->max_torque; } template -float TorqueSensors::get_torque_raw(uint8_t sensor_index) const { +float TTorqueSensors::get_torque_raw(uint8_t sensor_index) const { return this->get_adc_reading(sensor_index) * this->max_torque; } template -float TorqueSensors::get_current(uint8_t sensor_index) const { +float TTorqueSensors::get_current(uint8_t sensor_index) const { return this->filters.at(sensor_index).get_last() * this->max_current; } template -float TorqueSensors::get_current_raw(uint8_t sensor_index) const { +float TTorqueSensors::get_current_raw(uint8_t sensor_index) const { return this->get_adc_reading(sensor_index) * this->max_current; ; } template -float TorqueSensors::get_adc_reading(uint8_t sensor_index) const { +float TTorqueSensors::get_adc_reading(uint8_t sensor_index) const { return static_cast(this->buffer.at(sensor_index)) / this->adc.get_max_reading() - this->base_reading.at(sensor_index); } diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index 1c3ee9a..c7f938b 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -10,7 +10,7 @@ namespace micras::proxy { template -WallSensors::WallSensors(const Config& config) : +TWallSensors::TWallSensors(const Config& config) : adc{config.adc}, led_0_pwm{config.led_0_pwm}, led_1_pwm{config.led_1_pwm}, @@ -23,26 +23,26 @@ WallSensors::WallSensors(const Config& config) : } template -void WallSensors::turn_on() { +void TWallSensors::turn_on() { this->led_0_pwm.set_duty_cycle(50.0F); this->led_1_pwm.set_duty_cycle(50.0F); } template -void WallSensors::turn_off() { +void TWallSensors::turn_off() { this->led_0_pwm.set_duty_cycle(0.0F); this->led_1_pwm.set_duty_cycle(0.0F); } template -void WallSensors::update() { +void TWallSensors::update() { for (uint8_t i = 0; i < num_of_sensors; i++) { this->filters[i].update(this->get_adc_reading(i)); } } template -core::Observation WallSensors::get_observation(uint8_t sensor_index) const { +core::Observation TWallSensors::get_observation(uint8_t sensor_index) const { float reading = this->filters.at(sensor_index).get_last(); if (reading > this->wall_threshold.at(sensor_index)) { @@ -57,51 +57,51 @@ core::Observation WallSensors::get_observation(uint8_t sensor_in } template -float WallSensors::get_reading(uint8_t sensor_index) const { +float TWallSensors::get_reading(uint8_t sensor_index) const { return this->filters.at(sensor_index).get_last(); } template -float WallSensors::get_adc_reading(uint8_t sensor_index) const { +float TWallSensors::get_adc_reading(uint8_t sensor_index) const { return static_cast(std::abs(this->buffer.at(sensor_index) - this->buffer.at(sensor_index + num_of_sensors)) ) / this->adc.get_max_reading(); } template -void WallSensors::calibrate_front_wall() { +void TWallSensors::calibrate_front_wall() { this->wall_calibration_measure[0] = this->get_reading(0); this->wall_calibration_measure[3] = this->get_reading(3); } template -void WallSensors::calibrate_left_wall() { +void TWallSensors::calibrate_left_wall() { this->wall_calibration_measure[1] = this->get_reading(1); } template -void WallSensors::calibrate_right_wall() { +void TWallSensors::calibrate_right_wall() { this->wall_calibration_measure[2] = this->get_reading(2); } template -void WallSensors::calibrate_front_free_space() { +void TWallSensors::calibrate_front_free_space() { this->free_space_calibration_measure[0] = this->get_reading(0); this->free_space_calibration_measure[3] = this->get_reading(3); } template -void WallSensors::calibrate_left_free_space() { +void TWallSensors::calibrate_left_free_space() { this->free_space_calibration_measure[1] = this->get_reading(1); } template -void WallSensors::calibrate_right_free_space() { +void TWallSensors::calibrate_right_free_space() { this->free_space_calibration_measure[2] = this->get_reading(2); } template -void WallSensors::update_thresholds() { +void TWallSensors::update_thresholds() { for (uint8_t i = 0; i < num_of_sensors; i++) { float mean_value = (this->wall_calibration_measure.at(i) + this->free_space_calibration_measure.at(i)) / 2.0F; From b6148807fbec122d24fd2b7f52fef5a304dbb239 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 27 Dec 2024 20:10:41 -0300 Subject: [PATCH 255/561] =?UTF-8?q?=E2=9C=A8=20Add=20color=20constants=20t?= =?UTF-8?q?o=20argb?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/argb.hpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index eab9b52..2bbd3cf 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -44,6 +44,17 @@ class TArgb { } }; + /** + * @brief Predefined colors + */ + static constexpr Color red{255, 0, 0}; + static constexpr Color green{0, 255, 0}; + static constexpr Color blue{0, 0, 255}; + static constexpr Color yellow{255, 255, 0}; + static constexpr Color cyan{0, 255, 255}; + static constexpr Color magenta{255, 0, 255}; + static constexpr Color white{255, 255, 255}; + /** * @brief Constructor for the Argb class * From 3fa7ca46d040b3ee667a25f6507bc1ddbe00ed8b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 27 Dec 2024 20:11:16 -0300 Subject: [PATCH 256/561] =?UTF-8?q?=F0=9F=9A=9A=20Rename=20buzzer=20wait?= =?UTF-8?q?=20parameter=20from=20duration=20to=20interval?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/buzzer.hpp | 6 +++--- micras_proxy/src/buzzer.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/micras_proxy/include/micras/proxy/buzzer.hpp b/micras_proxy/include/micras/proxy/buzzer.hpp index a498d60..beec2da 100644 --- a/micras_proxy/include/micras/proxy/buzzer.hpp +++ b/micras_proxy/include/micras/proxy/buzzer.hpp @@ -49,11 +49,11 @@ class Buzzer { void stop(); /** - * @brief Wait for a duration updating the buzzer + * @brief Wait for a time interval updating the buzzer * - * @param duration Duration to wait in ms + * @param interval Time to wait in ms */ - void wait(uint32_t duration); + void wait(uint32_t interval); private: /** diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp index 8e330ea..cd968df 100644 --- a/micras_proxy/src/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -26,14 +26,14 @@ void Buzzer::update() { } } -void Buzzer::wait(uint32_t duration) { +void Buzzer::wait(uint32_t interval) { while (this->duration > 0 and this->is_playing) { this->update(); } hal::Timer wait_timer; - while (wait_timer.elapsed_time_ms() < duration) { + while (wait_timer.elapsed_time_ms() < interval) { this->update(); } } From fa7b7ba552549920b51c465f72346daa48a3c921 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 27 Dec 2024 23:49:02 -0300 Subject: [PATCH 257/561] =?UTF-8?q?=F0=9F=93=9D=20Standardizes=20code=20do?= =?UTF-8?q?cumentation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../micras/core/butterworth_filter.hpp | 65 +++++---- micras_core/include/micras/core/types.hpp | 7 +- micras_core/include/micras/core/utils.hpp | 88 ++++++------- micras_hal/include/micras/hal/adc_dma.hpp | 28 ++-- micras_hal/include/micras/hal/crc.hpp | 19 ++- micras_hal/include/micras/hal/encoder.hpp | 16 +-- micras_hal/include/micras/hal/flash.hpp | 52 ++++---- micras_hal/include/micras/hal/gpio.hpp | 22 ++-- micras_hal/include/micras/hal/mcu.hpp | 6 +- micras_hal/include/micras/hal/pwm.hpp | 25 ++-- micras_hal/include/micras/hal/pwm_dma.hpp | 32 ++--- micras_hal/include/micras/hal/spi.hpp | 28 ++-- micras_hal/include/micras/hal/timer.hpp | 44 +++---- micras_hal/stm32/src/flash.cpp | 5 +- micras_hal/stm32/src/mcu.cpp | 5 +- micras_hal/stm32/src/pwm.cpp | 1 + micras_nav/include/micras/nav/follow_wall.hpp | 43 +++--- micras_nav/include/micras/nav/go_to_point.hpp | 60 ++++----- micras_nav/include/micras/nav/grid_pose.hpp | 66 +++++----- .../include/micras/nav/look_at_point.hpp | 38 +++--- micras_nav/include/micras/nav/mapping.hpp | 124 +++++++++--------- micras_nav/include/micras/nav/maze.hpp | 95 +++++++------- micras_nav/include/micras/nav/odometry.hpp | 43 +++--- .../include/micras/nav/pid_controller.hpp | 105 ++++++++++----- micras_nav/include/micras/nav/state.hpp | 96 +++++++------- micras_proxy/include/micras/proxy/argb.hpp | 54 ++++---- micras_proxy/include/micras/proxy/battery.hpp | 30 ++--- micras_proxy/include/micras/proxy/button.hpp | 54 ++++---- micras_proxy/include/micras/proxy/buzzer.hpp | 30 ++--- .../include/micras/proxy/dip_switch.hpp | 20 +-- micras_proxy/include/micras/proxy/fan.hpp | 40 +++--- micras_proxy/include/micras/proxy/imu.hpp | 83 ++++++------ micras_proxy/include/micras/proxy/led.hpp | 16 +-- .../include/micras/proxy/locomotion.hpp | 32 ++--- micras_proxy/include/micras/proxy/motor.hpp | 20 +-- .../include/micras/proxy/rotary_sensor.hpp | 30 ++--- .../micras/proxy/rotary_sensor_reg.hpp | 15 ++- .../include/micras/proxy/serializable.hpp | 22 ++-- micras_proxy/include/micras/proxy/storage.hpp | 74 +++++------ .../include/micras/proxy/torque_sensors.hpp | 54 ++++---- .../include/micras/proxy/wall_sensors.hpp | 66 +++++----- micras_proxy/src/buzzer.cpp | 2 +- 42 files changed, 901 insertions(+), 854 deletions(-) diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp index f9bc3c4..f5ed586 100644 --- a/micras_core/include/micras/core/butterworth_filter.hpp +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -1,9 +1,5 @@ /** - * @file butterworth_filter.hpp - * - * @brief Core Butterworth filter class header - * - * @date 09/2024 + * @file */ #ifndef MICRAS_CORE_BUTTERWORTH_FILTER_HPP @@ -14,71 +10,74 @@ namespace micras::core { /** - * @brief Implementation of Butterworth second order low-pass filter - * A generic digital filter follows the relation - * a0 * y[k] = sum(bi * x[k - i]) - sum(aj * y[k - j]) - * Where - * x[k] - measurement at instant k - * y[k] - filtered signal at instant k - * The Butterworth filter have the special property of being a - * maximally flat magnitude filter, in other words, is the best - * filter that doesn't present distortions around the cutoff - * frequency - * The formula for the continuos coefficients of the Butterworth - * filter is available here: - * https://en.wikipedia.org/wiki/Butterworth_filter - * The discrete version were computed with the Tustin method: - * https://en.wikipedia.org/wiki/Bilinear_transform + * @brief Implementation of Butterworth second order low-pass filter. + * + * @details + * A generic digital filter follows the relation: + * a0 * y[k] = sum(bi * x[k - i]) - sum(aj * y[k - j]) + * Where: + * x[k] - measurement at instant k + * y[k] - filtered signal at instant k + * The Butterworth filter have the special property of being a + * maximally flat magnitude filter, in other words, is the best + * filter that doesn't present distortions around the cutoff + * frequency. + * + * @see + * The formula for the continuos coefficients of the Butterworth + * filter is available here: + * https://en.wikipedia.org/wiki/Butterworth_filter + * The discrete version were computed with the Tustin method: + * https://en.wikipedia.org/wiki/Bilinear_transform */ class ButterworthFilter { public: /** - * @brief Construct a new Butterworth Second Order filter object + * @brief Construct a new Butterworth Second Order filter object. * - * @param cutoff_frequency Low-pass cutoff frequency in Hz + * @param cutoff_frequency Low-pass cutoff frequency in Hz. * @param sampling_frequency Sampling frequency in Hz. */ explicit ButterworthFilter(float cutoff_frequency, float sampling_frequency = 100.0F); /** - * @brief Produces a new value from measured data - * - * @param x0 Last measure + * @brief Produces a new value from measured data. * - * @return Filtered value + * @param x0 Last measure. + * @return Filtered value. */ float update(float x0); /** - * @brief Get the last filtered value + * @brief Get the last filtered value. * - * @return Last filtered value + * @return Last filtered value. */ float get_last() const; private: /** - * @brief Order of the filter + * @brief Order of the filter. */ static constexpr uint8_t filter_order{2}; /** - * @brief Last input values of the filter + * @brief Last input values of the filter. */ std::array x_array{}; /** - * @brief Last output values of the filter + * @brief Last output values of the filter. */ std::array y_array{}; /** - * @brief Coefficients of the filter related to the output value + * @brief Coefficients of the filter related to the output value. */ std::array a_array{}; /** - * @brief Coefficients of the filter related to the input value + * @brief Coefficients of the filter related to the input value. */ std::array b_array{}; }; diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index ce432e3..244d389 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -9,7 +9,7 @@ namespace micras::core { /** - * @brief Possible values for the observation of a wall + * @brief Possible values for the observation of a wall. */ enum Observation : uint8_t { UNKNOWN = 0, @@ -18,7 +18,7 @@ enum Observation : uint8_t { }; /** - * @brief Type of wall following the robot is able to do + * @brief Types of wall following the robot is able to do. */ enum FollowWallType : uint8_t { NONE = 0, @@ -29,6 +29,9 @@ enum FollowWallType : uint8_t { BACK = 5, }; +/** + * @brief Possible objectives of the robot. + */ enum Objective : uint8_t { EXPLORE = 0, RETURN = 1, diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index ba32cd3..a9319b9 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -12,15 +12,15 @@ namespace micras::core { /** - * @brief Remap a value from one range to another + * @brief Remap a value from one range to another. * - * @tparam T Type of the value - * @param value Value to be remapped - * @param in_min Minimum value of the input range - * @param in_max Maximum value of the input range - * @param out_min Minimum value of the output range - * @param out_max Maximum value of the output range - * @return T Remapped value + * @tparam T Type of the value. + * @param value Value to be remapped. + * @param in_min Minimum value of the input range. + * @param in_max Maximum value of the input range. + * @param out_min Minimum value of the output range. + * @param out_max Maximum value of the output range. + * @return Remapped value. */ template constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) { @@ -28,13 +28,13 @@ constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) { } /** - * @brief Move a value towards a target limiting the step size + * @brief Move a value towards a target limiting by the step size. * - * @tparam T Type of the value - * @param value Current value - * @param target Target value - * @param step Step size - * @return T New value + * @tparam T Type of the value. + * @param value Current value. + * @param target Target value. + * @param step Step size. + * @return New value. */ template constexpr T move_towards(T value, T target, T step) { @@ -42,14 +42,14 @@ constexpr T move_towards(T value, T target, T step) { } /** - * @brief Varies a value from start to end smoothly + * @brief Vary a value from start to end smoothly. * - * @tparam T Type of the value - * @param value Current value - * @param resistance Value resistance to variation - * @param start Start value - * @param end End value - * @return T New value + * @tparam T Type of the value. + * @param value Current value. + * @param start Start value. + * @param end End value. + * @param resistance Value resistance to variation. + * @return New value. */ template constexpr T transition(T value, T start, T end, T resistance) { @@ -57,13 +57,13 @@ constexpr T transition(T value, T start, T end, T resistance) { } /** - * @brief Create an array objects calling its constructors from an array of parameters + * @brief Create an array objects calling its constructors from an array of parameters. * - * @tparam T Type of the array - * @tparam N Size of the array - * @tparam C Type of the parameters - * @param parameters List of parameters - * @return std::array Array with the objects created from the parameters + * @tparam T Type of the array. + * @tparam N Size of the array. + * @tparam C Type of the parameters. + * @param parameters List of parameters. + * @return Array with the objects created from the parameters. */ template constexpr std::array make_array(const std::array& parameters) { @@ -73,13 +73,13 @@ constexpr std::array make_array(const std::array& parameters) { } /** - * @brief Create an array objects calling its constructors from a single parameter + * @brief Create an array objects calling its constructors from a single parameter. * - * @tparam T Type of the array - * @tparam N Size of the array - * @tparam C Type of the parameter - * @param value Parameter - * @return std::array Array with the objects created from the parameter + * @tparam T Type of the array. + * @tparam N Size of the array. + * @tparam C Type of the parameter. + * @param value Parameter. + * @return Array with the objects created from the parameter. */ template constexpr std::array make_array(C value) { @@ -89,10 +89,10 @@ constexpr std::array make_array(C value) { } /** - * @brief Assert an angle to be in the range [-pi, pi] + * @brief Assert an angle to be in the range [-pi, pi]. * - * @param angle Angle to be asserted - * @return float Asserted angle + * @param angle Angle to be asserted. + * @return Asserted angle. */ constexpr float assert_angle(float angle) { angle = std::fmod(angle, 2 * std::numbers::pi_v); @@ -107,10 +107,10 @@ constexpr float assert_angle(float angle) { } /** - * @brief Assert an angle to be in the range [-pi/2, pi/2] + * @brief Assert an angle to be in the range [-pi/2, pi/2]. * - * @param angle Angle to be asserted - * @return float Asserted angle + * @param angle Angle to be asserted. + * @return Asserted angle. */ constexpr float assert_half_angle(float angle) { angle = std::fmod(angle, std::numbers::pi_v); @@ -125,12 +125,12 @@ constexpr float assert_half_angle(float angle) { } /** - * @brief Check if two floating point numbers are near each other + * @brief Check if two floating point numbers are near each other. * - * @param x First number - * @param y Second number - * @param tolerance Tolerance - * @return true If the numbers are near each other, false otherwise + * @param x First number. + * @param y Second number. + * @param tolerance Tolerance. + * @return True if the numbers are near each other, false otherwise. */ constexpr bool is_near(float x, float y, float tolerance = 0.001) { return std::abs(x - y) <= tolerance; diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 77d3e66..891d257 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -11,12 +11,12 @@ namespace micras::hal { /** - * @brief Class to handle ADC peripheral on STM32 microcontrollers using DMA + * @brief Class to handle ADC peripheral on STM32 microcontrollers using DMA. */ class AdcDma { public: /** - * @brief Configuration structure for ADC DMA + * @brief Configuration struct for ADC DMA. */ struct Config { void (*init_function)(); @@ -25,51 +25,51 @@ class AdcDma { }; /** - * @brief Construct a new AdcDma object + * @brief Construct a new AdcDma object. * - * @param config ADC DMA configuration struct + * @param config ADC DMA configuration struct. */ explicit AdcDma(const Config& config); /** - * @brief Enable ADC, start conversion of regular group and transfer result through DMA + * @brief Enable ADC, start conversion of regular group and transfer result through DMA. * - * @param buffer Destination Buffer address + * @param buffer Destination Buffer address. */ void start_dma(std::span buffer); /** - * @brief Enable ADC, start conversion of regular group and transfer result through DMA + * @brief Enable ADC, start conversion of regular group and transfer result through DMA. * - * @param buffer Destination Buffer address + * @param buffer Destination Buffer address. */ void start_dma(std::span buffer); /** - * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode) + * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode). */ void stop_dma(); /** - * @brief Get the maximum reading of the ADC + * @brief Get the maximum reading of the ADC. * - * @return uint16_t Maximum reading of the ADC + * @return Maximum reading of the ADC. */ uint16_t get_max_reading() const; /** - * @brief Reference voltage for the ADC measurement + * @brief Reference voltage for the ADC measurement. */ static constexpr float reference_voltage{3.3F}; private: /** - * @brief Maximum ADC reading + * @brief Maximum ADC reading. */ uint16_t max_reading; /** - * @brief ADC handle + * @brief ADC handle. */ ADC_HandleTypeDef* handle; }; diff --git a/micras_hal/include/micras/hal/crc.hpp b/micras_hal/include/micras/hal/crc.hpp index 648c942..e22dea8 100644 --- a/micras_hal/include/micras/hal/crc.hpp +++ b/micras_hal/include/micras/hal/crc.hpp @@ -9,37 +9,36 @@ namespace micras::hal { /** - * @brief Class to handle the cyclic redundancy check peripheral on STM32 microcontrollers + * @brief Class to handle the cyclic redundancy check peripheral on STM32 microcontrollers. */ class Crc { public: /** - * @brief CRC configuration struct + * @brief CRC configuration struct. */ struct Config { CRC_HandleTypeDef* handle; }; /** - * @brief Construct a new Crc object + * @brief Construct a new Crc object. * - * @param config Configuration for the CRC + * @param config Configuration for the CRC. */ explicit Crc(const Config& config); /** - * @brief Calculate the CRC value + * @brief Calculate the CRC value. * - * @param data Data to calculate the CRC - * @param size Size of the buffer - * - * @return uint32_t CRC value + * @param data Data to calculate the CRC. + * @param size Size of the buffer. + * @return CRC value. */ uint32_t calculate(uint32_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) private: /** - * @brief CRC handle + * @brief CRC handle. */ CRC_HandleTypeDef* handle; }; diff --git a/micras_hal/include/micras/hal/encoder.hpp b/micras_hal/include/micras/hal/encoder.hpp index bbad892..39f4380 100644 --- a/micras_hal/include/micras/hal/encoder.hpp +++ b/micras_hal/include/micras/hal/encoder.hpp @@ -9,12 +9,12 @@ namespace micras::hal { /** - * @brief Class to handle encoder peripheral on STM32 microcontrollers + * @brief Class to handle encoder peripheral on STM32 microcontrollers. */ class Encoder { public: /** - * @brief Encoder configuration struct + * @brief Encoder configuration struct. */ struct Config { void (*init_function)(); @@ -23,27 +23,27 @@ class Encoder { }; /** - * @brief Construct a new Encoder object + * @brief Construct a new Encoder object. * - * @param config Configuration for the encoder + * @param config Configuration for the encoder. */ explicit Encoder(const Config& config); /** - * @brief Get the counter value + * @brief Get the counter value. * - * @return int32_t Current value of the counter + * @return Current value of the counter. */ int32_t get_counter() const; private: /** - * @brief Timer handle + * @brief Timer handle. */ TIM_HandleTypeDef* handle; /** - * @brief Start value of the timer counter + * @brief Start value of the timer counter. */ uint32_t start_count; }; diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp index 859253f..5768f84 100644 --- a/micras_hal/include/micras/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -10,78 +10,78 @@ namespace micras::hal { /** - * @brief Class to handle flash memory on STM32 microcontrollers + * @brief Class to handle flash memory on STM32 microcontrollers. */ class Flash { public: /** - * @brief Deleted constructor for static class + * @brief Deleted constructor for static class. */ Flash() = delete; // NOLINTBEGIN(*-avoid-c-arrays) /** - * @brief Read data from flash memory + * @brief Read data from flash memory. * - * @param address address to read from (indexed by double words) - * @param data pointer to store the data read - * @param size size in double words of the data to read + * @param address Address to read from (indexed by double words). + * @param data Pointer to store the data read. + * @param size Size in double words of the data to read. */ static void read(uint32_t address, uint64_t data[], uint32_t size = 1); /** - * @brief Read data from flash memory + * @brief Read data from flash memory. * - * @param page page to read from (counting from the last to the first) - * @param page_address address inside the page to read from (indexed by double words) - * @param data pointer to store the data read - * @param size size in double words of the data to read + * @param page Page to read from (counting from the last to the first). + * @param page_address Address inside the page to read from (indexed by double words). + * @param data Pointer to store the data read. + * @param size Size in double words of the data to read. */ static void read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); /** - * @brief Write data to flash memory + * @brief Write data to flash memory. * - * @param address address to write to (indexed by double words) - * @param data pointer to the data to write - * @param size size in double words of the data to write + * @param address Address to write to (indexed by double words). + * @param data Pointer to the data to write. + * @param size Size in double words of the data to write. */ static void write(uint32_t address, const uint64_t data[], uint32_t size = 1); /** - * @brief Write data to flash memory + * @brief Write data to flash memory. * - * @param page page to write to (counting from the last to the first) - * @param page_address address inside the page to write to (indexed by double words) - * @param data pointer to the data to write - * @param size size in double words of the data to write + * @param page Page to write to (counting from the last to the first). + * @param page_address Address inside the page to write to (indexed by double words). + * @param data Pointer to the data to write. + * @param size Size in double words of the data to write. */ static void write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size = 1); // NOLINTEND(*-avoid-c-arrays) /** - * @brief Erase flash memory pages + * @brief Erase flash memory pages. * - * @param page first page to erase (counting from the last to the first) - * @param number_of_pages number of pages to erase + * @param page First page to erase (counting from the last to the first). + * @param number_of_pages Number of pages to erase. */ static void erase_pages(uint16_t page, uint16_t number_of_pages = 1); private: /** - * @brief Number of double words per row + * @brief Number of double words per row. */ static constexpr uint32_t double_words_per_row{32}; /** - * @brief Number of bytes per row + * @brief Number of bytes per row. */ static constexpr uint32_t bytes_per_row{8 * double_words_per_row}; /** - * @brief Number of double words per page + * @brief Number of double words per page. */ static constexpr uint32_t double_words_per_page{FLASH_PAGE_SIZE / 8}; }; diff --git a/micras_hal/include/micras/hal/gpio.hpp b/micras_hal/include/micras/hal/gpio.hpp index 261220e..97d2d63 100644 --- a/micras_hal/include/micras/hal/gpio.hpp +++ b/micras_hal/include/micras/hal/gpio.hpp @@ -10,12 +10,12 @@ namespace micras::hal { /** - * @brief Class for controlling GPIO pins on STM32 microcontrollers + * @brief Class to handle GPIO pins on STM32 microcontrollers. */ class Gpio { public: /** - * @brief Configuration structure for GPIO pin + * @brief Configuration struct for GPIO pin. */ struct Config { GPIO_TypeDef* port; @@ -23,39 +23,39 @@ class Gpio { }; /** - * @brief Constructor for the Gpio class + * @brief Construct a new Gpio object. * - * @param config Configuration for the GPIO pin + * @param config Configuration for the GPIO pin. */ explicit Gpio(const Config& config); /** - * @brief Read the current state of the GPIO pin + * @brief Read the current state of the GPIO pin. * - * @return bool The current state of the GPIO pin (true for high, false for low) + * @return True if the current state of the GPIO pin is high, false otherwise. */ bool read() const; /** - * @brief Write a new state to the GPIO pin + * @brief Write a new state to the GPIO pin. * - * @param pin_state The state to be written (true for high, false for low) + * @param pin_state The state to be written (true for high, false for low). */ void write(bool state); /** - * @brief Toggle the state of the GPIO pin + * @brief Toggle the state of the GPIO pin. */ void toggle(); private: /** - * @brief The port of the GPIO + * @brief The port of the GPIO. */ GPIO_TypeDef* port; /** - * @brief The pin number of the GPIO + * @brief The pin number of the GPIO. */ uint16_t pin; }; diff --git a/micras_hal/include/micras/hal/mcu.hpp b/micras_hal/include/micras/hal/mcu.hpp index 8a9c19d..f963489 100644 --- a/micras_hal/include/micras/hal/mcu.hpp +++ b/micras_hal/include/micras/hal/mcu.hpp @@ -9,17 +9,17 @@ namespace micras::hal { /** - * @brief Microcontroller unit class + * @brief Microcontroller unit class. */ class Mcu { public: /** - * @brief Deleted constructor for static class + * @brief Deleted constructor for static class. */ Mcu() = delete; /** - * @brief Initializes MCU and some peripherals + * @brief Initializes MCU and some peripherals. */ static void init(); }; diff --git a/micras_hal/include/micras/hal/pwm.hpp b/micras_hal/include/micras/hal/pwm.hpp index 6b95854..548b8cb 100644 --- a/micras_hal/include/micras/hal/pwm.hpp +++ b/micras_hal/include/micras/hal/pwm.hpp @@ -10,12 +10,12 @@ namespace micras::hal { /** - * @brief Class to handle PWM peripheral on STM32 microcontrollers + * @brief Class to handle PWM peripheral on STM32 microcontrollers. */ class Pwm { public: /** - * @brief PWM configuration struct + * @brief PWM configuration struct. */ struct Config { void (*init_function)(); @@ -24,34 +24,39 @@ class Pwm { }; /** - * @brief Construct a new Pwm object + * @brief Construct a new Pwm object. * - * @param config Configuration for the PWM + * @param config Configuration for the PWM. */ explicit Pwm(const Config& config); /** - * @brief Set the PWM duty cycle + * @brief Set the PWM duty cycle. * - * @param duty_cycle Duty cycle value + * @param duty_cycle Duty cycle value. */ void set_duty_cycle(float duty_cycle); /** - * @brief Set the PWM frequency + * @brief Set the PWM frequency. * - * @param frequency Frequency value in Hz + * @note Changing the pwm frequency will also change the duty cycle and reset the counter. + * There is a minimum and maximum frequency that can be set by changing the auto-reload register. + * The minimum frequency is base_freq / (2^timer_resolution * (prescaler + 1)). + * The maximum frequency is base_freq / (prescaler + 1). + * + * @param frequency Frequency value in Hz. */ void set_frequency(uint32_t frequency); private: /** - * @brief Timer handle + * @brief Timer handle. */ TIM_HandleTypeDef* handle; /** - * @brief Channel number of the timer + * @brief Channel number of the timer. */ uint32_t channel; }; diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp index cc1c632..536d522 100644 --- a/micras_hal/include/micras/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -11,12 +11,12 @@ namespace micras::hal { /** - * @brief Class to handle PWM peripheral on STM32 microcontrollers using DMA + * @brief Class to handle PWM peripheral on STM32 microcontrollers using DMA. */ class PwmDma { public: /** - * @brief PWM configuration struct + * @brief PWM configuration struct. */ struct Config { void (*init_function)(); @@ -25,54 +25,54 @@ class PwmDma { }; /** - * @brief Construct a new PwmDma object + * @brief Construct a new PwmDma object. * - * @param config Configuration for the PWM + * @param config Configuration for the PWM. */ explicit PwmDma(const Config& config); /** - * @brief Start PWM and DMA transfer + * @brief Start PWM and DMA transfer. * - * @param buffer Buffer to transfer + * @param buffer Buffer to transfer. */ void start_dma(std::span buffer); /** - * @brief Start PWM and DMA transfer + * @brief Start PWM and DMA transfer. * - * @param buffer Buffer to transfer + * @param buffer Buffer to transfer. */ void start_dma(std::span buffer); /** - * @brief Stop PWM and DMA transfer + * @brief Stop PWM and DMA transfer. */ void stop_dma(); /** - * @brief Get the compare value for ad duty cycle + * @brief Get the compare value for ad duty cycle. * - * @param duty_cycle Duty cycle to get the compare value for - * @return uint32_t Compare value for the duty cycle + * @param duty_cycle Duty cycle to get the compare value for. + * @return Compare value for the duty cycle. */ uint32_t get_compare(float duty_cycle) const; /** - * @brief Check if the PWM is busy + * @brief Check if the PWM is busy. * - * @return true If the PWM is busy, false otherwise + * @return True If the PWM is busy, false otherwise. */ bool is_busy(); private: /** - * @brief Timer handle + * @brief Timer handle. */ TIM_HandleTypeDef* handle; /** - * @brief Channel number of the timer + * @brief Channel number of the timer. */ uint32_t channel; }; diff --git a/micras_hal/include/micras/hal/spi.hpp b/micras_hal/include/micras/hal/spi.hpp index ce1ae35..52c352c 100644 --- a/micras_hal/include/micras/hal/spi.hpp +++ b/micras_hal/include/micras/hal/spi.hpp @@ -13,12 +13,12 @@ namespace micras::hal { /** - * @brief Class to handle SPI peripheral on STM32 microcontrollers + * @brief Class to handle SPI peripheral on STM32 microcontrollers. */ class Spi { public: /** - * @brief SPI configuration struct + * @brief SPI configuration struct. */ struct Config { void (*init_function)(); @@ -28,51 +28,51 @@ class Spi { }; /** - * @brief Construct a new Spi object + * @brief Construct a new Spi object. * - * @param config Configuration for the SPI + * @param config Configuration for the SPI. */ explicit Spi(const Config& config); /** - * @brief Activate the chip select + * @brief Activate the chip select. * - * @return bool True if the device was successfully selected, false otherwise + * @return True if the device was successfully selected, false otherwise. */ bool select_device(); /** - * @brief Deactivate the chip select + * @brief Deactivate the chip select. */ void unselect_device(); /** - * @brief Transmit data over SPI + * @brief Transmit data over SPI. * - * @param data Data to transmit + * @param data Data to transmit. */ void transmit(std::span data); /** - * @brief Receive data over SPI + * @brief Receive data over SPI. * - * @param data Data to receive data + * @param data Data to receive data. */ void receive(std::span data); private: /** - * @brief Handle for the SPI + * @brief Handle for the SPI. */ SPI_HandleTypeDef* handle; /** - * @brief GPIO for the chip select pin + * @brief GPIO for the chip select pin. */ hal::Gpio cs_gpio; /** - * @brief Timeout for the SPI operations in ms + * @brief Timeout for the SPI operations in ms. */ uint32_t timeout; }; diff --git a/micras_hal/include/micras/hal/timer.hpp b/micras_hal/include/micras/hal/timer.hpp index 8c031bf..f166ffb 100644 --- a/micras_hal/include/micras/hal/timer.hpp +++ b/micras_hal/include/micras/hal/timer.hpp @@ -10,12 +10,12 @@ namespace micras::hal { /** - * @brief Class to handle timer peripheral on STM32 microcontrollers + * @brief Class to handle timer peripheral on STM32 microcontrollers. */ class Timer { public: /** - * @brief Timer configuration struct + * @brief Timer configuration struct. */ struct Config { void (*init_function)(); @@ -23,82 +23,82 @@ class Timer { }; /** - * @brief Construct a new Timer object + * @brief Construct a new Timer object. */ Timer(); /** - * @brief Construct a new Timer object + * @brief Construct a new Timer object. * - * @param config Configuration for the timer + * @param config Configuration for the timer. */ explicit Timer(const Config& config); /** - * @brief Reset the timer counter in milliseconds + * @brief Reset the milliseconds timer counter. */ void reset_ms(); /** - * @brief Reset the timer counter in microseconds + * @brief Reset the microseconds timer counter. */ void reset_us(); /** - * @brief Get the time elapsed since the last reset + * @brief Get the time elapsed since the last reset. * - * @return uint32_t Time elapsed in miliseconds + * @return Time elapsed in miliseconds. */ uint32_t elapsed_time_ms() const; /** - * @brief Get the time elapsed since the last reset + * @brief Get the time elapsed since the last reset. * - * @return uint32_t Time elapsed in microseconds + * @return Time elapsed in microseconds. */ uint32_t elapsed_time_us() const; /** - * @brief Sleep for a given amount of time + * @brief Sleep for a given amount of time. * - * @param time Time to sleep in milliseconds + * @param time Time to sleep in milliseconds. */ static void sleep_ms(uint32_t time); /** - * @brief Sleep for a given amount of time + * @brief Sleep for a given amount of time. * - * @param time Time to sleep in microseconds + * @param time Time to sleep in microseconds. */ void sleep_us(uint32_t time) const; private: /** - * @brief Get the current timer counter + * @brief Get the current timer counter. * - * @return uint32_t Current timer counter in milliseconds + * @return Current timer counter in milliseconds. */ static uint32_t get_counter_ms(); /** - * @brief Get the current timer counter + * @brief Get the current timer counter. * - * @return uint32_t Current timer counter in microseconds + * @return Current timer counter in microseconds. */ uint32_t get_counter_us() const; /** - * @brief Timer handle + * @brief Timer handle. */ TIM_HandleTypeDef* handle{}; /** - * @brief Timer counter + * @brief Timer counter. */ uint32_t counter{}; /** - * @brief Flag to enable microseconds + * @brief Flag to enable microseconds. */ bool enable_microseconds{false}; }; diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index fce95a4..86a2139 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -8,8 +8,9 @@ namespace micras::hal { /** - * @brief Last address of the flash memory - * needs to be here because it is not defined at compile time + * @brief Last address of the flash memory. + * + * @note Needs to be here because it is not defined at compile time. */ static const uint32_t base_address = FLASH_BASE + FLASH_SIZE - 8; diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp index beeaed6..4a54cf9 100644 --- a/micras_hal/stm32/src/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -10,8 +10,9 @@ extern "C" { /** - * @brief Initializes System Clock - * @note Defined by cube + * @brief Initializes System Clock. + * + * @note Defined by cube. */ void SystemClock_Config(); } diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index 6b2164c..a88312d 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -24,5 +24,6 @@ void Pwm::set_frequency(uint32_t frequency) { uint32_t autoreload = base_freq / ((prescaler + 1) * frequency) - 1; __HAL_TIM_SET_AUTORELOAD(this->handle, autoreload); + __HAL_TIM_SET_COUNTER(this->handle, 0); } } // namespace micras::hal diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index f9e1591..910a3f2 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -11,12 +11,12 @@ namespace micras::nav { /** - * @brief Class to follow a wall using a PID controller + * @brief Class to follow the side walls using a PID controller. */ class FollowWall { public: /** - * @brief Configuration for the FollowWall class + * @brief Configuration struct for the FollowWall class. */ struct Config { PidController::Config pid; @@ -26,75 +26,74 @@ class FollowWall { }; /** - * @brief Construct a new FollowWall object + * @brief Construct a new FollowWall object. * - * @param wall_sensors The wall sensors of the robot - * @param config The configuration for the FollowWall class + * @param wall_sensors The wall sensors of the robot. + * @param config The configuration for the FollowWall class. */ FollowWall(const proxy::TWallSensors<4>& wall_sensors, const Config& config); /** - * @brief Update the PID controller and return the response + * @brief Update the PID controller and return the response. * - * @param follow_wall_type The type of wall following to perform - * @param elapsed_time The time elapsed since the last update - * - * @return The response of the PID controller + * @param follow_wall_type The type of wall following to perform. + * @param elapsed_time The time elapsed since the last update. + * @return The response of the PID controller. */ float action(core::FollowWallType follow_wall_type, float elapsed_time); /** - * @brief Resets the PID controller + * @brief Resets the PID controller. */ void reset(); /** - * @brief Resets the base readings of the wall sensors + * @brief Resets the base readings of the wall sensors. */ void reset_base_readings(); private: /** - * @briefGet the reading of the left wall sensor + * @brief Get the reading of the left wall sensor. * - * @return float + * @return The reading of the left wall sensor. */ float get_left_value() const; /** - * @brief Get the reading of the right wall sensor + * @brief Get the reading of the right wall sensor. * - * @return float + * @return The reading of the right wall sensor. */ float get_right_value() const; /** - * @brief Wall sensors of the robot + * @brief Wall sensors of the robot. */ const proxy::TWallSensors<4>& wall_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) /** - * @brief PID controller for the wall following + * @brief PID controller for the wall following. */ PidController pid; /** - * @brief Butterworth filter for the derivative of the error + * @brief Butterworth filter for the derivative of the error. */ core::ButterworthFilter filter; /** - * @brief Last difference between the left and right wall sensors + * @brief Last difference between the left and right wall sensors. */ float last_diff{}; /** - * @brief Base reading of the left wall sensor when aligned with the wall + * @brief Base reading of the left wall sensor when aligned with the wall. */ float base_left_reading; /** - * @brief Base reading of the right wall sensor when aligned with the wall + * @brief Base reading of the right wall sensor when aligned with the wall. */ float base_right_reading; }; diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index dfefe60..9e6f7a0 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -11,12 +11,12 @@ namespace micras::nav { /** - * @brief Class to calculate a command to go to a point + * @brief Class to calculate a command to go to a point. */ class GoToPoint { public: /** - * @brief Configuration for the GoToPoint class + * @brief Configuration struct for the GoToPoint class. */ struct Config { PidController::Config stop_pid; @@ -30,26 +30,25 @@ class GoToPoint { }; /** - * @brief Constructor for the GoToPoint class + * @brief Construct a new GoToPoint object. * - * @param wall_sensors The wall sensors of the robot - * @param config The configuration for the GoToPoint class - * @param follow_wall_config The configuration for the FollowWall class + * @param wall_sensors The wall sensors of the robot. + * @param config The configuration for the GoToPoint class. + * @param follow_wall_config The configuration for the FollowWall class. */ GoToPoint( const proxy::TWallSensors<4>& wall_sensors, const Config& config, const FollowWall::Config& follow_wall_config ); /** - * @brief Calculates the command to go to a point + * @brief Calculates the command to go to a point. * - * @param state The current state of the robot - * @param goal The goal point - * @param follow_wall_type The type of wall following to perform - * @param elapsed_time The time elapsed since the last update - * @param stop Whether to stop at the goal - * - * @return The command to go to the point + * @param state The current state of the robot. + * @param goal The goal point. + * @param follow_wall_type The type of wall following to perform. + * @param elapsed_time The time elapsed since the last update. + * @param stop Whether to stop at the goal. + * @return The command to go to the point. */ Twist action( const State& state, const Point& goal, core::FollowWallType follow_wall_type, float elapsed_time, @@ -57,69 +56,68 @@ class GoToPoint { ); /** - * @brief Checks if the robot has reached the goal - * - * @param state The current state of the robot - * @param goal The goal point - * @param stop Whether to stop at the goal + * @brief Checks if the robot has reached the goal. * - * @return True if the robot has reached the goal, false otherwise + * @param state The current state of the robot. + * @param goal The goal point. + * @param stop Whether to stop at the goal. + * @return True if the robot has reached the goal, false otherwise. */ bool finished(const State& state, const Point& goal, bool stop = true) const; /** - * @brief Resets the PID controllers + * @brief Reset the PID controllers. */ void reset(); /** - * @brief Calibrates the wall sensors + * @brief Calibrate the wall sensors. */ void calibrate(); private: /** - * @brief PID controller for stoppping at the goal + * @brief PID controller for stopping at the goal. */ PidController stop_pid; /** - * @brief PID controller for the orientation + * @brief PID controller for the orientation. */ PidController angular_pid; /** - * @brief FollowWall controller + * @brief FollowWall controller. */ FollowWall follow_wall; /** - * @brief Size of the grid cells + * @brief Size of the grid cells. */ float cell_size; /** - * @brief Minimum linear command + * @brief Minimum linear command. */ float min_linear_command; /** - * @brief Maximum linear command + * @brief Maximum linear command. */ float max_linear_command; /** - * @brief Deceleration factor for the linear command transition + * @brief Deceleration factor for the linear command transition. */ float deceleration_factor; /** - * @brief Linear tolerance for the goal point + * @brief Linear tolerance for the goal point. */ float distance_tolerance; /** - * @brief Velocity tolerance at the goal point + * @brief Velocity tolerance at the goal point. */ float velocity_tolerance; }; diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index 6139054..80c0376 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -12,7 +12,7 @@ namespace micras::nav { /** - * @brief Possible sides in the grid + * @brief Possible sides in the grid. */ enum Side : uint8_t { RIGHT = 0, @@ -22,19 +22,19 @@ enum Side : uint8_t { }; /** - * @brief Converts an angle in radians to a Grid side + * @brief Converts an angle in radians to a Grid side. * - * @param angle The angle in radians - * @return The corresponding Grid side + * @param angle The angle in radians. + * @return The corresponding Grid side. */ Side angle_to_grid(float angle); /** - * @brief Type to store information originating from the wall sensors + * @brief Type to store information originating from the wall sensors. */ struct Information { /** - * @brief Possible walls in the grid to be checked + * @brief Possible walls in the grid to be checked. */ core::Observation left; core::Observation front_left; @@ -44,88 +44,88 @@ struct Information { }; /** - * @brief Type to store a point in the grid + * @brief Type to store a point in the grid. */ struct GridPoint { /** - * @brief Returns the direction from this point to the next one + * @brief Returns the direction from this point to the next one. * - * @param next The next point - * @return The direction from this point to the next one + * @param next The next point. + * @return The direction from this point to the next one. */ Side direction(const GridPoint& next) const; /** - * @brief Moves in the grid in the direction of the side + * @brief Moves in the grid in the direction of the side. * - * @param side The side to move to - * @return The new point after moving + * @param side The side to move to. + * @return The new point after moving. */ GridPoint operator+(const Side& side) const; /** - * @brief Compares two points for equality + * @brief Compares two points for equality. * - * @param other The other point to compare - * @return True if the points are equal, false otherwise + * @param other The other point to compare. + * @return True if the points are equal, false otherwise. */ bool operator==(const GridPoint& other) const; /** - * @brief The x coordinate of the point on the grid + * @brief The x coordinate of the point on the grid. */ uint8_t x; /** - * @brief The y coordinate of the point on the grid + * @brief The y coordinate of the point on the grid. */ uint8_t y; }; struct GridPose { /** - * @brief Returns the pose after moving forward + * @brief Returns the pose after moving forward. * - * @return The pose after moving forward + * @return The pose after moving forward. */ GridPose front() const; /** - * @brief Returns the pose after turning back + * @brief Returns the pose after turning back. * - * @return The pose after turning back + * @return The pose after turning back. */ GridPose turned_back() const; /** - * @brief Returns the pose after turning left + * @brief Returns the pose after turning left. * - * @return The pose after turning left + * @return The pose after turning left. */ GridPose turned_left() const; /** - * @brief Returns the pose after turning right + * @brief Returns the pose after turning right. * - * @return The pose after turning right + * @return The pose after turning right. */ GridPose turned_right() const; /** - * @brief Compares two poses for equality + * @brief Compares two poses for equality. * - * @param other The other pose to compare - * @return True if the poses are equal, false otherwise + * @param other The other pose to compare. + * @return True if the poses are equal, false otherwise. */ bool operator==(const GridPose& other) const; /** - * @brief The position of the pose on the grid + * @brief The position of the pose on the grid. */ GridPoint position; /** - * @brief The orientation of the pose on the grid + * @brief The orientation of the pose on the grid. */ Side orientation; }; @@ -133,9 +133,9 @@ struct GridPose { namespace std { /** - * @brief Hash specialization for the GridPoint type + * @brief Hash specialization for the GridPoint type. * - * @tparam T GridPoint type + * @tparam T GridPoint type. */ template <> struct hash { diff --git a/micras_nav/include/micras/nav/look_at_point.hpp b/micras_nav/include/micras/nav/look_at_point.hpp index 53f2e4b..65cace5 100644 --- a/micras_nav/include/micras/nav/look_at_point.hpp +++ b/micras_nav/include/micras/nav/look_at_point.hpp @@ -10,12 +10,12 @@ namespace micras::nav { /** - * @brief Class to calculate a command to look at a point + * @brief Class to calculate a command to look at a point. */ class LookAtPoint { public: /** - * @brief Configuration for the LookAtPoint class + * @brief Configuration struct for the LookAtPoint class. */ struct Config { PidController::Config linear_pid; @@ -25,56 +25,54 @@ class LookAtPoint { }; /** - * @brief Constructor for the LookAtPoint class + * @brief Construct a new LookAtPoint object. * - * @param config The configuration for the LookAtPoint class + * @param config The configuration for the LookAtPoint class. */ explicit LookAtPoint(Config config); /** - * @brief Calculates the command to look at a point + * @brief Calculates the command to look at a point. * - * @param pose The current pose of the robot - * @param goal The goal point - * @param elapsed_time The time elapsed since the last update - * - * @return The command to look at the point + * @param pose The current pose of the robot. + * @param goal The goal point. + * @param elapsed_time The time elapsed since the last update. + * @return The command to look at the point. */ Twist action(const State& state, const Point& goal, float elapsed_time); /** - * @brief Resets the PID controllers + * @brief Resets the PID controllers. */ void reset(); /** - * @brief Checks if the robot has reached the goal - * - * @param pose The current pose of the robot - * @param goal The goal point + * @brief Checks if the robot has reached the goal. * - * @return True if the robot has reached the goal, false otherwise + * @param pose The current pose of the robot. + * @param goal The goal point. + * @return True if the robot has reached the goal, false otherwise. */ bool finished(const State& state, const Point& goal, bool stop = true) const; private: /** - * @brief PID controller for the linear velocity + * @brief PID controller for the linear velocity. */ PidController linear_pid; /** - * @brief PID controller for the angular velocity + * @brief PID controller for the angular velocity. */ PidController angular_pid; /** - * @brief Angular tolerance for the goal + * @brief Angular tolerance for the goal. */ float distance_tolerance; /** - * @brief Velocity tolerance for the goal + * @brief Velocity tolerance for the goal. */ float velocity_tolerance; }; diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index a3edb5b..249b1a5 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -15,16 +15,16 @@ namespace micras::nav { /** - * @brief Class for mapping the maze + * @brief Class for mapping the maze. * - * @tparam width The width of the maze - * @tparam height The height of the maze + * @tparam width The width of the maze. + * @tparam height The height of the maze. */ template class TMapping : public proxy::ISerializable { public: /** - * @brief Configuration for the Mapping class + * @brief Configuration struct for the Mapping class. */ struct Config { float wall_thickness{}; @@ -49,7 +49,7 @@ class TMapping : public proxy::ISerializable { }; /** - * @brief Type to identify the wall sensors + * @brief Type to identify the wall sensors. */ enum Sensor : uint8_t { FRONT_LEFT, @@ -59,11 +59,11 @@ class TMapping : public proxy::ISerializable { }; /** - * @brief Type to store an action to be taken + * @brief Type to store an action to be taken. */ struct Action { /** - * @brief Possible types of actions + * @brief Possible types of actions. */ enum Type : uint8_t { LOOK_AT, @@ -74,211 +74,211 @@ class TMapping : public proxy::ISerializable { }; /** - * @brief Type of the action + * @brief Type of the action. */ Type type; /** - * @brief The next goal point + * @brief The next goal point. */ Point point; /** - * @brief The orientation of the goal point + * @brief The orientation of the goal point. */ Direction direction; }; /** - * @brief Constructor for the Mapping class + * @brief Construct a new Mapping object. * - * @param wall_sensors The wall sensors - * @param config The configuration for the mapping + * @param wall_sensors The wall sensors. + * @param config The configuration for the mapping. */ TMapping(const proxy::TWallSensors<4>& wall_sensors, Config config); /** - * @brief Update the mapping of the maze using the current pose and wall sensors + * @brief Update the mapping of the maze using the current pose and wall sensors. * - * @param pose The current pose of the robot + * @param pose The current pose of the robot. */ void update(const Pose& pose); /** - * @brief Get the action to be taken based on the current pose + * @brief Get the action to be taken based on the current pose. * - * @param pose The current pose of the robot - * @param objective The curent objective of the robot - * @return The action to be taken + * @param pose The current pose of the robot. + * @param objective The curent objective of the robot. + * @return The action to be taken. */ Action get_action(const Pose& pose, core::Objective objective); /** - * @brief Fix the pose based on the mapping information + * @brief Fix the pose based on the mapping information. * - * @param pose The current pose of the robot - * @param follow_wall_type The type of wall following being performed - * @return The fixed pose + * @param pose The current pose of the robot. + * @param follow_wall_type The type of wall following being performed. + * @return The fixed pose. */ Pose correct_pose(const Pose& pose, core::FollowWallType follow_wall_type) const; /** - * @brief Calibrate front alignment of the wall sensors + * @brief Calibrate front alignment of the wall sensors. */ void calibrate_front(); /** - * @brief Calibrate side alignment of the wall sensors + * @brief Calibrate side alignment of the wall sensors. */ void calibrate_side(); /** - * @brief Check if the robot is at a distance from a wall at the front + * @brief Check if the robot is at a distance from a wall at the front. * - * @return True if the robot is aligned, false otherwise + * @return True if the robot is aligned, false otherwise. */ bool is_distance_front_aligned() const; /** - * @brief Check if the robot orientation is aligned with walls at the front + * @brief Check if the robot orientation is aligned with walls at the front. * - * @return True if the robot is aligned, false otherwise + * @return True if the robot is aligned, false otherwise. */ bool is_orientation_front_aligned() const; /** - * @brief Check if the robot is at a distance from a wall at the sides + * @brief Check if the robot is at a distance from a wall at the sides. * - * @return True if the robot is aligned, false otherwise + * @return True if the robot is aligned, false otherwise. */ bool is_distance_side_aligned() const; /** - * @brief Check if the robot orientation is aligned with walls at the sides + * @brief Check if the robot orientation is aligned with walls at the sides. * - * @return True if the robot is aligned, false otherwise + * @return True if the robot is aligned, false otherwise. */ bool is_orientation_side_aligned() const; /** - * @brief Get the type of wall following the robot can do + * @brief Get the type of wall following the robot can do. * - * @param pose The current pose of the robot - * @return The type of wall following + * @param pose The current pose of the robot. + * @return The type of wall following. */ core::FollowWallType get_follow_wall_type(const Pose& pose) const; /** - * @brief Serialize the best route to the goal + * @brief Serialize the best route to the goal. * - * @return The serialized data + * @return The serialized data. */ std::vector serialize() const override; /** - * @brief Deserialize the best route to the goal + * @brief Deserialize the best route to the goal. * - * @param buffer The serialized data - * @param size The size of the serialized data + * @param buffer The serialized data. + * @param size The size of the serialized data. */ void deserialize(const uint8_t* buffer, uint16_t size) override; /** - * @brief Check if the robot can align its back with a wall + * @brief Check if the robot can align its back with a wall. * - * @param pose The current pose of the robot - * @return True if the robot can align its back, false otherwise + * @param pose The current pose of the robot. + * @return True if the robot can align its back, false otherwise. */ bool can_align_back(const Pose& pose) const; /** - * @brief Adds diagonal movements to the best route + * @brief Adds diagonal movements to the best route. */ void diagonalize_best_route(); private: /** - * @brief Wall sensors of the robot + * @brief Wall sensors of the robot. */ const proxy::TWallSensors<4>& wall_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) /** - * @brief The maze information the robot has + * @brief The maze information the robot has. */ TMaze maze; /** - * @brief Thickness of the maze walls + * @brief Thickness of the maze walls. */ float wall_thickness; /** - * @brief Size of the cells in the maze + * @brief Size of the cells in the maze. */ float cell_size; /** - * @brief Pose of the front wall sensors + * @brief Pose of the front wall sensors. */ Pose front_sensor_pose; /** - * @brief Pose of the side wall sensors + * @brief Pose of the side wall sensors. */ Pose side_sensor_pose; /** - * @brief Region of the cell to consider the information of the front sensors to the current cell + * @brief Region of the cell to consider the information of the front sensors to the current cell. */ float front_sensors_region_division; /** - * @brief Region of the cell to consider the information of the side sensors to the current cell + * @brief Region of the cell to consider the information of the side sensors to the current cell. */ float side_sensors_region_division; /** - * @brief Sensor readings tolerance for the front alignment of the robot + * @brief Sensor readings tolerance for the front alignment of the robot. */ float front_distance_alignment_tolerance; /** - * @brief Sensor readings tolerance for the side alignment of the robot + * @brief Sensor readings tolerance for the side alignment of the robot. */ float side_distance_alignment_tolerance; /** - * @brief Sensor readings for the front alignment of the robot at the front + * @brief Sensor readings for the front alignment of the robot at the front. */ float front_orientation_alignment_tolerance; /** - * @brief Sensor readings for the front alignment of the robot with walls at the side + * @brief Sensor readings for the front alignment of the robot with walls at the side. */ float side_orientation_alignment_tolerance; /** - * @brief Sensor readings when the front of the robot is distance aligned + * @brief Sensor readings when the front of the robot is distance aligned. */ std::array front_distance_reading{}; /** - * @brief Sensor readings when the front of the robot is orientation aligned + * @brief Sensor readings when the front of the robot is orientation aligned. */ std::array front_orientation_reading{}; /** - * @brief Sensor readings when the sides of the robot are distance aligned + * @brief Sensor readings when the sides of the robot are distance aligned. */ std::array side_distance_reading{}; /** - * @brief List of points and directions that represent the best route to the goal + * @brief List of points and directions that represent the best route to the goal. */ std::list> best_route; /** - * @brief Current iterator of the best route + * @brief Current iterator of the best route. */ std::list>::iterator best_route_iterator{best_route.begin()}; }; diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 8ad1af8..5b37765 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -14,99 +14,98 @@ namespace micras::nav { /** - * @brief Class for storing the robot information about the maze + * @brief Class for storing the robot information about the maze. * - * @tparam width The width of the maze - * @tparam height The height of the maze + * @tparam width The width of the maze. + * @tparam height The height of the maze. */ template class TMaze { public: /** - * @brief Construct a new Maze object + * @brief Construct a new Maze object. * - * @param start The start pose of the robot - * @param goal The goal points in the maze + * @param start The start pose of the robot. + * @param goal The goal points in the maze. */ TMaze(const GridPose& start, const std::unordered_set& goal); /** - * @brief Updates the maze walls with the current pose and new information + * @brief Updates the maze walls with the current pose and new information. * - * @param pose The pose of the robot - * @param information The information from the wall sensors + * @param pose The pose of the robot. + * @param information The information from the wall sensors. */ void update(const GridPose& pose, Information information); /** - * @brief Returns the next point the robot should go to when exploring + * @brief Returns the next point the robot should go to when exploring. * - * @param position The current position of the robot - * @return The next point the robot should go to when exploring + * @param position The current position of the robot. + * @return The next point the robot should go to when exploring. */ GridPose get_current_exploration_goal(const GridPoint& position) const; /** - * @brief Returns the next point the robot should go to when returning + * @brief Returns the next point the robot should go to when returning. * - * @param position The current position of the robot - * @return The next point the robot should go to when returning + * @param position The current position of the robot. + * @return The next point the robot should go to when returning. */ GridPose get_current_returning_goal(const GridPoint& position) const; /** - * @brief Checks the type of wall following the robot can do + * @brief Checks the type of wall following the robot can do. * - * @param pose The pose of the robot - * @param front_cell Whether to check the cell at the front of the robot - * @return The type of wall following + * @param pose The pose of the robot. + * @return The type of wall following. */ core::FollowWallType get_follow_wall_type(const GridPose& pose) const; /** - * @brief Checks whether the robot has finished the maze + * @brief Checks whether the robot has finished the maze. * - * @param position The current position of the robot - * @return True if the robot has finished the maze, false otherwise + * @param position The current position of the robot. + * @return True if the robot has finished the maze, false otherwise. */ bool finished(const GridPoint& position) const; /** - * @brief Checks whether the robot has returned to the start + * @brief Checks whether the robot has returned to the start. * - * @param position The current position of the robot - * @return True if the robot has returned to the start, false otherwise + * @param position The current position of the robot. + * @return True if the robot has returned to the start, false otherwise. */ bool returned(const GridPoint& position) const; /** - * @brief Calculates the best route to the goal using the current costmap + * @brief Calculates the best route to the goal using the current costmap. */ void calculate_best_route(); /** - * @brief Improves the route to the goal + * @brief Improves the route to the goal. */ void optimize_route(); /** - * @brief Returns the best route to the goal + * @brief Returns the best route to the goal. * - * @return The best route to the goal + * @return The best route to the goal. */ const std::map>& get_best_route() const; /** - * @brief Checks whether there is a wall at the front of a given pose + * @brief Checks whether there is a wall at the front of a given pose. * - * @param pose The pose to check - * @return True if there is a wall, false otherwise + * @param pose The pose to check. + * @return True if there is a wall, false otherwise. */ bool has_wall(const GridPose& pose) const; private: /** - * @brief Type to store the information of a cell in the maze + * @brief Type to store the information of a cell in the maze. */ struct Cell { std::array wall_count{}; @@ -115,52 +114,52 @@ class TMaze { }; /** - * @brief Calculates the costmap for the flood fill algorithm + * @brief Calculates the costmap for the flood fill algorithm. */ void calculate_costmap(); /** - * @brief Returns the cell at the given position + * @brief Returns the cell at the given position. * - * @param position The position of the cell - * @return The cell at the given position + * @param position The position of the cell. + * @return The cell at the given position. */ const Cell& get_cell(const GridPoint& position) const; /** - * @brief Returns the cell at the given position + * @brief Returns the cell at the given position. * - * @param position The position of the cell - * @return The cell at the given position + * @param position The position of the cell. + * @return The cell at the given position. */ Cell& get_cell(const GridPoint& position); /** - * @brief Update the probability of a wall in the maze + * @brief Update the probability of a wall in the maze. * - * @param pose The pose of the robot - * @param wall Whether there is a wall in front of the robot - * @return True if the information is new, false otherwise + * @param pose The pose of the robot. + * @param wall Whether there is a wall in front of the robot. + * @return True if the information is new, false otherwise. */ bool update_wall(const GridPose& pose, bool wall); /** - * @brief Cells matrix representing the maze + * @brief Cells matrix representing the maze. */ std::array, height> cells{}; /** - * @brief Start pose of the robot in the maze + * @brief Start pose of the robot in the maze. */ GridPose start; /** - * @brief Goal points in the maze + * @brief Goal points in the maze. */ std::unordered_set goal; /** - * @brief Current best found route to the goal + * @brief Current best found route to the goal. */ std::map> best_route; }; diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index bb8d0a9..f7e8cd9 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -15,12 +15,12 @@ namespace micras::nav { /** - * @brief Class for calculating the robot odometry + * @brief Class for calculating the robot odometry. */ class Odometry { public: /** - * @brief Configuration for the odometry + * @brief Configuration for the odometry. */ struct Config { float linear_cutoff_frequency; @@ -29,11 +29,12 @@ class Odometry { }; /** - * @brief Constructor for the Odometry class + * @brief Construct a newOdometry object. * - * @param left_rotary_sensor Left rotary sensor - * @param right_rotary_sensor Right rotary sensor - * @param config Configuration for the odometry + * @param left_rotary_sensor Left rotary sensor. + * @param right_rotary_sensor Right rotary sensor. + * @param imu IMU sensor. + * @param config Configuration for the odometry. */ Odometry( const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, @@ -41,69 +42,69 @@ class Odometry { ); /** - * @brief Update the odometry + * @brief Update the odometry. * - * @param elapsed_time Time since the last update + * @param elapsed_time Time since the last update. */ void update(float elapsed_time); /** - * @brief Reset the odometry + * @brief Reset the odometry. */ void reset(); /** - * @brief Get the state of the robot + * @brief Get the state of the robot. * - * @return State current state of the robot in space + * @return Current state of the robot in space. */ const State& get_state() const; /** - * @brief Set the state of the robot + * @brief Set the state of the robot. * - * @param state New state of the robot + * @param state New state of the robot. */ void set_state(const State& new_state); private: /** - * @brief Left rotary sensor + * @brief Left rotary sensor. */ const proxy::RotarySensor& left_rotary_sensor; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) /** - * @brief Right rotary sensor + * @brief Right rotary sensor. */ const proxy::RotarySensor& right_rotary_sensor; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) /** - * @brief IMU sensor + * @brief IMU sensor. */ const proxy::Imu& imu; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) /** - * @brief Wheel radius + * @brief Wheel radius. */ float wheel_radius; /** - * @brief Last left rotary sensor position + * @brief Last left rotary sensor position. */ float left_last_position{}; /** - * @brief Last right rotary sensor position + * @brief Last right rotary sensor position. */ float right_last_position{}; /** - * @brief Linear velocity filter + * @brief Linear velocity filter. */ core::ButterworthFilter linear_filter; /** - * @brief Current state of the robot in space + * @brief Current state of the robot in space. */ State state; diff --git a/micras_nav/include/micras/nav/pid_controller.hpp b/micras_nav/include/micras/nav/pid_controller.hpp index a056b16..e3742ef 100644 --- a/micras_nav/include/micras/nav/pid_controller.hpp +++ b/micras_nav/include/micras/nav/pid_controller.hpp @@ -9,72 +9,107 @@ namespace micras::nav { /** - * @brief Implementation of simple PID controller - * Response = Kp(error + Ki * integral(error) Kd * d/dt(error)) + * @brief Implementation of simple PID controller. Response = kp * (error + hi * integral(error) kd * d/dt(error)) */ class PidController { public: + /** + * @brief Configuration struct for the PID controller. + */ struct Config { - float kp{}; /**< Proportional constant */ - float ki{}; /**< Integrative constant */ - float kd{}; /**< Derivative constant */ - float setpoint{}; /**< Desired state */ - float saturation{-1.0F}; /**< Maximum response returned by the controller */ - float max_integral{-1.0F}; /**< Maximum integrative response */ + float kp{}; + float ki{}; + float kd{}; + float setpoint{}; + float saturation{-1.0F}; + float max_integral{-1.0F}; }; /** - * @brief Construct a new Pid Controller object + * @brief Construct a new Pid Controller object. * - * @param config Controller parameters + * @param config Controller parameters. */ explicit PidController(Config config); /** - * @brief Set the setpoint object + * @brief Set the desired setpoint. * - * @param setpoint Desired state + * @param setpoint Desired state. */ void set_setpoint(float setpoint); /** - * @brief Reset prev_error and error_acc objects + * @brief Reset prev_error and error_acc objects. */ void reset(); /** - * @brief Update PID with new state and return response + * @brief Update PID with new state and return response. * - * @param state Current value of the controlled variable - * @param elapsed_time Time since the last update - * @param save Whether to save the variables for calibration - * - * @return Response + * @param state Current value of the controlled variable. + * @param elapsed_time Time since the last update. + * @param save Whether to save the variables for calibration. + * @return Response of the controller. */ float update(float state, float elapsed_time, bool save = false); /** - * @brief Update PID with new state and return response - * - * @param state Current value of the controlled variable - * @param state_change Derivative of the controlled variable - * @param elapsed_time Time since the last update - * @param save Whether to save the variables for calibration + * @brief Update PID with new state and return response. * - * @return Response + * @param state Current value of the controlled variable. + * @param state_change Derivative of the controlled variable. + * @param elapsed_time Time since the last update. + * @param save Whether to save the variables for calibration. + * @return Response of the controller. */ float update(float state, float elapsed_time, float state_change, bool save = false); private: - float kp; /**< Proportional constant */ - float ki; /**< Integrative constant */ - float kd; /**< Derivative constant */ - float setpoint; /**< Desired state */ - float saturation; /**< Maximum response returned by the controller */ - float max_integral; /**< Maximum integrative accumulative response */ - float error_acc = 0; /**< Accumulated error for i term */ - float prev_state = 0; /**< Previous state for d term */ - float last_response = 0; /**< Last response returned by the controller */ + /** + * @brief Proportional constant. + */ + float kp; + + /** + * @brief Integrative constant. + */ + float ki; + + /** + * @brief Derivative constant. + */ + float kd; + + /** + * @brief Desired state. + */ + float setpoint; + + /** + * @brief Maximum response returned by the controller. + */ + float saturation; + + /** + * @brief Maximum integrative response. + */ + float max_integral; + + /** + * @brief Accumulated error for integrative term. + */ + float error_acc = 0; + + /** + * @brief Previous error for derivative term. + */ + float prev_state = 0; + + /** + * @brief Last response returned by the controller. + */ + float last_response = 0; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index 6220aac..a8c5a51 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -9,7 +9,7 @@ namespace micras::nav { /** - * @brief Possible directions in the maze + * @brief Possible directions in the maze. */ enum Direction : uint8_t { EAST = 0, @@ -23,151 +23,151 @@ enum Direction : uint8_t { }; /** - * @brief Type to store a point in 2D space + * @brief Type to store a point in 2D space. */ struct Point { /** - * @brief Calculates the distance to another point + * @brief Calculates the distance to another point. * - * @param other The other point - * @return The distance to the other point + * @param other The other point. + * @return The distance to the other point. */ float distance(const Point& other) const; /** - * @brief Calculates the angle between two points + * @brief Calculates the angle between two points. * - * @param other The other point - * @return The angle between the two points + * @param other The other point. + * @return The angle between the two points. */ float angle_between(const Point& other) const; /** - * @brief Converts the point to a grid point + * @brief Converts the point to a grid point. * - * @param cell_size The size of the grid cells - * @return The grid point corresponding to the point + * @param cell_size The size of the grid cells. + * @return The grid point corresponding to the point. */ GridPoint to_grid(float cell_size) const; /** - * @brief Rotates the point by a given angle + * @brief Rotates the point by a given angle. * - * @param angle The angle to rotate the point by - * @return The rotated point + * @param angle The angle to rotate the point by. + * @return The rotated point. */ Point rotate(Direction angle); /** - * @brief Moves the point towards another point by a given distance + * @brief Moves the point towards another point by a given distance. * - * @param other The point to move towards - * @param distance The distance to move - * @return The point after moving towards the other point + * @param other The point to move towards. + * @param distance The distance to move. + * @return The point after moving towards the other point. */ Point move_towards(const Point& other, float distance) const; /** - * @brief Subtracts a point from another + * @brief Subtracts a point from another. * - * @param other The point to subtract - * @return The result of the subtraction + * @param other The point to subtract. + * @return The result of the subtraction. */ Point operator-(const Point& other) const; /** - * @brief Compares two points for equality + * @brief Compares two points for equality. * - * @param other The other point to compare - * @return True if the points are equal, false otherwise + * @param other The other point to compare. + * @return True if the points are equal, false otherwise. */ bool operator==(const Point& other) const; /** - * @brief Calculates the remainder of the division of the point by a value + * @brief Calculates the remainder of the division of the point by a value. * - * @param value The value to divide the point by - * @return The remainder of the division + * @param value The value to divide the point by. + * @return The remainder of the division. */ Point operator%(float value) const; /** - * @brief Converts a grid point to a point + * @brief Converts a grid point to a point. * - * @param grid_point The grid point to convert - * @param cell_size The size of the grid cells - * @return The point corresponding to the grid point + * @param grid_point The grid point to convert. + * @param cell_size The size of the grid cells. + * @return The point corresponding to the grid point. */ static Point from_grid(const GridPoint& grid_point, float cell_size); /** - * @brief The x coordinate of the point in 2D space + * @brief The x coordinate of the point in 2D space. */ float x; /** - * @brief The y coordinate of the point in 2D space + * @brief The y coordinate of the point in 2D space. */ float y; }; /** - * @brief Type to store a pose in 2D space + * @brief Type to store a pose in 2D space. */ struct Pose { /** - * @brief Converts the pose to a grid pose + * @brief Converts the pose to a grid pose. * - * @param cell_size The size of the grid cells - * @return The grid pose corresponding to the pose + * @param cell_size The size of the grid cells. + * @return The grid pose corresponding to the pose. */ GridPose to_grid(float cell_size) const; /** - * @brief Converts the pose to a cell reference + * @brief Converts the pose to a cell reference. * - * @param cell_size The size of the grid cells - * @return The point inside the cell reference + * @param cell_size The size of the grid cells. + * @return The point inside the cell reference. */ Point to_cell(float cell_size) const; /** - * @brief The position of the pose + * @brief The position of the pose. */ Point position; /** - * @brief The orientation of the pose + * @brief The orientation of the pose. */ float orientation; }; /** - * @brief Type to store a twist in 2D space + * @brief Type to store a twist in 2D space. */ struct Twist { /** - * @brief The linear velocity of the twist + * @brief The linear velocity of the twist. */ float linear; /** - * @brief The angular velocity of the twist + * @brief The angular velocity of the twist. */ float angular; }; /** - * @brief Type to store the state of the robot + * @brief Type to store the state of the robot. */ struct State { /** - * @brief The pose of the robot + * @brief The pose of the robot. */ Pose pose; /** - * @brief The velocity of the robot + * @brief The velocity of the robot. */ Twist velocity; }; diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index 2bbd3cf..1299d5a 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -12,13 +12,13 @@ namespace micras::proxy { /** - * @brief Class for controlling an addressable RGB LED + * @brief Class for controlling an addressable RGB LED. */ template class TArgb { public: /** - * @brief Configuration structure for the addressable RGB LED + * @brief Configuration struct for the addressable RGB LED. */ struct Config { hal::PwmDma::Config pwm; @@ -28,7 +28,7 @@ class TArgb { }; /** - * @brief Structure for storing color information + * @brief Struct for storing color information. */ struct Color { uint8_t red; @@ -45,8 +45,9 @@ class TArgb { }; /** - * @brief Predefined colors + * @brief Predefined colors. */ + ///@{ static constexpr Color red{255, 0, 0}; static constexpr Color green{0, 255, 0}; static constexpr Color blue{0, 0, 255}; @@ -54,87 +55,90 @@ class TArgb { static constexpr Color cyan{0, 255, 255}; static constexpr Color magenta{255, 0, 255}; static constexpr Color white{255, 255, 255}; + ///@} /** - * @brief Constructor for the Argb class + * @brief Construct a new Argb object. * - * @param config Configuration for the addressable RGB LED + * @param config Configuration for the addressable RGB LED. */ explicit TArgb(const Config& config); /** - * @brief Set the color of the ARGB at the specified index + * @brief Set the color of the ARGB at the specified index. * - * @param index The index of the ARGB to set the color of - * @param color The color to set the ARGB to + * @param index The index of the ARGB to set the color of. + * @param color The color to set the ARGB to. */ void set_color(const Color& color, uint8_t index); /** - * @brief Set the color of all ARGBs + * @brief Set the color of all ARGBs. * - * @param color The color to set all ARGBs to + * @param color The color to set all ARGBs to. */ void set_color(const Color& color); /** - * @brief Set the colors of all ARGBs + * @brief Set the colors of all ARGBs. * - * @param colors The colors to set the ARGBs to + * @param colors The colors to set the ARGBs to. */ void set_colors(const std::array& colors); /** - * @brief Turn off the ARGB at the specified index + * @brief Turn off the ARGB at the specified index. * - * @param index The index of the ARGB to turn off + * @param index The index of the ARGB to turn off. */ void turn_off(uint8_t index); /** - * @brief Turn off all ARGBs + * @brief Turn off all ARGBs. */ void turn_off(); private: /** - * @brief Encode a color into the data buffer + * @brief Encode a color into the data buffer. * - * @param color The color to encode - * @param index The index to encode the color at + * @param color The color to encode. + * @param index The index to encode the color at. */ void encode_color(const Color& color, uint8_t index); /** - * @brief Protocol constants for the addressable RGB LED communication + * @brief Protocol constants for the addressable RGB LED communication. */ + ///@{ static constexpr uint8_t bits_per_color{8}; static constexpr uint8_t colors_per_led{3}; static constexpr uint8_t bits_per_led{bits_per_color * colors_per_led}; static constexpr uint8_t reset_length{225}; + ///@} /** - * @brief PWM control object + * @brief PWM control object. */ hal::PwmDma pwm; /** - * @brief PWM autoreload value for the low signal + * @brief PWM autoreload value for the low signal. */ uint32_t low_bit; /** - * @brief PWM autoreload value for the high signal + * @brief PWM autoreload value for the high signal. */ uint32_t high_bit; /** - * @brief Maximum brightness of the addressable RGB LED + * @brief Maximum brightness of the addressable RGB LED. */ float brightness; /** - * @brief Data buffer to send to the addressable RGB LED + * @brief Data buffer to send to the addressable RGB LED. */ std::array buffer{}; }; diff --git a/micras_proxy/include/micras/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp index 2c931e5..e6d71d0 100644 --- a/micras_proxy/include/micras/proxy/battery.hpp +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -12,12 +12,12 @@ namespace micras::proxy { /** - * @brief Class for getting the battery voltage + * @brief Class for getting the battery voltage. */ class Battery { public: /** - * @brief Configuration structure for the battery + * @brief Configuration struct for the battery. */ struct Config { hal::AdcDma::Config adc; @@ -26,56 +26,56 @@ class Battery { }; /** - * @brief Constructor for the Battery class + * @brief Construct a new Battery object. * - * @param config Configuration for the battery + * @param config Configuration for the battery. */ explicit Battery(const Config& config); /** - * @brief Update the battery reading + * @brief Update the battery reading. */ void update(); /** - * @brief Get the battery voltage + * @brief Get the battery voltage. * - * @return float Battery voltage in volts + * @return Battery voltage in volts. */ float get_voltage() const; /** - * @brief Get the battery voltage in volts without the filter applied + * @brief Get the battery voltage in volts without the filter applied. * - * @return float Battery voltage in volts + * @return Battery voltage in volts. */ float get_voltage_raw() const; /** - * @brief Get the battery reading from the ADC + * @brief Get the battery reading from the ADC. * - * @return float Battery reading from 0 to 1 + * @return Battery reading from 0 to 1. */ float get_adc_reading() const; private: /** - * @brief Adc object + * @brief Adc object. */ hal::AdcDma adc; /** - * @brief Raw reading from the battery + * @brief Raw reading from the battery. */ uint16_t raw_reading{}; /** - * @brief Maximum voltage that can be read + * @brief Maximum voltage that can be read. */ float max_voltage; /** - * @brief Butterworth filter for the battery reading + * @brief Butterworth filter for the battery reading. */ core::ButterworthFilter filter; }; diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index e3440d6..709d371 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -12,12 +12,12 @@ namespace micras::proxy { /** - * @brief Class for acquiring a button data + * @brief Class for acquiring button data. */ class Button { public: /** - * @brief Enum for button status + * @brief Enum for button status. */ enum Status : uint8_t { NO_PRESS, @@ -27,7 +27,7 @@ class Button { }; /** - * @brief Enum for button pull resistor + * @brief Enum for button pull resistor. */ enum PullResistor : uint8_t { PULL_UP, @@ -35,7 +35,7 @@ class Button { }; /** - * @brief Configuration structure for button + * @brief Configuration struct for the button. */ struct Config { hal::Gpio::Config gpio{}; @@ -46,94 +46,96 @@ class Button { }; /** - * @brief Constructor for Button class + * @brief Construct a new Button object. * - * @param config Button configuration + * @param config Button configuration. */ explicit Button(const Config& config); /** - * @brief Check if button is pressed + * @brief Check if button is pressed. * - * @return bool True if button is pressed, false otherwise + * @return True if button is pressed, false otherwise. */ bool is_pressed(); /** - * @brief Get button status + * @brief Get button status. * - * @return Status Button status + * @return Current button status. */ Status get_status(); private: /** - * @brief Get raw button reading + * @brief Get raw button reading. * - * @return bool Button reading without debounce + * @return Button reading without debounce. */ bool get_raw_reading() const; /** - * @brief Update button state + * @brief Update button state. * - * @return Current state of the button + * @return Current state of the button. */ bool update_state(); /** - * @brief Check if button was just pressed + * @brief Check if button was just pressed. * - * @return True if button was just pressed, false otherwise + * @return True if button was just pressed, false otherwise. */ bool is_rising_edge() const; /** - * @brief Check if button was just released + * @brief Check if button was just released. * - * @return True if button was just released, false otherwise + * @return True if button was just released, false otherwise. */ bool is_falling_edge() const; /** - * @brief Button pressing delays in ms + * @brief Button pressing delays in ms. */ + ///@{ uint16_t debounce_delay; uint16_t long_press_delay; uint16_t extra_long_press_delay; + ///@} /** - * @brief Gpio object for button + * @brief Gpio object for button. */ hal::Gpio gpio; /** - * @brief Pull resistor configuration + * @brief Pull resistor configuration. */ PullResistor pull_resistor; /** - * @brief Timer to check if button is debouncing + * @brief Timer to check if button is debouncing. */ hal::Timer debounce_timer; /** - * @brief Timer to determine type of button press + * @brief Timer to determine type of button press. */ hal::Timer status_timer; /** - * @brief Flag to know when button is debouncing + * @brief Flag to know when button is debouncing. */ bool is_debouncing{false}; /** - * @brief Flag to know if button was being pressed + * @brief Flag to know if button was being pressed. */ bool previous_state{false}; /** - * @brief Flag to know if button is being pressed + * @brief Flag to know if button is being pressed. */ bool current_state{false}; }; diff --git a/micras_proxy/include/micras/proxy/buzzer.hpp b/micras_proxy/include/micras/proxy/buzzer.hpp index beec2da..a2f9d01 100644 --- a/micras_proxy/include/micras/proxy/buzzer.hpp +++ b/micras_proxy/include/micras/proxy/buzzer.hpp @@ -12,67 +12,67 @@ namespace micras::proxy { /** - * @brief Class for controlling a buzzer + * @brief Class for controlling a buzzer. */ class Buzzer { public: /** - * @brief Configuration structure for the buzzer + * @brief Configuration struct for the buzzer. */ struct Config { hal::Pwm::Config pwm; }; /** - * @brief Constructor for the Buzzer class + * @brief Construct a new Buzzer object. * - * @param config Configuration for the buzzer + * @param config Configuration for the buzzer. */ explicit Buzzer(const Config& config); /** - * @brief Play a tone for a duration + * @brief Play a tone for a duration. * - * @param frequency Buzzer sound frequency in Hz - * @param duration Duration of the sound in ms + * @param frequency Buzzer sound frequency in Hz. + * @param duration Duration of the sound in ms. */ void play(uint32_t frequency, uint32_t duration = 0); /** - * @brief Update the buzzer state + * @brief Update the buzzer state. */ void update(); /** - * @brief Stop the buzzer sound + * @brief Stop the buzzer sound. */ void stop(); /** - * @brief Wait for a time interval updating the buzzer + * @brief Wait for a time interval updating the buzzer. * - * @param interval Time to wait in ms + * @param interval Time to wait in ms. */ void wait(uint32_t interval); private: /** - * @brief PWM object + * @brief PWM object. */ hal::Pwm pwm; /** - * @brief Timer to play the sound + * @brief Timer to play the sound. */ hal::Timer timer; /** - * @brief Flag to check if the buzzer is playing + * @brief Flag to check if the buzzer is playing. */ bool is_playing{}; /** - * @brief Duration of the sound + * @brief Duration of the sound. */ uint32_t duration{}; }; diff --git a/micras_proxy/include/micras/proxy/dip_switch.hpp b/micras_proxy/include/micras/proxy/dip_switch.hpp index 6c4c2d7..7bfa4ed 100644 --- a/micras_proxy/include/micras/proxy/dip_switch.hpp +++ b/micras_proxy/include/micras/proxy/dip_switch.hpp @@ -12,43 +12,43 @@ namespace micras::proxy { /** - * @brief Class for acquiring a dip switch data + * @brief Class for acquiring dip switch data. */ template class TDipSwitch { public: /** - * @brief Configuration struct for DipSwitch + * @brief Configuration struct for the Dip Switch. */ struct Config { std::array gpio_array; }; /** - * @brief Construct a new Dip Switch object + * @brief Construct a new Dip Switch object. * - * @param config Configuration struct for DipSwitch + * @param config Configuration struct for the DipSwitch. */ explicit TDipSwitch(const Config& config); /** - * @brief Get the state of a switch + * @brief Get the state of a switch. * - * @param switch_index Index of the switch - * @return bool True if the switch is on, false otherwise + * @param switch_index Index of the switch. + * @return True if the switch is on, false otherwise. */ bool get_switch_state(uint8_t switch_index) const; /** - * @brief Get the value of all switches + * @brief Get the value of all switches. * - * @return uint8_t Value of all switches + * @return Value of all switches. */ uint8_t get_switches_value() const; private: /** - * @brief Array of GPIOs for the switches + * @brief Array of GPIOs for the switches. */ std::array gpio_array; }; diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index 277587d..3a784a2 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -13,12 +13,12 @@ namespace micras::proxy { /** - * @brief Class for controlling the fan driver + * @brief Class for controlling the fan driver. */ class Fan { public: /** - * @brief Configuration structure for the fan + * @brief Configuration struct for the fan. */ struct Config { hal::Pwm::Config pwm; @@ -28,42 +28,42 @@ class Fan { }; /** - * @brief Construct a new fan object + * @brief Construct a new fan object. * - * @param config Configuration for the fan driver + * @param config Configuration for the fan driver. */ explicit Fan(const Config& config); /** - * @brief Enable the fan + * @brief Enable the fan. */ void enable(); /** - * @brief Disable the fan + * @brief Disable the fan. */ void disable(); /** - * @brief Set the speed of the fans + * @brief Set the speed of the fans. * - * @param speed Speed percentage of the fan + * @param speed Speed percentage of the fan. */ void set_speed(float speed); /** - * @brief Update the speed of the fan + * @brief Update the speed of the fan. */ float update(); /** - * @brief Stop the fan + * @brief Stop the fan. */ void stop(); private: /** - * @brief Enum for rotation direction + * @brief Enum for rotation direction. */ enum RotationDirection : uint8_t { FORWARD, @@ -71,44 +71,44 @@ class Fan { }; /** - * @brief Set the rotation direction of the fan + * @brief Set the rotation direction of the fan. * - * @param direction Rotation direction + * @param direction Rotation direction. */ void set_direction(RotationDirection direction); /** - * @brief PWM object for controlling the fan speed + * @brief PWM object for controlling the fan speed. */ hal::Pwm pwm; /** - * @brief GPIO object for controlling the fan rotation direction + * @brief GPIO object for controlling the fan rotation direction. */ hal::Gpio direction_gpio; /** - * @brief GPIO handle for the fan enable pin + * @brief GPIO handle for the fan enable pin. */ hal::Gpio enable_gpio; /** - * @brief Current speed of the fan + * @brief Current speed of the fan. */ float current_speed{}; /** - * @brief Target speed of the fan + * @brief Target speed of the fan. */ float target_speed{}; /** - * @brief Maximum acceleration of the fan in percentage per millisecond + * @brief Maximum acceleration of the fan in percentage per millisecond. */ float max_acceleration; /** - * @brief Timer for limiting the acceleration of the fan + * @brief Timer for limiting the acceleration of the fan. */ hal::Timer acceleration_timer; }; diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 19d1c07..5ce0d54 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -15,12 +15,12 @@ namespace micras::proxy { /** - * @brief Class to handle IMU peripheral on STM32 microcontrollers + * @brief Class for acquiring IMU data. */ class Imu { public: /** - * @brief IMU configuration struct + * @brief IMU configuration struct. */ struct Config { hal::Spi::Config spi; @@ -34,12 +34,8 @@ class Imu { }; /** - * @brief Construct a new Imu object - * - * @param config Configuration for the IMU + * @brief Enum to select the axis of the IMU. */ - explicit Imu(const Config& config); - enum Axis : uint8_t { X, Y, @@ -47,108 +43,111 @@ class Imu { }; /** - * @brief Check the IMU device + * @brief Construct a new Imu object. + * + * @param config Configuration for the IMU. + */ + explicit Imu(const Config& config); + + /** + * @brief Check the IMU device. * - * @return bool true if the device is correct, false otherwise + * @return True if the device is correct, false otherwise. */ bool check_whoami(); /** - * @brief Update the IMU data + * @brief Update the IMU data. */ void update(); /** - * @brief Get the IMU angular velocity over an axis - * - * @param axis Axis to get the angular velocity from + * @brief Get the IMU angular velocity over an axis. * - * @return float Angular velocity over the desired axis in rad/s + * @param axis Axis to get the angular velocity from. + * @return Angular velocity over the desired axis in rad/s. */ float get_angular_velocity(Axis axis) const; /** - * @brief Get the IMU linear acceleration over an axis + * @brief Get the IMU linear acceleration over an axis. * - * @param axis Axis to get the linear acceleration from - * - * @return float Linear acceleration over the desired axis in m/sยฒ + * @param axis Axis to get the linear acceleration from. + * @return Linear acceleration over the desired axis in m/sยฒ. */ float get_linear_acceleration(Axis axis) const; /** - * @brief Define the base reading to be removed from the IMU value + * @brief Define the base reading to be removed from the IMU value. */ void calibrate(); private: /** - * @brief Read data from the IMU - * - * @param handle Pointer to a SPI object - * @param reg Register to read from - * @param bufp Buffer to read - * @param len Length of the buffer + * @brief Read data from the IMU. * - * @return int32_t 0 if the operation was successful, -1 otherwise + * @param handle Pointer to a SPI object. + * @param reg Register to read from. + * @param bufp Buffer to read. + * @param len Length of the buffer. + * @return 0 if the operation was successful, -1 otherwise. */ static int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len); /** - * @brief Write data to the IMU - * - * @param handle Pointer to a SPI object - * @param reg Register to write to - * @param bufp Buffer to write - * @param len Length of the buffer + * @brief Write data to the IMU. * - * @return int32_t 0 if the operation was successful, -1 otherwise + * @param handle Pointer to a SPI object. + * @param reg Register to write to. + * @param bufp Buffer to write. + * @param len Length of the buffer. + * @return 0 if the operation was successful, -1 otherwise. */ static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len); /** - * @brief Conversion constants + * @brief Conversion constants. */ static constexpr float mdps_to_radps{std::numbers::pi_v / 180000.0F}; static constexpr float mg_to_mps2{0.00980665F}; /** - * @brief SPI for the IMU communication + * @brief SPI for the IMU communication. */ hal::Spi spi; /** - * @brief Device context for the IMU library + * @brief Device context for the IMU library. */ stmdev_ctx_t dev_ctx{}; /** - * @brief Current angular velocity on each axis + * @brief Current angular velocity on each axis. */ std::array angular_velocity{}; /** - * @brief Current linear acceleration on each axis + * @brief Current linear acceleration on each axis. */ std::array linear_acceleration{}; /** - * @brief Gyroscope conversion factor + * @brief Gyroscope conversion factor. */ float gy_factor; /** - * @brief Accelerometer conversion factor + * @brief Accelerometer conversion factor. */ float xl_factor; /** - * @brief Gyroscope Butterworth filter for the calibration + * @brief Gyroscope Butterworth filter for the calibration. */ core::ButterworthFilter calibration_filter{5.0F}; /** - * @brief Flag to check if the IMU is calibrated + * @brief Flag to check if the IMU is calibrated. */ bool calibrated{}; }; diff --git a/micras_proxy/include/micras/proxy/led.hpp b/micras_proxy/include/micras/proxy/led.hpp index 868b9cc..f4f2a3e 100644 --- a/micras_proxy/include/micras/proxy/led.hpp +++ b/micras_proxy/include/micras/proxy/led.hpp @@ -9,42 +9,42 @@ namespace micras::proxy { /** - * @brief Class for controlling an LED + * @brief Class for controlling an LED. */ class Led { public: /** - * @brief Configuration structure for LED + * @brief Configuration struct for LED. */ struct Config { hal::Gpio::Config gpio; }; /** - * @brief Constructor for the Led class + * @brief Construct a new Led object. * - * @param config Configuration for the LED + * @param config Configuration for the LED. */ explicit Led(const Config& config); /** - * @brief Turn the LED on + * @brief Turn the LED on. */ void turn_on(); /** - * @brief Turn the LED off + * @brief Turn the LED off. */ void turn_off(); /** - * @brief Toggle the LED + * @brief Toggle the LED. */ void toggle(); private: /** - * @brief Gpio object for the LED + * @brief Gpio object for the LED. */ hal::Gpio gpio; }; diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index ec99675..9344c46 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -12,12 +12,12 @@ namespace micras::proxy { /** - * @brief Class for controlling the locomotion driver + * @brief Class for controlling the locomotion driver. */ class Locomotion { public: /** - * @brief Configuration structure for the locomotion + * @brief Configuration struct for the locomotion. */ struct Config { Motor::Config left_motor; @@ -26,56 +26,56 @@ class Locomotion { }; /** - * @brief Construct a new locomotion object + * @brief Construct a new locomotion object. * - * @param config Configuration for the locomotion driver + * @param config Configuration for the locomotion driver. */ explicit Locomotion(const Config& config); /** - * @brief Enable the locomotion driver + * @brief Enable the locomotion driver. */ void enable(); /** - * @brief Disable the locomotion driver + * @brief Disable the locomotion driver. */ void disable(); /** - * @brief Set the command of the wheels + * @brief Set the command of the wheels. * - * @param left_command Command of the left wheels - * @param right_command Command of the right wheels + * @param left_command Command of the left wheels. + * @param right_command Command of the right wheels. */ void set_wheel_command(float left_command, float right_command); /** - * @brief Set the linear and angular commands of the robot + * @brief Set the linear and angular commands of the robot. * - * @param linear Linear command of the robot - * @param angular Angular command of the robot + * @param linear Linear command of the robot. + * @param angular Angular command of the robot. */ void set_command(float linear, float angular); /** - * @brief Stop the motors + * @brief Stop the motors. */ void stop(); private: /** - * @brief Left motor of the robot + * @brief Left motor of the robot. */ Motor left_motor; /** - * @brief Right motor of the robot + * @brief Right motor of the robot. */ Motor right_motor; /** - * @brief GPIO handle for the motor driver enable pin + * @brief GPIO handle for the motor driver enable pin. */ hal::Gpio enable_gpio; }; diff --git a/micras_proxy/include/micras/proxy/motor.hpp b/micras_proxy/include/micras/proxy/motor.hpp index 8bf7117..8d1b2a7 100644 --- a/micras_proxy/include/micras/proxy/motor.hpp +++ b/micras_proxy/include/micras/proxy/motor.hpp @@ -9,12 +9,12 @@ namespace micras::proxy { /** - * @brief Class for controlling a motor driver + * @brief Class for controlling a motor driver. */ class Motor { public: /** - * @brief Configuration structure for the motor + * @brief Configuration struct for the motor. */ struct Config { hal::Pwm::Config backwards_pwm; @@ -24,37 +24,37 @@ class Motor { }; /** - * @brief Construct a new motor object + * @brief Construct a new motor object. * - * @param config Configuration for the motor driver + * @param config Configuration for the motor driver. */ explicit Motor(const Config& config); /** - * @brief Set the command for the motor + * @brief Set the command for the motor. * - * @param command Command for the motor in percentage + * @param command Command for the motor in percentage. */ void set_command(float command); private: /** - * @brief PWM object for controlling the motor in the backwards direction + * @brief PWM object for controlling the motor in the backwards direction. */ hal::Pwm backwards_pwm; /** - * @brief PWM object for controlling the motor in the forward direction + * @brief PWM object for controlling the motor in the forward direction. */ hal::Pwm forward_pwm; /** - * @brief Maximum command value for the motor to be considered stopped + * @brief Maximum command value for the motor to be considered stopped. */ float max_stopped_command; /** - * @brief Deadzone for the motor + * @brief Deadzone for the motor. */ float deadzone; }; diff --git a/micras_proxy/include/micras/proxy/rotary_sensor.hpp b/micras_proxy/include/micras/proxy/rotary_sensor.hpp index 3aaee90..464daed 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor.hpp @@ -13,14 +13,14 @@ namespace micras::proxy { /** - * @brief Class to handle rotary sensor peripheral on STM32 microcontrollers + * @brief Class for acquiring rotary sensor data. */ class RotarySensor { public: #include "micras/proxy/rotary_sensor_reg.hpp" /** - * @brief Rotary sensor configuration struct + * @brief Rotary sensor configuration struct. */ struct Config { hal::Spi::Config spi; @@ -55,52 +55,52 @@ class RotarySensor { }; /** - * @brief Construct a new RotarySensor object + * @brief Construct a new RotarySensor object. * - * @param config Configuration for the rotary sensor + * @param config Configuration for the rotary sensor. */ explicit RotarySensor(const Config& config); /** - * @brief Get the rotary sensor position over an axis + * @brief Get the rotary sensor position over an axis. * - * @return float Current angular position of the sensor in radians + * @return Current angular position of the sensor in radians. */ float get_position() const; /** - * @brief Read a register to the rotary sensor + * @brief Read a register to the rotary sensor. * - * @param command_frame Command frame to send trough SPI + * @param command_frame Command frame to send trough SPI. */ uint16_t read_register(uint16_t address); /** - * @brief Write a register to the rotary sensor + * @brief Write a register to the rotary sensor. * - * @param command_frame Command frame to send trough SPI - * @param data_frame Data frame to send trough SPI + * @param command_frame Command frame to send trough SPI. + * @param data_frame Data frame to send trough SPI. */ void write_register(CommandFrame& command_frame, DataFrame& data_frame); private: /** - * @brief SPI for the rotary sensor configuration + * @brief SPI for the rotary sensor configuration. */ hal::Spi spi; /** - * @brief Encoder for getting the rotary sensor data + * @brief Encoder for getting the rotary sensor data. */ hal::Encoder encoder; /** - * @brief CRC for the rotary sensor configuration + * @brief CRC for the rotary sensor configuration. */ hal::Crc crc; /** - * @brief Resolution of the rotary sensor + * @brief Resolution of the rotary sensor. */ uint32_t resolution; }; diff --git a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp index 3ce466a..bc3ff74 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp @@ -8,12 +8,13 @@ #include /** - * @brief Registers class for the rotary sensor configuration + * @brief Registers class for the rotary sensor configuration. */ struct Registers { /** - * @brief Registers union types definition + * @brief Registers union types definition. */ + ///@{ union Disable { struct __attribute__((__packed__)) Fields { uint8_t UVW_off : 1; @@ -95,9 +96,12 @@ struct Registers { uint8_t raw; }; + ///@} + /** - * @brief Register addresses in the rotary sensor memory + * @brief Register addresses in the rotary sensor memory. */ + ///@{ static constexpr uint16_t disable_addr{0x0015}; static constexpr uint16_t zposm_addr{0x0016}; static constexpr uint16_t zposl_addr{0x0017}; @@ -105,10 +109,12 @@ struct Registers { static constexpr uint16_t settings2_addr{0x0019}; static constexpr uint16_t settings3_addr{0x001A}; static constexpr uint16_t ecc_addr{0x001B}; + ///@} /** - * @brief Member variables to be configured in the rotary sensor + * @brief Member variables to be configured in the rotary sensor. */ + ///@{ Disable disable; Zposm zposm; Zposl zposl; @@ -116,6 +122,7 @@ struct Registers { Settings2 settings2; Settings3 settings3; Ecc ecc; + ///@} }; #endif // MICRAS_PROXY_ROTARY_SENSOR_REG_HPP diff --git a/micras_proxy/include/micras/proxy/serializable.hpp b/micras_proxy/include/micras/proxy/serializable.hpp index a82431c..eff0aac 100644 --- a/micras_proxy/include/micras/proxy/serializable.hpp +++ b/micras_proxy/include/micras/proxy/serializable.hpp @@ -10,43 +10,41 @@ namespace micras::proxy { /** - * @brief Interface class for serializable classes + * @brief Interface class for serializable classes. */ class ISerializable { public: /** - * @brief Virtual destructor for the ISerializable class + * @brief Virtual destructor for the ISerializable class. */ virtual ~ISerializable() = default; /** - * @brief Serialize the class instance + * @brief Serialize the class instance. * - * @return std::vector Serialized data + * @return Serialized data. */ virtual std::vector serialize() const = 0; /** - * @brief Deserialize the class instance + * @brief Deserialize the class instance. * - * @param serial_data Serialized data - * @param size Size of the serialized data + * @param serial_data Serialized data. + * @param size Size of the serialized data. */ virtual void deserialize(const uint8_t* serial_data, uint16_t size) = 0; protected: /** - * @brief Special member functions declared as default + * @brief Special member functions declared as default. */ + ///@{ ISerializable() = default; - ISerializable(const ISerializable&) = default; - ISerializable(ISerializable&&) = default; - ISerializable& operator=(const ISerializable&) = default; - ISerializable& operator=(ISerializable&&) = default; + ///@} }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index 60d90ab..00c895e 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -18,12 +18,12 @@ template concept Fundamental = std::is_fundamental_v; /** - * @brief Class for controlling the storage + * @brief Class for storing variable and classes in the flash memory. */ class Storage { public: /** - * @brief Configuration structure for the storage + * @brief Configuration struct for the storage. */ struct Config { uint16_t start_page; @@ -31,18 +31,18 @@ class Storage { }; /** - * @brief Constructor for the Storage class + * @brief Construct a new Storage object. * - * @param config Configuration for the storage + * @param config Configuration for the storage. */ explicit Storage(const Config& config); /** - * @brief Create a new primitive variable in the storage + * @brief Create a new primitive variable in the storage. * - * @tparam T Type of the variable - * @param name Name of the variable - * @param data Reference to the variable + * @tparam T Type of the variable. + * @param name Name of the variable. + * @param data Reference to the variable. */ template void create(const std::string& name, const T& data) { @@ -51,19 +51,19 @@ class Storage { } /** - * @brief Create a new serializable variable in the storage + * @brief Create a new serializable variable in the storage. * - * @param name Name of the variable - * @param data Reference to the variable + * @param name Name of the variable. + * @param data Reference to the variable. */ void create(const std::string& name, const ISerializable& data); /** - * @brief Sync a primitive variable with the storage + * @brief Sync a primitive variable with the storage. * - * @tparam T Type of the variable - * @param name Name of the variable - * @param data Reference to the variable + * @tparam T Type of the variable. + * @param name Name of the variable. + * @param data Reference to the variable. */ template void sync(const std::string& name, T& data) { @@ -76,21 +76,21 @@ class Storage { } /** - * @brief Sync a serializable variable with the storage + * @brief Sync a serializable variable with the storage. * - * @param name Name of the variable - * @param data Reference to the variable + * @param name Name of the variable. + * @param data Reference to the variable. */ void sync(const std::string& name, ISerializable& data); /** - * @brief Save the storage to the flash + * @brief Save the storage to the flash. */ void save(); private: /** - * @brief Structure for primitive variables + * @brief Struct for primitive variables. */ struct PrimitiveVariable { const void* ram_pointer{nullptr}; @@ -99,7 +99,7 @@ class Storage { }; /** - * @brief Structure for serializable variables + * @brief Struct for serializable variables. */ struct SerializableVariable { const ISerializable* ram_pointer{nullptr}; @@ -108,55 +108,53 @@ class Storage { }; /** - * @brief Serialize a map of variables + * @brief Serialize a map of variables. * - * @tparam T Type of the variables - * @param variables Map of variables - * - * @return std::vector Serialized buffer + * @tparam T Type of the variables. + * @param variables Map of variables. + * @return Serialized buffer. */ template static std::vector serialize_var_map(const std::unordered_map& variables); /** - * @brief Deserialize a map of variables - * - * @tparam T Type of the variables - * @param buffer Serialized buffer - * @param num_vars Number of variables + * @brief Deserialize a map of variables. * - * @return std::unordered_map Deserialized map of variables + * @tparam T Type of the variables. + * @param buffer Serialized buffer. + * @param num_vars Number of variables. + * @return Deserialized map of variables. */ template static std::unordered_map deserialize_var_map(std::vector& buffer, uint16_t num_vars); /** - * @brief Start symbol to avoid reading garbage from flash + * @brief Start symbol to avoid reading garbage from flash. */ static constexpr uint16_t start_symbol = 0xABAB; /** - * @brief Map of primitive variables + * @brief Map of primitive variables. */ std::unordered_map primitives; /** - * @brief Map of serializable variables + * @brief Map of serializable variables. */ std::unordered_map serializables; /** - * @brief Serialized buffer for the storage + * @brief Serialized buffer for the storage. */ std::vector buffer; /** - * @brief Start page of the storage in the flash memory + * @brief Start page of the storage in the flash memory. */ uint16_t start_page; /** - * @brief Maximum number of pages used by the storage in the flash memory + * @brief Maximum number of pages used by the storage in the flash memory. */ uint16_t number_of_pages; }; diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 3ddb32f..83193a1 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -13,13 +13,13 @@ namespace micras::proxy { /** - * @brief Class for acquiring torque sensors data + * @brief Class for acquiring torque sensors data. */ template class TTorqueSensors { public: /** - * @brief Configuration structure for torque sensors + * @brief Configuration struct for torque sensors. */ struct Config { hal::AdcDma::Config adc; @@ -29,90 +29,90 @@ class TTorqueSensors { }; /** - * @brief Constructor for the TorqueSensors class + * @brief Construct a newTorqueSensors object. * - * @param config Configuration for the torque sensors + * @param config Configuration for the torque sensors. */ explicit TTorqueSensors(const Config& config); /** - * @brief Calibrate the torque sensors + * @brief Calibrate the torque sensors. */ void calibrate(); /** - * @brief Update the torque sensors readings + * @brief Update the torque sensors readings. */ void update(); /** - * @brief Get the torque from the sensor + * @brief Get the torque from the sensor. * - * @param sensor_index Index of the sensor - * @return float Torque reading from the sensor in N*m + * @param sensor_index Index of the sensor. + * @return Torque reading from the sensor in N*m. */ float get_torque(uint8_t sensor_index) const; /** - * @brief Get the raw torque from the sensor without filtering + * @brief Get the raw torque from the sensor without filtering. * - * @param sensor_index Index of the sensor - * @return float Raw torque reading from the sensor + * @param sensor_index Index of the sensor. + * @return Raw torque reading from the sensor. */ float get_torque_raw(uint8_t sensor_index) const; /** - * @brief Get the current from the sensor + * @brief Get the current from the sensor. * - * @param sensor_index Index of the sensor - * @return float Current reading from the sensor in amps + * @param sensor_index Index of the sensor. + * @return Current reading from the sensor in amps. */ float get_current(uint8_t sensor_index) const; /** - * @brief Get the raw current from the sensor without filtering + * @brief Get the raw current from the sensor without filtering. * - * @param sensor_index Index of the sensor - * @return float Raw current reading from the sensor + * @param sensor_index Index of the sensor. + * @return Raw current reading from the sensor. */ float get_current_raw(uint8_t sensor_index) const; /** - * @brief Get the ADC reading from the sensor + * @brief Get the ADC reading from the sensor. * - * @param sensor_index Index of the sensor - * @return float Adc reading from the sensor from 0 to 1 + * @param sensor_index Index of the sensor. + * @return Adc reading from the sensor from 0 to 1. */ float get_adc_reading(uint8_t sensor_index) const; private: /** - * @brief ADC DMA handle + * @brief ADC DMA handle. */ hal::AdcDma adc; /** - * @brief Buffer to store the ADC values + * @brief Buffer to store the ADC values. */ std::array buffer; /** - * @brief Reading of each sensor when no current is flowing + * @brief Reading of each sensor when no current is flowing. */ std::array base_reading{}; /** - * @brief Value of the maximum current that can be measured by the sensor + * @brief Value of the maximum current that can be measured by the sensor. */ float max_current; /** - * @brief Maximum torque that can be measured by the sensor + * @brief Maximum torque that can be measured by the sensor. */ float max_torque; /** - * @brief Butterworth filters for the torque reading + * @brief Butterworth filters for the torque reading. */ std::array filters; }; diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 12d2b65..d5680b0 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -15,13 +15,13 @@ namespace micras::proxy { /** - * @brief Class for controlling WallSensors + * @brief Class for controlling Wall Sensors. */ template class TWallSensors { public: /** - * @brief Configuration structure for wall sensors + * @brief Configuration struct for wall sensors. */ struct Config { hal::AdcDma::Config adc; @@ -34,134 +34,134 @@ class TWallSensors { }; /** - * @brief Constructor for the WallSensors class + * @brief Construct a newWallSensors object. * - * @param config Configuration for the wall sensors + * @param config Configuration for the wall sensors. */ explicit TWallSensors(const Config& config); /** - * @brief Turn on the wall sensors IR LED + * @brief Turn on the wall sensors IR LED. */ void turn_on(); /** - * @brief Turn off the wall sensors IR LED + * @brief Turn off the wall sensors IR LED. */ void turn_off(); /** - * @brief Update the wall sensors readings + * @brief Update the wall sensors readings. */ void update(); /** - * @brief Get the observation from a sensor + * @brief Get the observation from a sensor. * - * @param sensor_index Index of the sensor - * @return core::Observation Observation from the sensor + * @param sensor_index Index of the sensor. + * @return core::Observation Observation from the sensor. */ core::Observation get_observation(uint8_t sensor_index) const; /** - * @brief Get the reading from a sensor + * @brief Get the reading from a sensor. * - * @param sensor_index Index of the sensor - * @return float Reading from the sensor + * @param sensor_index Index of the sensor. + * @return Reading from the sensor. */ float get_reading(uint8_t sensor_index) const; /** - * @brief Get the ADC reading from a sensor + * @brief Get the ADC reading from a sensor. * - * @param sensor_index Index of the sensor - * @return float ADC reading from the sensor from 0 to 1 + * @param sensor_index Index of the sensor. + * @return ADC reading from the sensor from 0 to 1. */ float get_adc_reading(uint8_t sensor_index) const; /** - * @brief Calibrate the wall sensors for a wall at the front + * @brief Calibrate the wall sensors for a wall at the front. */ void calibrate_front_wall(); /** - * @brief Calibrate the wall sensors for a wall at the left + * @brief Calibrate the wall sensors for a wall at the left. */ void calibrate_left_wall(); /** - * @brief Calibrate the wall sensors for a wall at the right + * @brief Calibrate the wall sensors for a wall at the right. */ void calibrate_right_wall(); /** - * @brief Calibrate the wall sensors for free space at the front + * @brief Calibrate the wall sensors for free space at the front. */ void calibrate_front_free_space(); /** - * @brief Calibrate the wall sensors for free space at the left + * @brief Calibrate the wall sensors for free space at the left. */ void calibrate_left_free_space(); /** - * @brief Calibrate the wall sensors for free space at the right + * @brief Calibrate the wall sensors for free space at the right. */ void calibrate_right_free_space(); /** - * @brief Update the wall sensors thresholds + * @brief Update the wall sensors thresholds. */ void update_thresholds(); private: /** - * @brief ADC DMA handle + * @brief ADC DMA handle. */ hal::AdcDma adc; /** - * @brief PWM handle for the even infrared LEDs + * @brief PWM handle for the even infrared LEDs. */ hal::Pwm led_0_pwm; /** - * @brief PWM handle for the odd infrared LEDs + * @brief PWM handle for the odd infrared LEDs. */ hal::Pwm led_1_pwm; /** - * @brief Buffer to store the ADC values + * @brief Buffer to store the ADC values. */ std::array buffer; /** - * @brief Butterworth filter for the ADC readings + * @brief Butterworth filter for the ADC readings. */ std::array filters; /** - * @brief Uncertainty of the wall sensors + * @brief Uncertainty of the wall sensors. */ float uncertainty; /** - * @brief Measured wall value during calibration + * @brief Measured wall value during calibration. */ std::array wall_calibration_measure; /** - * @brief Measured free space value during calibration + * @brief Measured free space value during calibration. */ std::array free_space_calibration_measure; /** - * @brief Minimum reading value to identify a wall + * @brief Minimum reading value to identify a wall. */ std::array wall_threshold; /** - * @brief Maximum reading value to identify a free space + * @brief Maximum reading value to identify a free space. */ std::array free_space_threshold; diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp index cd968df..ec127f5 100644 --- a/micras_proxy/src/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -10,8 +10,8 @@ Buzzer::Buzzer(const Config& config) : pwm{config.pwm} { } void Buzzer::play(uint32_t frequency, uint32_t duration) { - this->pwm.set_duty_cycle(50.0F); this->pwm.set_frequency(frequency); + this->pwm.set_duty_cycle(50.0F); this->is_playing = true; this->duration = duration; From 3721691ac2324909ca0912110bfe248ded80d463 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 28 Dec 2024 14:43:32 -0300 Subject: [PATCH 258/561] =?UTF-8?q?=F0=9F=8E=A8=20Group=20Argb=20colors=20?= =?UTF-8?q?inside=20struct?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Eduardo Santos Barreto --- .clang-tidy | 1 + micras_proxy/include/micras/proxy/argb.hpp | 20 +++++++++++--------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index d684a6e..39cde64 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -50,6 +50,7 @@ CheckOptions: readability-identifier-length.IgnoredParameterNames: ^[nxyz]$ misc-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic: true cppcoreguidelines-narrowing-conversions.WarnOnIntegerToFloatingPointNarrowingConversion: false + readability-function-cognitive-complexity.Threshold: 30 SystemHeaders: false ExtraArgsBefore: - '--target=arm-none-eabi' diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index 1299d5a..79c6ead 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -47,15 +47,17 @@ class TArgb { /** * @brief Predefined colors. */ - ///@{ - static constexpr Color red{255, 0, 0}; - static constexpr Color green{0, 255, 0}; - static constexpr Color blue{0, 0, 255}; - static constexpr Color yellow{255, 255, 0}; - static constexpr Color cyan{0, 255, 255}; - static constexpr Color magenta{255, 0, 255}; - static constexpr Color white{255, 255, 255}; - ///@} + struct Colors { + Colors() = delete; + + static constexpr Color red{255, 0, 0}; + static constexpr Color green{0, 255, 0}; + static constexpr Color blue{0, 0, 255}; + static constexpr Color yellow{255, 255, 0}; + static constexpr Color cyan{0, 255, 255}; + static constexpr Color magenta{255, 0, 255}; + static constexpr Color white{255, 255, 255}; + }; /** * @brief Construct a new Argb object. From f7c9e5bffd3f9b48fd7d45a9f4738f679a14d654 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 28 Dec 2024 15:07:25 -0300 Subject: [PATCH 259/561] =?UTF-8?q?=F0=9F=93=9D=20Fix=20function=20documen?= =?UTF-8?q?tations?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/pwm.hpp | 5 +++-- micras_nav/include/micras/nav/look_at_point.hpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/micras_hal/include/micras/hal/pwm.hpp b/micras_hal/include/micras/hal/pwm.hpp index 548b8cb..5bbd867 100644 --- a/micras_hal/include/micras/hal/pwm.hpp +++ b/micras_hal/include/micras/hal/pwm.hpp @@ -40,8 +40,9 @@ class Pwm { /** * @brief Set the PWM frequency. * - * @note Changing the pwm frequency will also change the duty cycle and reset the counter. - * There is a minimum and maximum frequency that can be set by changing the auto-reload register. + * @note Changing the pwm frequency will modify the autoreload and reset the counter, + * but the compare value will stay the same, so the duty cycle will be different from the previously set value. + * There is a minimum and maximum frequency that can be set by changing the autoreload register. * The minimum frequency is base_freq / (2^timer_resolution * (prescaler + 1)). * The maximum frequency is base_freq / (prescaler + 1). * diff --git a/micras_nav/include/micras/nav/look_at_point.hpp b/micras_nav/include/micras/nav/look_at_point.hpp index 65cace5..0e092fa 100644 --- a/micras_nav/include/micras/nav/look_at_point.hpp +++ b/micras_nav/include/micras/nav/look_at_point.hpp @@ -34,7 +34,7 @@ class LookAtPoint { /** * @brief Calculates the command to look at a point. * - * @param pose The current pose of the robot. + * @param state The current state of the robot. * @param goal The goal point. * @param elapsed_time The time elapsed since the last update. * @return The command to look at the point. @@ -49,8 +49,9 @@ class LookAtPoint { /** * @brief Checks if the robot has reached the goal. * - * @param pose The current pose of the robot. + * @param state The current state of the robot. * @param goal The goal point. + * @param stop Whether to stop the robot when it reaches the goal. * @return True if the robot has reached the goal, false otherwise. */ bool finished(const State& state, const Point& goal, bool stop = true) const; From 0b47e7b1311ce08713f68873fbb8230f4cad1cfd Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 30 Dec 2024 17:12:32 -0300 Subject: [PATCH 260/561] =?UTF-8?q?=F0=9F=93=9D=20Improve=20code=20documen?= =?UTF-8?q?tation=20and=20fix=20typos?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: dardengo --- .../micras/core/butterworth_filter.hpp | 2 +- micras_core/include/micras/core/utils.hpp | 4 +-- micras_hal/include/micras/hal/adc_dma.hpp | 4 +-- micras_hal/include/micras/hal/flash.hpp | 8 +++--- micras_hal/include/micras/hal/mcu.hpp | 2 +- micras_hal/stm32/src/mcu.cpp | 2 +- micras_nav/include/micras/nav/follow_wall.hpp | 12 ++++----- micras_nav/include/micras/nav/go_to_point.hpp | 4 +-- micras_nav/include/micras/nav/grid_pose.hpp | 18 ++++++------- .../include/micras/nav/look_at_point.hpp | 6 ++--- micras_nav/include/micras/nav/mapping.hpp | 16 ++++++------ micras_nav/include/micras/nav/maze.hpp | 26 +++++++++---------- .../include/micras/nav/pid_controller.hpp | 2 +- micras_nav/include/micras/nav/state.hpp | 22 ++++++++-------- .../micras/proxy/rotary_sensor_reg.hpp | 2 +- .../include/micras/proxy/torque_sensors.hpp | 8 +++--- .../include/micras/proxy/wall_sensors.hpp | 2 +- 17 files changed, 70 insertions(+), 70 deletions(-) diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp index f5ed586..e68fd59 100644 --- a/micras_core/include/micras/core/butterworth_filter.hpp +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -41,7 +41,7 @@ class ButterworthFilter { explicit ButterworthFilter(float cutoff_frequency, float sampling_frequency = 100.0F); /** - * @brief Produces a new value from measured data. + * @brief Produce a new value from measured data. * * @param x0 Last measure. * @return Filtered value. diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index a9319b9..0553c1f 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -57,7 +57,7 @@ constexpr T transition(T value, T start, T end, T resistance) { } /** - * @brief Create an array objects calling its constructors from an array of parameters. + * @brief Create an array of objects calling their constructors from an array of parameters. * * @tparam T Type of the array. * @tparam N Size of the array. @@ -73,7 +73,7 @@ constexpr std::array make_array(const std::array& parameters) { } /** - * @brief Create an array objects calling its constructors from a single parameter. + * @brief Create an array of objects calling their constructors from a single parameter. * * @tparam T Type of the array. * @tparam N Size of the array. diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 891d257..2a53da0 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -34,14 +34,14 @@ class AdcDma { /** * @brief Enable ADC, start conversion of regular group and transfer result through DMA. * - * @param buffer Destination Buffer address. + * @param buffer 32 bit destination buffer address. */ void start_dma(std::span buffer); /** * @brief Enable ADC, start conversion of regular group and transfer result through DMA. * - * @param buffer Destination Buffer address. + * @param buffer 16 bit destination buffer address. */ void start_dma(std::span buffer); diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp index 5768f84..2a4e96c 100644 --- a/micras_hal/include/micras/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -22,7 +22,7 @@ class Flash { // NOLINTBEGIN(*-avoid-c-arrays) /** - * @brief Read data from flash memory. + * @brief Read data from flash memory at an absolute address. * * @param address Address to read from (indexed by double words). * @param data Pointer to store the data read. @@ -31,7 +31,7 @@ class Flash { static void read(uint32_t address, uint64_t data[], uint32_t size = 1); /** - * @brief Read data from flash memory. + * @brief Read data from flash memory at an address relative to a page. * * @param page Page to read from (counting from the last to the first). * @param page_address Address inside the page to read from (indexed by double words). @@ -41,7 +41,7 @@ class Flash { static void read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); /** - * @brief Write data to flash memory. + * @brief Write data to flash memory at an absolute address. * * @param address Address to write to (indexed by double words). * @param data Pointer to the data to write. @@ -50,7 +50,7 @@ class Flash { static void write(uint32_t address, const uint64_t data[], uint32_t size = 1); /** - * @brief Write data to flash memory. + * @brief Write data to flash memory at an address relative to a page. * * @param page Page to write to (counting from the last to the first). * @param page_address Address inside the page to write to (indexed by double words). diff --git a/micras_hal/include/micras/hal/mcu.hpp b/micras_hal/include/micras/hal/mcu.hpp index f963489..2ad6c59 100644 --- a/micras_hal/include/micras/hal/mcu.hpp +++ b/micras_hal/include/micras/hal/mcu.hpp @@ -19,7 +19,7 @@ class Mcu { Mcu() = delete; /** - * @brief Initializes MCU and some peripherals. + * @brief Initialize MCU and some peripherals. */ static void init(); }; diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp index 4a54cf9..8110583 100644 --- a/micras_hal/stm32/src/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -10,7 +10,7 @@ extern "C" { /** - * @brief Initializes System Clock. + * @brief Initialize System Clock. * * @note Defined by cube. */ diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 910a3f2..dbad186 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -43,27 +43,27 @@ class FollowWall { float action(core::FollowWallType follow_wall_type, float elapsed_time); /** - * @brief Resets the PID controller. + * @brief Reset the PID controller. */ void reset(); /** - * @brief Resets the base readings of the wall sensors. + * @brief Reset the base readings of the wall sensors. */ void reset_base_readings(); private: /** - * @brief Get the reading of the left wall sensor. + * @brief Get the reading from the left wall sensor. * - * @return The reading of the left wall sensor. + * @return The reading from the left wall sensor. */ float get_left_value() const; /** - * @brief Get the reading of the right wall sensor. + * @brief Get the reading from the right wall sensor. * - * @return The reading of the right wall sensor. + * @return The reading from the right wall sensor. */ float get_right_value() const; diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index 9e6f7a0..2315ea2 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -41,7 +41,7 @@ class GoToPoint { ); /** - * @brief Calculates the command to go to a point. + * @brief Calculate the command to go to a point. * * @param state The current state of the robot. * @param goal The goal point. @@ -56,7 +56,7 @@ class GoToPoint { ); /** - * @brief Checks if the robot has reached the goal. + * @brief Check if the robot has reached the goal. * * @param state The current state of the robot. * @param goal The goal point. diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index 80c0376..976582a 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -22,7 +22,7 @@ enum Side : uint8_t { }; /** - * @brief Converts an angle in radians to a Grid side. + * @brief Convert an angle in radians to a Grid side. * * @param angle The angle in radians. * @return The corresponding Grid side. @@ -48,7 +48,7 @@ struct Information { */ struct GridPoint { /** - * @brief Returns the direction from this point to the next one. + * @brief Return the direction from this point to the next one. * * @param next The next point. * @return The direction from this point to the next one. @@ -56,7 +56,7 @@ struct GridPoint { Side direction(const GridPoint& next) const; /** - * @brief Moves in the grid in the direction of the side. + * @brief Move in the grid in the direction of the side. * * @param side The side to move to. * @return The new point after moving. @@ -64,7 +64,7 @@ struct GridPoint { GridPoint operator+(const Side& side) const; /** - * @brief Compares two points for equality. + * @brief Compare two points for equality. * * @param other The other point to compare. * @return True if the points are equal, false otherwise. @@ -84,35 +84,35 @@ struct GridPoint { struct GridPose { /** - * @brief Returns the pose after moving forward. + * @brief Return the pose after moving forward. * * @return The pose after moving forward. */ GridPose front() const; /** - * @brief Returns the pose after turning back. + * @brief Return the pose after turning back. * * @return The pose after turning back. */ GridPose turned_back() const; /** - * @brief Returns the pose after turning left. + * @brief Return the pose after turning left. * * @return The pose after turning left. */ GridPose turned_left() const; /** - * @brief Returns the pose after turning right. + * @brief Return the pose after turning right. * * @return The pose after turning right. */ GridPose turned_right() const; /** - * @brief Compares two poses for equality. + * @brief Compare two poses for equality. * * @param other The other pose to compare. * @return True if the poses are equal, false otherwise. diff --git a/micras_nav/include/micras/nav/look_at_point.hpp b/micras_nav/include/micras/nav/look_at_point.hpp index 0e092fa..8411881 100644 --- a/micras_nav/include/micras/nav/look_at_point.hpp +++ b/micras_nav/include/micras/nav/look_at_point.hpp @@ -32,7 +32,7 @@ class LookAtPoint { explicit LookAtPoint(Config config); /** - * @brief Calculates the command to look at a point. + * @brief Calculate the command to look at a point. * * @param state The current state of the robot. * @param goal The goal point. @@ -42,12 +42,12 @@ class LookAtPoint { Twist action(const State& state, const Point& goal, float elapsed_time); /** - * @brief Resets the PID controllers. + * @brief Reset the PID controllers. */ void reset(); /** - * @brief Checks if the robot has reached the goal. + * @brief Check if the robot has reached the goal. * * @param state The current state of the robot. * @param goal The goal point. diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index 249b1a5..e06045e 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -66,11 +66,11 @@ class TMapping : public proxy::ISerializable { * @brief Possible types of actions. */ enum Type : uint8_t { - LOOK_AT, - GO_TO, - ALIGN_BACK, - FINISHED, - ERROR, + LOOK_AT, // Rotate the robot to look at a specific point. + GO_TO, // Move and turn the robot to go to a specific point. + ALIGN_BACK, // Align the back of the robot with a wall. + FINISHED, // The robot has reached the goal. + ERROR, // An error occurred. }; /** @@ -133,7 +133,7 @@ class TMapping : public proxy::ISerializable { void calibrate_side(); /** - * @brief Check if the robot is at a distance from a wall at the front. + * @brief Check if the robot is at a predefined distance from the wall at the front. * * @return True if the robot is aligned, false otherwise. */ @@ -147,7 +147,7 @@ class TMapping : public proxy::ISerializable { bool is_orientation_front_aligned() const; /** - * @brief Check if the robot is at a distance from a wall at the sides. + * @brief Check if the robot is at a predefined distance from the wallz at the sides. * * @return True if the robot is aligned, false otherwise. */ @@ -192,7 +192,7 @@ class TMapping : public proxy::ISerializable { bool can_align_back(const Pose& pose) const; /** - * @brief Adds diagonal movements to the best route. + * @brief Add diagonal movements to the best route. */ void diagonalize_best_route(); diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 5b37765..24ebe2a 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -31,7 +31,7 @@ class TMaze { TMaze(const GridPose& start, const std::unordered_set& goal); /** - * @brief Updates the maze walls with the current pose and new information. + * @brief Update the maze walls with the current pose and new information. * * @param pose The pose of the robot. * @param information The information from the wall sensors. @@ -39,7 +39,7 @@ class TMaze { void update(const GridPose& pose, Information information); /** - * @brief Returns the next point the robot should go to when exploring. + * @brief Return the next point the robot should go to when exploring. * * @param position The current position of the robot. * @return The next point the robot should go to when exploring. @@ -47,7 +47,7 @@ class TMaze { GridPose get_current_exploration_goal(const GridPoint& position) const; /** - * @brief Returns the next point the robot should go to when returning. + * @brief Return the next point the robot should go to when returning. * * @param position The current position of the robot. * @return The next point the robot should go to when returning. @@ -55,7 +55,7 @@ class TMaze { GridPose get_current_returning_goal(const GridPoint& position) const; /** - * @brief Checks the type of wall following the robot can do. + * @brief Check the type of wall following the robot can do. * * @param pose The pose of the robot. * @return The type of wall following. @@ -63,7 +63,7 @@ class TMaze { core::FollowWallType get_follow_wall_type(const GridPose& pose) const; /** - * @brief Checks whether the robot has finished the maze. + * @brief Check whether the robot has finished the maze. * * @param position The current position of the robot. * @return True if the robot has finished the maze, false otherwise. @@ -71,7 +71,7 @@ class TMaze { bool finished(const GridPoint& position) const; /** - * @brief Checks whether the robot has returned to the start. + * @brief Check whether the robot has returned to the start. * * @param position The current position of the robot. * @return True if the robot has returned to the start, false otherwise. @@ -79,24 +79,24 @@ class TMaze { bool returned(const GridPoint& position) const; /** - * @brief Calculates the best route to the goal using the current costmap. + * @brief Calculate the best route to the goal using the current costmap. */ void calculate_best_route(); /** - * @brief Improves the route to the goal. + * @brief Improve the route to the goal. */ void optimize_route(); /** - * @brief Returns the best route to the goal. + * @brief Return the best route to the goal. * * @return The best route to the goal. */ const std::map>& get_best_route() const; /** - * @brief Checks whether there is a wall at the front of a given pose. + * @brief Check whether there is a wall at the front of a given pose. * * @param pose The pose to check. * @return True if there is a wall, false otherwise. @@ -114,12 +114,12 @@ class TMaze { }; /** - * @brief Calculates the costmap for the flood fill algorithm. + * @brief Calculate the costmap for the flood fill algorithm. */ void calculate_costmap(); /** - * @brief Returns the cell at the given position. + * @brief Return the cell at the given position. * * @param position The position of the cell. * @return The cell at the given position. @@ -127,7 +127,7 @@ class TMaze { const Cell& get_cell(const GridPoint& position) const; /** - * @brief Returns the cell at the given position. + * @brief Return the cell at the given position. * * @param position The position of the cell. * @return The cell at the given position. diff --git a/micras_nav/include/micras/nav/pid_controller.hpp b/micras_nav/include/micras/nav/pid_controller.hpp index e3742ef..7b1ca02 100644 --- a/micras_nav/include/micras/nav/pid_controller.hpp +++ b/micras_nav/include/micras/nav/pid_controller.hpp @@ -9,7 +9,7 @@ namespace micras::nav { /** - * @brief Implementation of simple PID controller. Response = kp * (error + hi * integral(error) kd * d/dt(error)) + * @brief Implementation of simple PID controller. Response = kp * (error + ki * integral(error) kd * d/dt(error)) */ class PidController { public: diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index a8c5a51..7cc62e9 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -27,7 +27,7 @@ enum Direction : uint8_t { */ struct Point { /** - * @brief Calculates the distance to another point. + * @brief Calculate the distance to another point. * * @param other The other point. * @return The distance to the other point. @@ -35,7 +35,7 @@ struct Point { float distance(const Point& other) const; /** - * @brief Calculates the angle between two points. + * @brief Calculate the angle between two points. * * @param other The other point. * @return The angle between the two points. @@ -43,7 +43,7 @@ struct Point { float angle_between(const Point& other) const; /** - * @brief Converts the point to a grid point. + * @brief Convert the point to a grid point. * * @param cell_size The size of the grid cells. * @return The grid point corresponding to the point. @@ -51,7 +51,7 @@ struct Point { GridPoint to_grid(float cell_size) const; /** - * @brief Rotates the point by a given angle. + * @brief Rotate the point by a given angle. * * @param angle The angle to rotate the point by. * @return The rotated point. @@ -59,7 +59,7 @@ struct Point { Point rotate(Direction angle); /** - * @brief Moves the point towards another point by a given distance. + * @brief Move the point towards another point by a given distance. * * @param other The point to move towards. * @param distance The distance to move. @@ -68,7 +68,7 @@ struct Point { Point move_towards(const Point& other, float distance) const; /** - * @brief Subtracts a point from another. + * @brief Subtract a point from another. * * @param other The point to subtract. * @return The result of the subtraction. @@ -76,7 +76,7 @@ struct Point { Point operator-(const Point& other) const; /** - * @brief Compares two points for equality. + * @brief Compare two points for equality. * * @param other The other point to compare. * @return True if the points are equal, false otherwise. @@ -84,7 +84,7 @@ struct Point { bool operator==(const Point& other) const; /** - * @brief Calculates the remainder of the division of the point by a value. + * @brief Calculate the remainder of the division of the point by a value. * * @param value The value to divide the point by. * @return The remainder of the division. @@ -92,7 +92,7 @@ struct Point { Point operator%(float value) const; /** - * @brief Converts a grid point to a point. + * @brief Convert a grid point to a point. * * @param grid_point The grid point to convert. * @param cell_size The size of the grid cells. @@ -116,7 +116,7 @@ struct Point { */ struct Pose { /** - * @brief Converts the pose to a grid pose. + * @brief Convert the pose to a grid pose. * * @param cell_size The size of the grid cells. * @return The grid pose corresponding to the pose. @@ -124,7 +124,7 @@ struct Pose { GridPose to_grid(float cell_size) const; /** - * @brief Converts the pose to a cell reference. + * @brief Convert the pose to a cell reference. * * @param cell_size The size of the grid cells. * @return The point inside the cell reference. diff --git a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp index bc3ff74..021eda2 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp @@ -8,7 +8,7 @@ #include /** - * @brief Registers class for the rotary sensor configuration. + * @brief Registers struct for the rotary sensor configuration. */ struct Registers { /** diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 83193a1..a0d230c 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -29,7 +29,7 @@ class TTorqueSensors { }; /** - * @brief Construct a newTorqueSensors object. + * @brief Construct a new TorqueSensors object. * * @param config Configuration for the torque sensors. */ @@ -62,7 +62,7 @@ class TTorqueSensors { float get_torque_raw(uint8_t sensor_index) const; /** - * @brief Get the current from the sensor. + * @brief Get the electric current through the sensor. * * @param sensor_index Index of the sensor. * @return Current reading from the sensor in amps. @@ -70,10 +70,10 @@ class TTorqueSensors { float get_current(uint8_t sensor_index) const; /** - * @brief Get the raw current from the sensor without filtering. + * @brief Get the raw electric current through the sensor without filtering. * * @param sensor_index Index of the sensor. - * @return Raw current reading from the sensor. + * @return Raw current reading from the sensor in amps. */ float get_current_raw(uint8_t sensor_index) const; diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index d5680b0..772fea0 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -34,7 +34,7 @@ class TWallSensors { }; /** - * @brief Construct a newWallSensors object. + * @brief Construct a new WallSensors object. * * @param config Configuration for the wall sensors. */ From 022660bd4f47cda35212b33fc739559cedcc7930 Mon Sep 17 00:00:00 2001 From: Eduardo-Barreto Date: Tue, 31 Dec 2024 23:44:16 -0300 Subject: [PATCH 261/561] =?UTF-8?q?=F0=9F=9A=A8=F0=9F=94=A7=20Set=20warnin?= =?UTF-8?q?g=20as=20errors=20on=20linter?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.clang-tidy b/.clang-tidy index d684a6e..0565f88 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -26,7 +26,7 @@ Checks: readability-*, -readability-duplicate-include, -readability-magic-numbers, -WarningsAsErrors: '' +WarningsAsErrors: '*' HeaderFileExtensions: - '' - h From 536694a19e3ab491a6f79c3741daba1b763fbd26 Mon Sep 17 00:00:00 2001 From: Eduardo-Barreto Date: Wed, 1 Jan 2025 00:43:31 -0300 Subject: [PATCH 262/561] =?UTF-8?q?=F0=9F=92=9A=20Test=20with=20format=20e?= =?UTF-8?q?rror?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/adc_dma.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 2525c34..3f74161 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -7,12 +7,13 @@ #include "micras/hal/adc_dma.hpp" namespace micras::hal { -AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, handle{config.handle} { +AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, handle{config.handle} +{ config.init_function(); HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); } -void AdcDma::start_dma(std::span buffer) { +void AdcDma::start_dma( std::span buffer) { HAL_ADC_Start_DMA(this->handle, buffer.data(), buffer.size()); } From 856f0c5feab8c51a44028964533b0b2ee8fc0240 Mon Sep 17 00:00:00 2001 From: Eduardo-Barreto Date: Wed, 1 Jan 2025 00:49:47 -0300 Subject: [PATCH 263/561] =?UTF-8?q?=F0=9F=92=9A=20Test=20with=20everything?= =?UTF-8?q?=20alright?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit olha o verdinho --- micras_hal/stm32/src/adc_dma.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 3f74161..2525c34 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -7,13 +7,12 @@ #include "micras/hal/adc_dma.hpp" namespace micras::hal { -AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, handle{config.handle} -{ +AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, handle{config.handle} { config.init_function(); HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); } -void AdcDma::start_dma( std::span buffer) { +void AdcDma::start_dma(std::span buffer) { HAL_ADC_Start_DMA(this->handle, buffer.data(), buffer.size()); } From 63f666ceebdbf896906d18ff97214a45e82cf2b2 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 2 Jan 2025 03:23:01 -0300 Subject: [PATCH 264/561] =?UTF-8?q?=E2=AC=86=EF=B8=8F=20Update=20imu=20lib?= =?UTF-8?q?=20version?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/libs/lsm6dsv-pid | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_proxy/libs/lsm6dsv-pid b/micras_proxy/libs/lsm6dsv-pid index ceed5e6..7b98628 160000 --- a/micras_proxy/libs/lsm6dsv-pid +++ b/micras_proxy/libs/lsm6dsv-pid @@ -1 +1 @@ -Subproject commit ceed5e6ed33e6eb40e9754b5ddb8cad4a47e5130 +Subproject commit 7b98628713090244cf14f54d9059938cb00342fd From 40eb0f7c956bfd1ec68ffbe9af59d88b79108de9 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 5 Jan 2025 23:48:20 -0300 Subject: [PATCH 265/561] Create emergency branch --- micras_nav/include/micras/nav/follow_wall.hpp | 11 ----- micras_nav/src/follow_wall.cpp | 9 +--- micras_nav/src/mapping.cpp | 4 +- micras_proxy/src/rotary_sensor.cpp | 46 +++++++++---------- 4 files changed, 26 insertions(+), 44 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 47b73e2..08a48c4 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -22,7 +22,6 @@ class FollowWall { PidController::Config pid; float base_left_reading{}; float base_right_reading{}; - float cutoff_frequency{}; }; /** @@ -78,16 +77,6 @@ class FollowWall { */ PidController pid; - /** - * @brief Butterworth filter for the derivative of the error - */ - core::ButterworthFilter filter; - - /** - * @brief Last difference between the left and right wall sensors - */ - float last_diff{}; - /** * @brief Base reading of the left wall sensor when aligned with the wall */ diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index b270519..c6478f8 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -8,7 +8,6 @@ namespace micras::nav { FollowWall::FollowWall(const proxy::WallSensors<4>& wall_sensors, const Config& config) : wall_sensors{wall_sensors}, pid{config.pid}, - filter{config.cutoff_frequency}, base_left_reading{config.base_left_reading}, base_right_reading{config.base_right_reading} { } @@ -35,14 +34,8 @@ float FollowWall::action(core::FollowWallType follow_wall_type, float elapsed_ti return 0.0F; } - if (this->last_diff == 0.0F) { - this->last_diff = error; - } - - float response = - this->pid.update(error, elapsed_time, this->filter.update((error - this->last_diff) / elapsed_time), true); + float response = this->pid.update(error, elapsed_time); - this->last_diff = error; return response; } diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 47184f8..58a6f7f 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -170,8 +170,8 @@ Pose Mapping::correct_pose(const Pose& pose, core::FollowWallType template void Mapping::calibrate_front() { this->front_distance_reading[0] = this->wall_sensors.get_reading(0); - front_orientation_reading[0] = this->wall_sensors.get_reading(1); - front_orientation_reading[1] = this->wall_sensors.get_reading(2); + this->front_orientation_reading[0] = this->wall_sensors.get_reading(1); + this->front_orientation_reading[1] = this->wall_sensors.get_reading(2); this->front_distance_reading[1] = this->wall_sensors.get_reading(3); } diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index a00e178..2a7b91b 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -10,36 +10,36 @@ namespace micras::proxy { RotarySensor::RotarySensor(const Config& config) : spi{config.spi}, encoder{config.encoder}, crc{config.crc}, resolution{config.resolution} { - CommandFrame command_frame{}; - DataFrame data_frame{}; + // CommandFrame command_frame{}; + // DataFrame data_frame{}; - command_frame.fields.address = Registers::disable_addr; - data_frame.fields.data = config.registers.disable.raw; - this->write_register(command_frame, data_frame); + // command_frame.fields.address = Registers::disable_addr; + // data_frame.fields.data = config.registers.disable.raw; + // this->write_register(command_frame, data_frame); - command_frame.fields.address = Registers::zposm_addr; - data_frame.fields.data = config.registers.zposm.raw; - this->write_register(command_frame, data_frame); + // command_frame.fields.address = Registers::zposm_addr; + // data_frame.fields.data = config.registers.zposm.raw; + // this->write_register(command_frame, data_frame); - command_frame.fields.address = Registers::zposl_addr; - data_frame.fields.data = config.registers.zposl.raw; - this->write_register(command_frame, data_frame); + // command_frame.fields.address = Registers::zposl_addr; + // data_frame.fields.data = config.registers.zposl.raw; + // this->write_register(command_frame, data_frame); - command_frame.fields.address = Registers::settings1_addr; - data_frame.fields.data = config.registers.settings1.raw; - this->write_register(command_frame, data_frame); + // command_frame.fields.address = Registers::settings1_addr; + // data_frame.fields.data = config.registers.settings1.raw; + // this->write_register(command_frame, data_frame); - command_frame.fields.address = Registers::settings2_addr; - data_frame.fields.data = config.registers.settings2.raw; - this->write_register(command_frame, data_frame); + // command_frame.fields.address = Registers::settings2_addr; + // data_frame.fields.data = config.registers.settings2.raw; + // this->write_register(command_frame, data_frame); - command_frame.fields.address = Registers::settings3_addr; - data_frame.fields.data = config.registers.settings3.raw; - this->write_register(command_frame, data_frame); + // command_frame.fields.address = Registers::settings3_addr; + // data_frame.fields.data = config.registers.settings3.raw; + // this->write_register(command_frame, data_frame); - command_frame.fields.address = Registers::ecc_addr; - data_frame.fields.data = config.registers.ecc.raw; - this->write_register(command_frame, data_frame); + // command_frame.fields.address = Registers::ecc_addr; + // data_frame.fields.data = config.registers.ecc.raw; + // this->write_register(command_frame, data_frame); } float RotarySensor::get_position() const { From 7f5b839fe1c783db9361591821ecc855557f2bb9 Mon Sep 17 00:00:00 2001 From: Eduardo-Barreto Date: Sat, 29 Mar 2025 17:38:44 -0300 Subject: [PATCH 266/561] =?UTF-8?q?=F0=9F=94=A7=20Update=20clang-format=20?= =?UTF-8?q?rules?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-format | 171 +++++--------------------------------------------- 1 file changed, 16 insertions(+), 155 deletions(-) diff --git a/.clang-format b/.clang-format index 4c488a9..45852c7 100644 --- a/.clang-format +++ b/.clang-format @@ -1,186 +1,47 @@ --- Language: Cpp -BasedOnStyle: LLVM AccessModifierOffset: -4 AlignAfterOpenBracket: BlockIndent -AlignArrayOfStructures: None -AlignConsecutiveMacros: None -AlignConsecutiveAssignments: None -AlignConsecutiveBitFields: Consecutive -AlignConsecutiveDeclarations: Consecutive +AlignConsecutiveBitFields: + Enabled: true + PadOperators: true +AlignConsecutiveDeclarations: + Enabled: true + PadOperators: true +AlignConsecutiveMacros: + PadOperators: true AlignEscapedNewlines: Left -AlignOperands: Align -AlignTrailingComments: true -AllowAllArgumentsOnNextLine: true -AllowAllParametersOfDeclarationOnNextLine: true -AllowShortEnumsOnASingleLine: false AllowShortBlocksOnASingleLine: Empty -AllowShortCaseLabelsOnASingleLine: false +AllowShortEnumsOnASingleLine: false AllowShortFunctionsOnASingleLine: Inline -AllowShortLambdasOnASingleLine: All -AllowShortIfStatementsOnASingleLine: Never -AllowShortLoopsOnASingleLine: false -AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None -AlwaysBreakBeforeMultilineStrings: false AlwaysBreakTemplateDeclarations: Yes -AttributeMacros: - - __capability -BinPackArguments: true -BinPackParameters: true -BraceWrapping: - AfterCaseLabel: false - AfterClass: false - AfterControlStatement: Never - AfterEnum: false - AfterFunction: false - AfterNamespace: false - AfterObjCDeclaration: false - AfterStruct: false - AfterUnion: false - AfterExternBlock: false - BeforeCatch: false - BeforeElse: false - BeforeLambdaBody: false - BeforeWhile: false - IndentBraces: false - SplitEmptyFunction: true - SplitEmptyRecord: true - SplitEmptyNamespace: true -BreakBeforeBinaryOperators: None -BreakBeforeConceptDeclarations: true -BreakBeforeBraces: Attach -BreakBeforeInheritanceComma: false -BreakInheritanceList: AfterColon BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: false BreakConstructorInitializers: AfterColon -BreakAfterJavaFieldAnnotations: false -BreakStringLiterals: true +BreakInheritanceList: AfterColon ColumnLimit: 120 -CommentPragmas: '^ IWYU pragma:' -QualifierAlignment: Leave -CompactNamespaces: false -ConstructorInitializerIndentWidth: 4 -ContinuationIndentWidth: 4 -Cpp11BracedListStyle: true -DeriveLineEnding: true -DerivePointerAlignment: false -DisableFormat: false -EmptyLineAfterAccessModifier: Never EmptyLineBeforeAccessModifier: Always -ExperimentalAutoDetectBinPacking: false -PackConstructorInitializers: NextLine -ConstructorInitializerAllOnOneLineOrOnePerLine: false -AllowAllConstructorInitializersOnNextLine: true -FixNamespaceComments: true -ForEachMacros: - - foreach - - Q_FOREACH - - BOOST_FOREACH -IfMacros: - - KJ_IF_MAYBE -IncludeBlocks: Preserve IncludeCategories: - Regex: '^<.*>' Priority: 1 SortPriority: 1 - CaseSensitive: false - Regex: '^".*"' - Priority: 2 SortPriority: 2 - CaseSensitive: false IncludeIsMainRegex: '$' -IncludeIsMainSourceRegex: '' -IndentAccessModifiers: false IndentCaseLabels: true -IndentCaseBlocks: false -IndentGotoLabels: true -IndentPPDirectives: BeforeHash IndentExternBlock: NoIndent -IndentRequires: false +IndentPPDirectives: BeforeHash +IndentRequiresClause: false IndentWidth: 4 IndentWrappedFunctionNames: true -InsertTrailingCommas: None -JavaScriptQuotes: Leave -JavaScriptWrapImports: true KeepEmptyLinesAtTheStartOfBlocks: false -LambdaBodyIndentation: Signature -MacroBlockBegin: '' -MacroBlockEnd: '' -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCBinPackProtocolList: Auto -ObjCBlockIndentWidth: 2 -ObjCBreakBeforeNestedBlockParam: true -ObjCSpaceAfterProperty: false -ObjCSpaceBeforeProtocolList: true -PenaltyBreakAssignment: 2 -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 300 -PenaltyBreakFirstLessLess: 120 -PenaltyBreakOpenParenthesis: 0 -PenaltyBreakString: 1000 -PenaltyBreakTemplateDeclaration: 10 -PenaltyExcessCharacter: 1000000 -PenaltyReturnTypeOnItsOwnLine: 60 -PenaltyIndentedWhitespace: 0 +PackConstructorInitializers: NextLine PointerAlignment: Left -PPIndentWidth: -1 -ReferenceAlignment: Pointer -ReflowComments: true -RemoveBracesLLVM: false SeparateDefinitionBlocks: Always ShortNamespaceLines: 0 -SortIncludes: Never -SortJavaStaticImport: Before -SortUsingDeclarations: true -SpaceAfterCStyleCast: false -SpaceAfterLogicalNot: false -SpaceAfterTemplateKeyword: true -SpaceBeforeAssignmentOperators: true -SpaceBeforeCaseColon: false -SpaceBeforeCpp11BracedList: false -SpaceBeforeCtorInitializerColon: true -SpaceBeforeInheritanceColon: true -SpaceBeforeParens: ControlStatements -SpaceBeforeParensOptions: - AfterControlStatements: true - AfterForeachMacros: true - AfterFunctionDefinitionName: false - AfterFunctionDeclarationName: false - AfterIfMacros: true - AfterOverloadedOperator: false - BeforeNonEmptyParentheses: false -SpaceAroundPointerQualifiers: Default -SpaceBeforeRangeBasedForLoopColon: true +SortIncludes: Never SpaceInEmptyBlock: true -SpaceInEmptyParentheses: false SpacesBeforeTrailingComments: 2 -SpacesInAngles: Never -SpacesInConditionalStatement: false -SpacesInContainerLiterals: true -SpacesInCStyleCastParentheses: false -SpacesInLineCommentPrefix: - Minimum: 1 - Maximum: -1 -SpacesInParentheses: false -SpacesInSquareBrackets: false -SpaceBeforeSquareBrackets: false -BitFieldColonSpacing: Both -Standard: c++20 -StatementAttributeLikeMacros: - - Q_EMIT -StatementMacros: - - Q_UNUSED - - QT_REQUIRE_VERSION +Standard: c++20 TabWidth: 4 -UseCRLF: false -UseTab: Never -WhitespaceSensitiveMacros: - - STRINGIZE - - PP_STRINGIZE - - BOOST_PP_STRINGIZE - - NS_SWIFT_NAME - - CF_SWIFT_NAME ... + From 85b58c8701914cb7cee52329b5821191a21fc6fb Mon Sep 17 00:00:00 2001 From: Eduardo-Barreto Date: Sat, 29 Mar 2025 17:39:59 -0300 Subject: [PATCH 267/561] =?UTF-8?q?=F0=9F=94=A7=20Remove=20cpp=20standard?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-format | 1 - 1 file changed, 1 deletion(-) diff --git a/.clang-format b/.clang-format index 45852c7..3e2e0ba 100644 --- a/.clang-format +++ b/.clang-format @@ -41,7 +41,6 @@ ShortNamespaceLines: 0 SortIncludes: Never SpaceInEmptyBlock: true SpacesBeforeTrailingComments: 2 -Standard: c++20 TabWidth: 4 ... From cacddaa03c1b020866e846bb47744c4ac7e43439 Mon Sep 17 00:00:00 2001 From: Eduardo-Barreto Date: Sat, 29 Mar 2025 17:40:15 -0300 Subject: [PATCH 268/561] =?UTF-8?q?=F0=9F=94=A7=20Remove=20trailing=20line?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-format | 1 - 1 file changed, 1 deletion(-) diff --git a/.clang-format b/.clang-format index 3e2e0ba..5ab089a 100644 --- a/.clang-format +++ b/.clang-format @@ -43,4 +43,3 @@ SpaceInEmptyBlock: true SpacesBeforeTrailingComments: 2 TabWidth: 4 ... - From ad2ee13a7c2f32a4730b4dba407ab3e2a8657185 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 31 Mar 2025 23:45:47 -0300 Subject: [PATCH 269/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20PID=20Controller?= =?UTF-8?q?=20to=20core?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/core}/pid_controller.hpp | 12 +++++----- .../src/pid_controller.cpp | 6 ++--- micras_nav/include/micras/nav/follow_wall.hpp | 12 +++++----- micras_nav/include/micras/nav/go_to_point.hpp | 22 +++++++++---------- .../include/micras/nav/look_at_point.hpp | 14 ++++++------ 5 files changed, 32 insertions(+), 34 deletions(-) rename {micras_nav/include/micras/nav => micras_core/include/micras/core}/pid_controller.hpp (92%) rename {micras_nav => micras_core}/src/pid_controller.cpp (95%) diff --git a/micras_nav/include/micras/nav/pid_controller.hpp b/micras_core/include/micras/core/pid_controller.hpp similarity index 92% rename from micras_nav/include/micras/nav/pid_controller.hpp rename to micras_core/include/micras/core/pid_controller.hpp index 7b1ca02..5028a38 100644 --- a/micras_nav/include/micras/nav/pid_controller.hpp +++ b/micras_core/include/micras/core/pid_controller.hpp @@ -2,12 +2,10 @@ * @file */ -#ifndef MICRAS_NAV_PID_CONTROLLER_HPP -#define MICRAS_NAV_PID_CONTROLLER_HPP +#ifndef MICRAS_CORE_PID_CONTROLLER_HPP +#define MICRAS_CORE_PID_CONTROLLER_HPP -#include "micras/hal/timer.hpp" - -namespace micras::nav { +namespace micras::core { /** * @brief Implementation of simple PID controller. Response = kp * (error + ki * integral(error) kd * d/dt(error)) */ @@ -111,6 +109,6 @@ class PidController { */ float last_response = 0; }; -} // namespace micras::nav +} // namespace micras::core -#endif // MICRAS_NAV_PID_CONTROLLER_HPP +#endif // MICRAS_CORE_PID_CONTROLLER_HPP diff --git a/micras_nav/src/pid_controller.cpp b/micras_core/src/pid_controller.cpp similarity index 95% rename from micras_nav/src/pid_controller.cpp rename to micras_core/src/pid_controller.cpp index ae8e60a..5375932 100644 --- a/micras_nav/src/pid_controller.cpp +++ b/micras_core/src/pid_controller.cpp @@ -5,7 +5,7 @@ #include #include -#include "micras/nav/pid_controller.hpp" +#include "micras/core/pid_controller.hpp" // NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) static volatile float test_error; @@ -16,7 +16,7 @@ static volatile float test_response; // NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) -namespace micras::nav { +namespace micras::core { PidController::PidController(Config config) : kp{config.kp}, ki{config.ki}, @@ -75,4 +75,4 @@ float PidController::update(float state, float elapsed_time, float state_change, return response; } -} // namespace micras::nav +} // namespace micras::core diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index dbad186..d90bf09 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -6,7 +6,7 @@ #define MICRAS_NAV_FOLLOW_WALL #include "micras/core/butterworth_filter.hpp" -#include "micras/nav/pid_controller.hpp" +#include "micras/core/pid_controller.hpp" #include "micras/proxy/wall_sensors.hpp" namespace micras::nav { @@ -19,10 +19,10 @@ class FollowWall { * @brief Configuration struct for the FollowWall class. */ struct Config { - PidController::Config pid; - float base_left_reading{}; - float base_right_reading{}; - float cutoff_frequency{}; + core::PidController::Config pid; + float base_left_reading{}; + float base_right_reading{}; + float cutoff_frequency{}; }; /** @@ -75,7 +75,7 @@ class FollowWall { /** * @brief PID controller for the wall following. */ - PidController pid; + core::PidController pid; /** * @brief Butterworth filter for the derivative of the error. diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index 2315ea2..abae30f 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -5,8 +5,8 @@ #ifndef MICRAS_NAV_GO_TO_POINT_HPP #define MICRAS_NAV_GO_TO_POINT_HPP +#include "micras/core/pid_controller.hpp" #include "micras/nav/follow_wall.hpp" -#include "micras/nav/pid_controller.hpp" #include "micras/nav/state.hpp" namespace micras::nav { @@ -19,14 +19,14 @@ class GoToPoint { * @brief Configuration struct for the GoToPoint class. */ struct Config { - PidController::Config stop_pid; - PidController::Config angular_pid; - float cell_size{}; - float min_linear_command{}; - float max_linear_command{}; - float deceleration_factor{}; - float distance_tolerance{}; - float velocity_tolerance{}; + core::PidController::Config stop_pid; + core::PidController::Config angular_pid; + float cell_size{}; + float min_linear_command{}; + float max_linear_command{}; + float deceleration_factor{}; + float distance_tolerance{}; + float velocity_tolerance{}; }; /** @@ -79,12 +79,12 @@ class GoToPoint { /** * @brief PID controller for stopping at the goal. */ - PidController stop_pid; + core::PidController stop_pid; /** * @brief PID controller for the orientation. */ - PidController angular_pid; + core::PidController angular_pid; /** * @brief FollowWall controller. diff --git a/micras_nav/include/micras/nav/look_at_point.hpp b/micras_nav/include/micras/nav/look_at_point.hpp index 8411881..4bfaacd 100644 --- a/micras_nav/include/micras/nav/look_at_point.hpp +++ b/micras_nav/include/micras/nav/look_at_point.hpp @@ -5,7 +5,7 @@ #ifndef MICRAS_NAV_LOOK_AT_POINT_HPP #define MICRAS_NAV_LOOK_AT_POINT_HPP -#include "micras/nav/pid_controller.hpp" +#include "micras/core/pid_controller.hpp" #include "micras/nav/state.hpp" namespace micras::nav { @@ -18,10 +18,10 @@ class LookAtPoint { * @brief Configuration struct for the LookAtPoint class. */ struct Config { - PidController::Config linear_pid; - PidController::Config angular_pid; - float distance_tolerance{}; - float velocity_tolerance{}; + core::PidController::Config linear_pid; + core::PidController::Config angular_pid; + float distance_tolerance{}; + float velocity_tolerance{}; }; /** @@ -60,12 +60,12 @@ class LookAtPoint { /** * @brief PID controller for the linear velocity. */ - PidController linear_pid; + core::PidController linear_pid; /** * @brief PID controller for the angular velocity. */ - PidController angular_pid; + core::PidController angular_pid; /** * @brief Angular tolerance for the goal. From f6375a4b5d2d961dcc3a9a46cebfc98d3103367a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 31 Mar 2025 23:47:04 -0300 Subject: [PATCH 270/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20friend=20class?= =?UTF-8?q?=20interface?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/odometry.hpp | 4 +--- micras_proxy/include/micras/proxy/wall_sensors.hpp | 2 -- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index f7e8cd9..2fdebe7 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -29,7 +29,7 @@ class Odometry { }; /** - * @brief Construct a newOdometry object. + * @brief Construct a new Odometry object. * * @param left_rotary_sensor Left rotary sensor. * @param right_rotary_sensor Right rotary sensor. @@ -107,8 +107,6 @@ class Odometry { * @brief Current state of the robot in space. */ State state; - - friend class Interface; }; } // namespace micras::nav diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 772fea0..0b12e57 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -164,8 +164,6 @@ class TWallSensors { * @brief Maximum reading value to identify a free space. */ std::array free_space_threshold; - - friend class Interface; }; } // namespace micras::proxy From e70741f0287cf68ecd448fc616c8296ea3774800 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Apr 2025 00:46:54 -0300 Subject: [PATCH 271/561] =?UTF-8?q?=F0=9F=9A=9A=20Rename=20MicrasControlle?= =?UTF-8?q?r=20to=20Micras?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/src/butterworth_filter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index f27ff3c..7a8803a 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -36,13 +36,13 @@ float ButterworthFilter::update(float x0) { float x_b_dot = 0; - for (int i = 0; i < filter_order + 1; i++) { + for (uint8_t i = 0; i < filter_order + 1; i++) { x_b_dot += this->x_array[i] * this->b_array[i]; // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index) } float y_a_dot = 0; - for (int i = 0; i < filter_order; i++) { + for (uint8_t i = 0; i < filter_order; i++) { y_a_dot += this->y_array[i] * this->a_array[i]; // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index) } From 373690873a3b4a448a90fd1e0e0d3de0bd2d95ff Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 2 Apr 2025 23:26:07 -0300 Subject: [PATCH 272/561] =?UTF-8?q?=E2=9C=A8=20Add=20FSM=20class=20to=20co?= =?UTF-8?q?re?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Claudio Dardengo Jr --- micras_core/include/micras/core/fsm.hpp | 91 +++++++++++++++++++++++++ micras_core/src/fsm.cpp | 27 ++++++++ 2 files changed, 118 insertions(+) create mode 100644 micras_core/include/micras/core/fsm.hpp create mode 100644 micras_core/src/fsm.cpp diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp new file mode 100644 index 0000000..80d6881 --- /dev/null +++ b/micras_core/include/micras/core/fsm.hpp @@ -0,0 +1,91 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_FSM_HPP +#define MICRAS_CORE_FSM_HPP + +#include +#include + +namespace micras::core { +class FSM { +public: + class State { + public: + /** + * @brief Construct a new State object. + * + * @param id The id of the state. + */ + explicit State(uint8_t id); + + /** + * @brief Destroy the State object. + */ + virtual ~State() = default; + + /** + * @brief Special member functions declared as default. + */ + ///@{ + State(const State&) = default; + State(State&&) = default; + State& operator=(const State&) = default; + State& operator=(State&&) = default; + ///@} + + /** + * @brief Execute this state. + * + * @return The id of the next state. + */ + virtual uint8_t run(); + + /** + * @brief Get the id object of the state. + * + * @return The id of the state. + */ + uint8_t get_id() const; + + private: + /** + * @brief Fixed id of the state. + */ + uint8_t id; + }; + + /** + * @brief Construct a new FSM object. + * + * @param initial_state_id The id of the initial state. + */ + explicit FSM(uint8_t initial_state_id); + + /** + * @brief Add a state to the FSM. + * + * @param state The state to be added. + */ + void add_state(const State& state); + + /** + * @brief Run the FSM current state to calculate the next state. + */ + void run(); + +private: + /** + * @brief Map of ids to states. + */ + std::unordered_map states; + + /** + * @brief Id of the state currently running. + */ + uint8_t current_state_id{0}; +}; +} // namespace micras::core + +#endif // MICRAS_CORE_FSM_HPP diff --git a/micras_core/src/fsm.cpp b/micras_core/src/fsm.cpp new file mode 100644 index 0000000..474187b --- /dev/null +++ b/micras_core/src/fsm.cpp @@ -0,0 +1,27 @@ +/** + * @file + */ + +#include "micras/core/fsm.hpp" + +namespace micras::core { +FSM::State::State(uint8_t id) : id{id} { } + +uint8_t FSM::State::run() { + return this->get_id(); +} + +uint8_t FSM::State::get_id() const { + return this->id; +} + +FSM::FSM(uint8_t initial_state_id) : current_state_id{initial_state_id} { } + +void FSM::add_state(const State& state) { + this->states.emplace(state.get_id(), state); +} + +void FSM::run() { + this->current_state_id = this->states.at(this->current_state_id).run(); +} +} // namespace micras::core From 15f04d87939aa0be31ad5292556ff3013c38578c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 3 Apr 2025 02:00:19 -0300 Subject: [PATCH 273/561] =?UTF-8?q?=E2=9C=A8=20Add=20BaseSate=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/fsm.hpp | 43 +++++++++++++------------ micras_core/src/fsm.cpp | 12 +++---- 2 files changed, 28 insertions(+), 27 deletions(-) diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 80d6881..96fa29d 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -6,6 +6,7 @@ #define MICRAS_CORE_FSM_HPP #include +#include #include namespace micras::core { @@ -13,34 +14,19 @@ class FSM { public: class State { public: - /** - * @brief Construct a new State object. - * - * @param id The id of the state. - */ - explicit State(uint8_t id); - /** * @brief Destroy the State object. */ virtual ~State() = default; - /** - * @brief Special member functions declared as default. - */ - ///@{ - State(const State&) = default; - State(State&&) = default; - State& operator=(const State&) = default; - State& operator=(State&&) = default; - ///@} - /** * @brief Execute this state. * + * @param previous_state_id The id of the last executed state. + * * @return The id of the next state. */ - virtual uint8_t run(); + virtual uint8_t run(uint8_t previous_state_id) = 0; /** * @brief Get the id object of the state. @@ -49,6 +35,18 @@ class FSM { */ uint8_t get_id() const; + protected: + /** + * @brief Special member functions declared as default. + */ + ///@{ + explicit State(uint8_t id); + State(const State&) = default; + State(State&&) = default; + State& operator=(const State&) = default; + State& operator=(State&&) = default; + ///@} + private: /** * @brief Fixed id of the state. @@ -68,7 +66,7 @@ class FSM { * * @param state The state to be added. */ - void add_state(const State& state); + void add_state(std::unique_ptr state); /** * @brief Run the FSM current state to calculate the next state. @@ -79,12 +77,17 @@ class FSM { /** * @brief Map of ids to states. */ - std::unordered_map states; + std::unordered_map> states; /** * @brief Id of the state currently running. */ uint8_t current_state_id{0}; + + /** + * @brief Id of the last executed state. + */ + uint8_t previous_state_id{0xFF}; }; } // namespace micras::core diff --git a/micras_core/src/fsm.cpp b/micras_core/src/fsm.cpp index 474187b..25313b8 100644 --- a/micras_core/src/fsm.cpp +++ b/micras_core/src/fsm.cpp @@ -7,21 +7,19 @@ namespace micras::core { FSM::State::State(uint8_t id) : id{id} { } -uint8_t FSM::State::run() { - return this->get_id(); -} - uint8_t FSM::State::get_id() const { return this->id; } FSM::FSM(uint8_t initial_state_id) : current_state_id{initial_state_id} { } -void FSM::add_state(const State& state) { - this->states.emplace(state.get_id(), state); +void FSM::add_state(std::unique_ptr state) { + this->states.emplace(state->get_id(), std::move(state)); } void FSM::run() { - this->current_state_id = this->states.at(this->current_state_id).run(); + uint8_t next_state = this->states.at(this->current_state_id)->run(this->previous_state_id); + this->previous_state_id = this->current_state_id; + this->current_state_id = next_state; } } // namespace micras::core From b46ea71de93aab82ae1db0061267cc6fdb47afda Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 3 Apr 2025 02:27:47 -0300 Subject: [PATCH 274/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20fsm=20map=20type?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/fsm.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 96fa29d..153cbe1 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -77,7 +77,7 @@ class FSM { /** * @brief Map of ids to states. */ - std::unordered_map> states; + std::unordered_map> states; /** * @brief Id of the state currently running. From f304af11da03c621e921f554d4f7ce6b2532bda7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 4 Apr 2025 19:16:31 -0300 Subject: [PATCH 275/561] =?UTF-8?q?=F0=9F=9A=A7=20Add=20const=20to=20varia?= =?UTF-8?q?bles=20to=20remove=20linter=20warnings?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 13 ++++--------- micras_core/include/micras/core/fsm.hpp | 7 ++++++- micras_core/src/butterworth_filter.cpp | 18 +++++++++--------- micras_core/src/fsm.cpp | 2 +- micras_core/src/pid_controller.cpp | 8 ++++---- micras_hal/stm32/src/encoder.cpp | 5 +++-- micras_hal/stm32/src/flash.cpp | 4 ++-- micras_hal/stm32/src/pwm.cpp | 8 ++++---- micras_hal/stm32/src/timer.cpp | 10 +++++----- micras_nav/include/micras/nav/follow_wall.hpp | 2 +- micras_nav/include/micras/nav/grid_pose.hpp | 4 ++-- micras_nav/include/micras/nav/mapping.hpp | 2 +- micras_nav/include/micras/nav/odometry.hpp | 6 +++--- micras_nav/src/follow_wall.cpp | 2 +- micras_nav/src/go_to_point.cpp | 9 +++++---- micras_nav/src/grid_pose.cpp | 2 +- micras_nav/src/look_at_point.cpp | 2 +- micras_nav/src/mapping.cpp | 12 ++++++------ micras_nav/src/odometry.cpp | 16 ++++++++-------- micras_nav/src/state.cpp | 4 ++-- micras_proxy/src/button.cpp | 2 +- micras_proxy/src/buzzer.cpp | 2 +- micras_proxy/src/motor.cpp | 2 +- micras_proxy/src/rotary_sensor.cpp | 4 ++-- micras_proxy/src/storage.cpp | 12 ++++++------ 25 files changed, 80 insertions(+), 78 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index fd05192..936865b 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -3,29 +3,24 @@ Checks: -*, bugprone-*, -bugprone-easily-swappable-parameters, - -bugprone-narrowing-conversions, -bugprone-dynamic-static-initializers, clang-*, - -clang-diagnostic-unused-command-line-argument, cppcoreguidelines-*, -cppcoreguidelines-avoid-magic-numbers, - -cppcoreguidelines-pro-bounds-pointer-arithmetic, -cppcoreguidelines-avoid-do-while, + -cppcoreguidelines-narrowing-conversions, -cppcoreguidelines-pro-type-union-access, google-*, llvm-*, -llvm-header-guard, misc-*, -misc-include-cleaner, - -misc-const-correctness, - -misc-use-anonymous-namespace, modernize-*, -modernize-use-nodiscard, -modernize-use-trailing-return-type, performance-*, readability-*, -readability-duplicate-include, - -readability-magic-numbers, WarningsAsErrors: '*' HeaderFileExtensions: - '' @@ -41,16 +36,16 @@ ImplementationFileExtensions: HeaderFilterRegex: '(include|config)/.*.hpp' FormatStyle: none CheckOptions: + readability-magic-numbers.IgnoredIntegerValues: '1;2;3;4;5;10;100;255;1000;65535;1000000' readability-magic-numbers.IgnorePowersOf2IntegerValues: true - readability-magic-numbers.IgnoreBitFieldsWidths: true readability-magic-numbers.IgnoreUserDefinedLiterals: true + readability-magic-numbers.IgnoredFloatingPointValues: '1.0;2.0;5.0;10.0;50.0;100.0;1000.0;1000000.0' readability-identifier-length.MinimumVariableNameLength: 2 readability-identifier-length.IgnoredVariableNames: ^[nxyz]$ readability-identifier-length.MinimumParameterNameLength: 2 readability-identifier-length.IgnoredParameterNames: ^[nxyz]$ misc-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic: true - cppcoreguidelines-narrowing-conversions.WarnOnIntegerToFloatingPointNarrowingConversion: false - readability-function-cognitive-complexity.Threshold: 30 + bugprone-narrowing-conversions.WarnOnIntegerToFloatingPointNarrowingConversion: false SystemHeaders: false ExtraArgsBefore: - '--target=arm-none-eabi' diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 153cbe1..a850cc6 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -35,6 +35,11 @@ class FSM { */ uint8_t get_id() const; + /** + * @brief The id of the state that is not valid. + */ + static constexpr uint8_t invalid_id{0xFF}; + protected: /** * @brief Special member functions declared as default. @@ -87,7 +92,7 @@ class FSM { /** * @brief Id of the last executed state. */ - uint8_t previous_state_id{0xFF}; + uint8_t previous_state_id{State::invalid_id}; }; } // namespace micras::core diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index 7a8803a..e623d80 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -9,17 +9,17 @@ namespace micras::core { ButterworthFilter::ButterworthFilter(float cutoff_frequency, float sampling_frequency) { - float relative_frequency = cutoff_frequency / sampling_frequency; - float relative_frequency_2 = relative_frequency * relative_frequency; + const float relative_frequency = cutoff_frequency / sampling_frequency; + const float relative_frequency_2 = relative_frequency * relative_frequency; - float b0 = 1; - float b1 = 2; - float b2 = 1; + const float b0 = 1; + const float b1 = 2; + const float b2 = 1; // Butterworth filter coefficients - float a0 = 1 + 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; - float a1 = 2 - 8 / relative_frequency_2; - float a2 = 1 - 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; + const float a0 = 1 + 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; + const float a1 = 2 - 8 / relative_frequency_2; + const float a2 = 1 - 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; this->a_array[0] = a2 / a0; this->a_array[1] = a1 / a0; @@ -46,7 +46,7 @@ float ButterworthFilter::update(float x0) { y_a_dot += this->y_array[i] * this->a_array[i]; // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index) } - float y0 = x_b_dot - y_a_dot; + const float y0 = x_b_dot - y_a_dot; this->y_array[0] = this->y_array[1]; this->y_array[1] = y0; diff --git a/micras_core/src/fsm.cpp b/micras_core/src/fsm.cpp index 25313b8..8664459 100644 --- a/micras_core/src/fsm.cpp +++ b/micras_core/src/fsm.cpp @@ -18,7 +18,7 @@ void FSM::add_state(std::unique_ptr state) { } void FSM::run() { - uint8_t next_state = this->states.at(this->current_state_id)->run(this->previous_state_id); + const uint8_t next_state = this->states.at(this->current_state_id)->run(this->previous_state_id); this->previous_state_id = this->current_state_id; this->current_state_id = next_state; } diff --git a/micras_core/src/pid_controller.cpp b/micras_core/src/pid_controller.cpp index 5375932..20d7698 100644 --- a/micras_core/src/pid_controller.cpp +++ b/micras_core/src/pid_controller.cpp @@ -7,14 +7,14 @@ #include "micras/core/pid_controller.hpp" -// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables, misc-use-anonymous-namespace) static volatile float test_error; static volatile float test_proportional; static volatile float test_integrative; static volatile float test_derivative; static volatile float test_response; -// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables, misc-use-anonymous-namespace) namespace micras::core { PidController::PidController(Config config) : @@ -36,12 +36,12 @@ void PidController::reset() { } float PidController::update(float state, float elapsed_time, bool save) { - float state_change = (state - this->prev_state) / elapsed_time; + const float state_change = (state - this->prev_state) / elapsed_time; return this->update(state, elapsed_time, state_change, save); } float PidController::update(float state, float elapsed_time, float state_change, bool save) { - float error = this->setpoint - state; + const float error = this->setpoint - state; this->prev_state = state; float response = this->kp * (error + this->ki * this->error_acc - this->kd * state_change); diff --git a/micras_hal/stm32/src/encoder.cpp b/micras_hal/stm32/src/encoder.cpp index 315817b..419a1cb 100644 --- a/micras_hal/stm32/src/encoder.cpp +++ b/micras_hal/stm32/src/encoder.cpp @@ -2,6 +2,8 @@ * @file */ +#include + #include "micras/hal/encoder.hpp" namespace micras::hal { @@ -15,7 +17,6 @@ Encoder::Encoder(const Config& config) : handle{config.handle} { } int32_t Encoder::get_counter() const { - // NOLINTNEXTLINE(cppcoreguidelines-narrowing-conversions) - return static_cast(__HAL_TIM_GET_COUNTER(this->handle)) - this->start_count; + return std::bit_cast(__HAL_TIM_GET_COUNTER(this->handle) - this->start_count); } } // namespace micras::hal diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index 86a2139..d53d901 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -16,7 +16,7 @@ static const uint32_t base_address = FLASH_BASE + FLASH_SIZE - 8; // NOLINTNEXTLINE(*-avoid-c-arrays) void Flash::read(uint32_t address, uint64_t data[], uint32_t size) { - uint32_t end = base_address - 8 * address; + const uint32_t end = base_address - 8 * address; for (address = end - 8 * (size - 1); address <= end; address += 8, data++) { (*data) = *(std::bit_cast(address)); @@ -32,7 +32,7 @@ void Flash::read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { HAL_FLASH_Unlock(); - uint32_t end = base_address - 8 * address; + const uint32_t end = base_address - 8 * address; address = end - 8 * (size - 1); while (address <= end) { diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index a88312d..7059462 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -14,15 +14,15 @@ Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_cha } void Pwm::set_duty_cycle(float duty_cycle) { - uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100.0F)); + const uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100.0F)); __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); } void Pwm::set_frequency(uint32_t frequency) { - uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); - uint32_t prescaler = this->handle->Instance->PSC; + const uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); + const uint32_t prescaler = this->handle->Instance->PSC; - uint32_t autoreload = base_freq / ((prescaler + 1) * frequency) - 1; + const uint32_t autoreload = base_freq / ((prescaler + 1) * frequency) - 1; __HAL_TIM_SET_AUTORELOAD(this->handle, autoreload); __HAL_TIM_SET_COUNTER(this->handle, 0); } diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index 2993db5..43e7e2c 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -14,8 +14,8 @@ Timer::Timer() { Timer::Timer(const Config& config) : handle{config.handle} { config.init_function(); - uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); - uint32_t prescaler = this->handle->Instance->PSC; + const uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); + const uint32_t prescaler = this->handle->Instance->PSC; if (base_freq / (prescaler + 1) == 1000000) { this->enable_microseconds = true; @@ -38,7 +38,7 @@ uint32_t Timer::elapsed_time_ms() const { } uint32_t Timer::elapsed_time_us() const { - uint32_t counter = this->get_counter_us(); + const uint32_t counter = this->get_counter_us(); if (counter < this->counter) { return std::numeric_limits::max() - this->counter + counter; @@ -48,13 +48,13 @@ uint32_t Timer::elapsed_time_us() const { } void Timer::sleep_ms(uint32_t time) { - uint32_t start = HAL_GetTick(); + const uint32_t start = HAL_GetTick(); while (HAL_GetTick() - start < time) { } } void Timer::sleep_us(uint32_t time) const { - uint32_t start = this->get_counter_us(); + const uint32_t start = this->get_counter_us(); while (this->get_counter_us() - start < time) { } } diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index d90bf09..bf490dd 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -70,7 +70,7 @@ class FollowWall { /** * @brief Wall sensors of the robot. */ - const proxy::TWallSensors<4>& wall_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + const proxy::TWallSensors<4>& wall_sensors; /** * @brief PID controller for the wall following. diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index 976582a..54c630e 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -140,8 +140,8 @@ namespace std { template <> struct hash { size_t operator()(const micras::nav::GridPoint& point) const noexcept { - size_t h1 = hash{}(point.x); - size_t h2 = hash{}(point.y); + const size_t h1 = hash{}(point.x); + const size_t h2 = hash{}(point.y); return h1 ^ (h2 << 1); } }; diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index e06045e..16f64cf 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -200,7 +200,7 @@ class TMapping : public proxy::ISerializable { /** * @brief Wall sensors of the robot. */ - const proxy::TWallSensors<4>& wall_sensors; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + const proxy::TWallSensors<4>& wall_sensors; /** * @brief The maze information the robot has. diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 2fdebe7..266d7b1 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -71,17 +71,17 @@ class Odometry { /** * @brief Left rotary sensor. */ - const proxy::RotarySensor& left_rotary_sensor; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + const proxy::RotarySensor& left_rotary_sensor; /** * @brief Right rotary sensor. */ - const proxy::RotarySensor& right_rotary_sensor; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + const proxy::RotarySensor& right_rotary_sensor; /** * @brief IMU sensor. */ - const proxy::Imu& imu; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) + const proxy::Imu& imu; /** * @brief Wheel radius. diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 54b6ed7..b209143 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -39,7 +39,7 @@ float FollowWall::action(core::FollowWallType follow_wall_type, float elapsed_ti this->last_diff = error; } - float response = + const float response = this->pid.update(error, elapsed_time, this->filter.update((error - this->last_diff) / elapsed_time), true); this->last_diff = error; diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp index 2f3d924..5dfda66 100644 --- a/micras_nav/src/go_to_point.cpp +++ b/micras_nav/src/go_to_point.cpp @@ -24,14 +24,15 @@ GoToPoint::GoToPoint( Twist GoToPoint::action( const State& state, const Point& goal, core::FollowWallType follow_wall_type, float elapsed_time, bool stop ) { - float linear_command{}; - float angular_command{}; - Point goal_distance = goal - state.pose.position; + float linear_command{}; + float angular_command{}; + const Point goal_distance = goal - state.pose.position; if (follow_wall_type != core::FollowWallType::NONE) { angular_command = this->follow_wall.action(follow_wall_type, elapsed_time); } else { - float angular_error = core::assert_angle(state.pose.orientation - std::atan2(this->cell_size, goal_distance.x)); + const float angular_error = + core::assert_angle(state.pose.orientation - std::atan2(this->cell_size, goal_distance.x)); angular_command = this->angular_pid.update(angular_error, elapsed_time, state.velocity.angular); } diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index dae0f78..88b9262 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -8,7 +8,7 @@ namespace micras::nav { Side angle_to_grid(float angle) { - int32_t grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); + const int32_t grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); return static_cast(grid_angle < 0 ? 4 + (grid_angle % 4) : grid_angle % 4); } diff --git a/micras_nav/src/look_at_point.cpp b/micras_nav/src/look_at_point.cpp index 77fcb29..c2106de 100644 --- a/micras_nav/src/look_at_point.cpp +++ b/micras_nav/src/look_at_point.cpp @@ -13,7 +13,7 @@ LookAtPoint::LookAtPoint(Config config) : velocity_tolerance{config.velocity_tolerance} { } Twist LookAtPoint::action(const State& state, const Point& goal, float elapsed_time) { - float angular_error = core::assert_angle(state.pose.orientation - state.pose.position.angle_between(goal)); + const float angular_error = core::assert_angle(state.pose.orientation - state.pose.position.angle_between(goal)); return { this->linear_pid.update(state.velocity.linear, elapsed_time), diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 7c3f79a..7193b30 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -33,7 +33,7 @@ TMapping::TMapping(const proxy::TWallSensors<4>& wall_sensors, TM template void TMapping::update(const Pose& pose) { - float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); + const float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); if (reliability < 30.0F) { return; @@ -117,16 +117,16 @@ TMapping::Action TMapping::get_action(const Pose& template Pose TMapping::correct_pose(const Pose& pose, core::FollowWallType follow_wall_type) const { - float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); + const float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); if (reliability < 20.0F) { return pose; } - Point cell_position = pose.to_cell(this->cell_size); - Point pose_correction{}; - Pose corrected_pose = pose; - Side direction = angle_to_grid(pose.orientation); + Point cell_position = pose.to_cell(this->cell_size); + Point pose_correction{}; + Pose corrected_pose = pose; + Side const direction = angle_to_grid(pose.orientation); switch (follow_wall_type) { case core::FollowWallType::BACK: diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index bdd71ca..03d62d0 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -23,24 +23,24 @@ Odometry::Odometry( state{config.initial_pose, {0.0F, 0.0F}} { } void Odometry::update(float elapsed_time) { - float left_position = this->left_rotary_sensor.get_position(); - float right_position = this->right_rotary_sensor.get_position(); + const float left_position = this->left_rotary_sensor.get_position(); + const float right_position = this->right_rotary_sensor.get_position(); - float left_distance = this->wheel_radius * (left_position - this->left_last_position); - float right_distance = this->wheel_radius * (right_position - this->right_last_position); + const float left_distance = this->wheel_radius * (left_position - this->left_last_position); + const float right_distance = this->wheel_radius * (right_position - this->right_last_position); this->left_last_position = left_position; this->right_last_position = right_position; - float linear_distance = (left_distance + right_distance) / 2; + const float linear_distance = (left_distance + right_distance) / 2; this->state.velocity.linear = this->linear_filter.update(linear_distance / elapsed_time); this->state.velocity.angular = this->imu.get_angular_velocity(proxy::Imu::Axis::Z); - float angular_distance = this->state.velocity.angular * elapsed_time; + const float angular_distance = this->state.velocity.angular * elapsed_time; - float half_angle = angular_distance / 2; - float linear_diagonal = + const float half_angle = angular_distance / 2; + const float linear_diagonal = angular_distance < 0.05F ? linear_distance : std::abs(std::sin(half_angle) * linear_distance / half_angle); this->state.pose.position.x += linear_diagonal * std::cos(this->state.pose.orientation + half_angle); diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 52bf837..5190c58 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -46,7 +46,7 @@ Point Point::rotate(Direction angle) { } Point Point::move_towards(const Point& other, float distance) const { - float angle = this->angle_between(other); + const float angle = this->angle_between(other); return {this->x + distance * std::cos(angle), this->y + distance * std::sin(angle)}; } @@ -71,7 +71,7 @@ GridPose Pose::to_grid(float cell_size) const { }; Point Pose::to_cell(float cell_size) const { - Point cell_position = this->position % cell_size; + const Point cell_position = this->position % cell_size; float cell_advance = 0.0F; float cell_alignment = 0.0F; diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp index 3d4da1c..1173ed2 100644 --- a/micras_proxy/src/button.cpp +++ b/micras_proxy/src/button.cpp @@ -42,7 +42,7 @@ bool Button::get_raw_reading() const { } bool Button::update_state() { - bool raw_reading = this->get_raw_reading(); + const bool raw_reading = this->get_raw_reading(); if ((raw_reading != this->current_state) and not this->is_debouncing) { this->is_debouncing = true; diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp index ec127f5..ce36b51 100644 --- a/micras_proxy/src/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -31,7 +31,7 @@ void Buzzer::wait(uint32_t interval) { this->update(); } - hal::Timer wait_timer; + const hal::Timer wait_timer; while (wait_timer.elapsed_time_ms() < interval) { this->update(); diff --git a/micras_proxy/src/motor.cpp b/micras_proxy/src/motor.cpp index ba1b7b5..97b3afb 100644 --- a/micras_proxy/src/motor.cpp +++ b/micras_proxy/src/motor.cpp @@ -17,7 +17,7 @@ Motor::Motor(const Config& config) : } void Motor::set_command(float command) { - bool is_positive = (command >= 0.0F); + const bool is_positive = (command >= 0.0F); command = std::abs(command); if (command <= max_stopped_command) { diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index a00e178..0660c31 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -47,8 +47,8 @@ float RotarySensor::get_position() const { } uint16_t RotarySensor::read_register(uint16_t address) { - CommandFrame command_frame = {{.crc = 0, .address = address, .rw = 1, .do_not_care = 0}}; - DataFrame data_frame{}; + CommandFrame command_frame = {{.crc = 0, .address = address, .rw = 1, .do_not_care = 0}}; + const DataFrame data_frame{}; command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2) ^ 0xFF; diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 4c9232c..b4d2445 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -16,9 +16,9 @@ Storage::Storage(const Config& config) : start_page{config.start_page}, number_o return; } - uint16_t total_size = header >> 32; - uint16_t num_primitives = header >> 16; - uint16_t num_serializables = header; + const uint16_t total_size = header >> 32; + const uint16_t num_primitives = header >> 16; + const uint16_t num_serializables = header; this->buffer.resize(8L * total_size); hal::Flash::read(this->start_page, 1, std::bit_cast(this->buffer.data()), total_size); @@ -80,7 +80,7 @@ void Storage::save() { this->buffer.insert(this->buffer.begin(), serialized_primitives.begin(), serialized_primitives.end()); this->buffer.insert(this->buffer.end(), (8 - (this->buffer.size() % 8)) % 8, 0); - uint16_t total_size = this->buffer.size() / 8; + const uint16_t total_size = this->buffer.size() / 8; this->buffer.emplace_back(this->serializables.size()); this->buffer.emplace_back(this->serializables.size() >> 8); @@ -120,9 +120,9 @@ std::unordered_map Storage::deserialize_var_map(std::vector Date: Fri, 4 Apr 2025 19:17:25 -0300 Subject: [PATCH 276/561] =?UTF-8?q?=F0=9F=9A=A8=20Change=20nav=20classes?= =?UTF-8?q?=20reference=20members=20to=20shared=5Fptr?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 6 ++- micras_nav/include/micras/nav/go_to_point.hpp | 3 +- micras_nav/include/micras/nav/mapping.hpp | 5 +- micras_nav/include/micras/nav/odometry.hpp | 12 +++-- micras_nav/src/follow_wall.cpp | 10 ++-- micras_nav/src/go_to_point.cpp | 3 +- micras_nav/src/mapping.cpp | 48 ++++++++++--------- micras_nav/src/odometry.cpp | 19 ++++---- 8 files changed, 58 insertions(+), 48 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index bf490dd..be7ca9b 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -5,6 +5,8 @@ #ifndef MICRAS_NAV_FOLLOW_WALL #define MICRAS_NAV_FOLLOW_WALL +#include + #include "micras/core/butterworth_filter.hpp" #include "micras/core/pid_controller.hpp" #include "micras/proxy/wall_sensors.hpp" @@ -31,7 +33,7 @@ class FollowWall { * @param wall_sensors The wall sensors of the robot. * @param config The configuration for the FollowWall class. */ - FollowWall(const proxy::TWallSensors<4>& wall_sensors, const Config& config); + FollowWall(const std::shared_ptr>& wall_sensors, const Config& config); /** * @brief Update the PID controller and return the response. @@ -70,7 +72,7 @@ class FollowWall { /** * @brief Wall sensors of the robot. */ - const proxy::TWallSensors<4>& wall_sensors; + std::shared_ptr> wall_sensors; /** * @brief PID controller for the wall following. diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp index abae30f..5bf2240 100644 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ b/micras_nav/include/micras/nav/go_to_point.hpp @@ -37,7 +37,8 @@ class GoToPoint { * @param follow_wall_config The configuration for the FollowWall class. */ GoToPoint( - const proxy::TWallSensors<4>& wall_sensors, const Config& config, const FollowWall::Config& follow_wall_config + const std::shared_ptr>& wall_sensors, const Config& config, + const FollowWall::Config& follow_wall_config ); /** diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp index 16f64cf..9416989 100644 --- a/micras_nav/include/micras/nav/mapping.hpp +++ b/micras_nav/include/micras/nav/mapping.hpp @@ -7,6 +7,7 @@ #include #include +#include #include "micras/nav/maze.hpp" #include "micras/nav/state.hpp" @@ -95,7 +96,7 @@ class TMapping : public proxy::ISerializable { * @param wall_sensors The wall sensors. * @param config The configuration for the mapping. */ - TMapping(const proxy::TWallSensors<4>& wall_sensors, Config config); + TMapping(const std::shared_ptr>& wall_sensors, Config config); /** * @brief Update the mapping of the maze using the current pose and wall sensors. @@ -200,7 +201,7 @@ class TMapping : public proxy::ISerializable { /** * @brief Wall sensors of the robot. */ - const proxy::TWallSensors<4>& wall_sensors; + std::shared_ptr> wall_sensors; /** * @brief The maze information the robot has. diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 266d7b1..7846265 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -6,6 +6,7 @@ #define MICRAS_NAV_ODOMETRY_HPP #include +#include #include "micras/core/butterworth_filter.hpp" #include "micras/hal/timer.hpp" @@ -37,8 +38,9 @@ class Odometry { * @param config Configuration for the odometry. */ Odometry( - const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - const proxy::Imu& imu, Config config + const std::shared_ptr& left_rotary_sensor, + const std::shared_ptr& right_rotary_sensor, const std::shared_ptr& imu, + Config config ); /** @@ -71,17 +73,17 @@ class Odometry { /** * @brief Left rotary sensor. */ - const proxy::RotarySensor& left_rotary_sensor; + std::shared_ptr left_rotary_sensor; /** * @brief Right rotary sensor. */ - const proxy::RotarySensor& right_rotary_sensor; + std::shared_ptr right_rotary_sensor; /** * @brief IMU sensor. */ - const proxy::Imu& imu; + std::shared_ptr imu; /** * @brief Wheel radius. diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index b209143..d306a10 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -5,7 +5,7 @@ #include "micras/nav/follow_wall.hpp" namespace micras::nav { -FollowWall::FollowWall(const proxy::TWallSensors<4>& wall_sensors, const Config& config) : +FollowWall::FollowWall(const std::shared_ptr>& wall_sensors, const Config& config) : wall_sensors{wall_sensors}, pid{config.pid}, filter{config.cutoff_frequency}, @@ -47,11 +47,11 @@ float FollowWall::action(core::FollowWallType follow_wall_type, float elapsed_ti } float FollowWall::get_left_value() const { - return this->wall_sensors.get_reading(1) - this->base_left_reading; + return this->wall_sensors->get_reading(1) - this->base_left_reading; } float FollowWall::get_right_value() const { - return this->wall_sensors.get_reading(2) - this->base_right_reading; + return this->wall_sensors->get_reading(2) - this->base_right_reading; } void FollowWall::reset() { @@ -59,7 +59,7 @@ void FollowWall::reset() { } void FollowWall::reset_base_readings() { - this->base_left_reading = this->wall_sensors.get_reading(1); - this->base_right_reading = this->wall_sensors.get_reading(2); + this->base_left_reading = this->wall_sensors->get_reading(1); + this->base_right_reading = this->wall_sensors->get_reading(2); } } // namespace micras::nav diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp index 5dfda66..ecac99f 100644 --- a/micras_nav/src/go_to_point.cpp +++ b/micras_nav/src/go_to_point.cpp @@ -9,7 +9,8 @@ namespace micras::nav { GoToPoint::GoToPoint( - const proxy::TWallSensors<4>& wall_sensors, const Config& config, const FollowWall::Config& follow_wall_config + const std::shared_ptr>& wall_sensors, const Config& config, + const FollowWall::Config& follow_wall_config ) : stop_pid(config.stop_pid), angular_pid(config.angular_pid), diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 7193b30..7d57a84 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -11,7 +11,9 @@ namespace micras::nav { template -TMapping::TMapping(const proxy::TWallSensors<4>& wall_sensors, TMapping::Config config) : +TMapping::TMapping( + const std::shared_ptr>& wall_sensors, TMapping::Config config +) : wall_sensors{wall_sensors}, maze{config.start, config.goal}, wall_thickness{config.wall_thickness}, @@ -43,19 +45,19 @@ void TMapping::update(const Pose& pose) { nav::Point cell_position = pose.to_cell(cell_size); if (cell_position.y < this->front_sensors_region_division) { - if (this->wall_sensors.get_observation(Sensor::FRONT_LEFT) == core::Observation::WALL and - this->wall_sensors.get_observation(Sensor::FRONT_RIGHT) == core::Observation::WALL) { + if (this->wall_sensors->get_observation(Sensor::FRONT_LEFT) == core::Observation::WALL and + this->wall_sensors->get_observation(Sensor::FRONT_RIGHT) == core::Observation::WALL) { information.front = core::Observation::WALL; - } else if (this->wall_sensors.get_observation(Sensor::FRONT_LEFT) == core::Observation::FREE_SPACE and - this->wall_sensors.get_observation(Sensor::FRONT_RIGHT) == core::Observation::FREE_SPACE) { + } else if (this->wall_sensors->get_observation(Sensor::FRONT_LEFT) == core::Observation::FREE_SPACE and + this->wall_sensors->get_observation(Sensor::FRONT_RIGHT) == core::Observation::FREE_SPACE) { information.front = core::Observation::FREE_SPACE; } } if (information.front != core::Observation::WALL and cell_position.y > this->side_sensors_region_division + 2 * this->wall_thickness) { - information.front_left = this->wall_sensors.get_observation(Sensor::LEFT); - information.front_right = this->wall_sensors.get_observation(Sensor::RIGHT); + information.front_left = this->wall_sensors->get_observation(Sensor::LEFT); + information.front_right = this->wall_sensors->get_observation(Sensor::RIGHT); } this->maze.update(pose.to_grid(this->cell_size), information); @@ -169,26 +171,26 @@ Pose TMapping::correct_pose(const Pose& pose, core::FollowWallTyp template void TMapping::calibrate_front() { - this->front_distance_reading[0] = this->wall_sensors.get_reading(0); - front_orientation_reading[0] = this->wall_sensors.get_reading(1); - front_orientation_reading[1] = this->wall_sensors.get_reading(2); - this->front_distance_reading[1] = this->wall_sensors.get_reading(3); + this->front_distance_reading[0] = this->wall_sensors->get_reading(0); + front_orientation_reading[0] = this->wall_sensors->get_reading(1); + front_orientation_reading[1] = this->wall_sensors->get_reading(2); + this->front_distance_reading[1] = this->wall_sensors->get_reading(3); } template void TMapping::calibrate_side() { - this->side_distance_reading[0] = this->wall_sensors.get_reading(1); - this->side_distance_reading[1] = this->wall_sensors.get_reading(2); + this->side_distance_reading[0] = this->wall_sensors->get_reading(1); + this->side_distance_reading[1] = this->wall_sensors->get_reading(2); } template bool TMapping::is_distance_front_aligned() const { return core::is_near( - this->wall_sensors.get_reading(0), this->front_distance_reading[0], + this->wall_sensors->get_reading(0), this->front_distance_reading[0], this->front_distance_alignment_tolerance ) and core::is_near( - this->wall_sensors.get_reading(3), this->front_distance_reading[1], + this->wall_sensors->get_reading(3), this->front_distance_reading[1], this->front_distance_alignment_tolerance ); } @@ -196,19 +198,19 @@ bool TMapping::is_distance_front_aligned() const { template bool TMapping::is_orientation_front_aligned() const { return std::abs( - (this->wall_sensors.get_reading(1) - this->front_orientation_reading[0]) + - (this->wall_sensors.get_reading(2) - this->front_orientation_reading[1]) + (this->wall_sensors->get_reading(1) - this->front_orientation_reading[0]) + + (this->wall_sensors->get_reading(2) - this->front_orientation_reading[1]) ) <= this->front_orientation_alignment_tolerance; } template bool TMapping::is_distance_side_aligned() const { return core::is_near( - this->wall_sensors.get_reading(1), this->side_distance_reading[0], + this->wall_sensors->get_reading(1), this->side_distance_reading[0], this->side_distance_alignment_tolerance ) and core::is_near( - this->wall_sensors.get_reading(2), this->side_distance_reading[1], + this->wall_sensors->get_reading(2), this->side_distance_reading[1], this->side_distance_alignment_tolerance ); } @@ -216,8 +218,8 @@ bool TMapping::is_distance_side_aligned() const { template bool TMapping::is_orientation_side_aligned() const { return std::abs( - (this->wall_sensors.get_reading(1) - this->side_distance_reading[0]) + - (this->wall_sensors.get_reading(2) - this->side_distance_reading[1]) + (this->wall_sensors->get_reading(1) - this->side_distance_reading[0]) + + (this->wall_sensors->get_reading(2) - this->side_distance_reading[1]) ) <= this->side_orientation_alignment_tolerance; } @@ -235,8 +237,8 @@ core::FollowWallType TMapping::get_follow_wall_type(const Pose& p return follow_wall_type; } - bool can_follow_left = this->wall_sensors.get_observation(Sensor::LEFT) == core::Observation::WALL; - bool can_follow_right = this->wall_sensors.get_observation(Sensor::RIGHT) == core::Observation::WALL; + bool can_follow_left = this->wall_sensors->get_observation(Sensor::LEFT) == core::Observation::WALL; + bool can_follow_right = this->wall_sensors->get_observation(Sensor::RIGHT) == core::Observation::WALL; if (can_follow_left and can_follow_right) { return core::FollowWallType::PARALLEL; diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 03d62d0..673f952 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -10,21 +10,22 @@ namespace micras::nav { Odometry::Odometry( - const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - const proxy::Imu& imu, Config config + const std::shared_ptr& left_rotary_sensor, + const std::shared_ptr& right_rotary_sensor, const std::shared_ptr& imu, + Config config ) : left_rotary_sensor{left_rotary_sensor}, right_rotary_sensor{right_rotary_sensor}, imu{imu}, wheel_radius{config.wheel_radius}, - left_last_position{left_rotary_sensor.get_position()}, - right_last_position{right_rotary_sensor.get_position()}, + left_last_position{left_rotary_sensor->get_position()}, + right_last_position{right_rotary_sensor->get_position()}, linear_filter{config.linear_cutoff_frequency}, state{config.initial_pose, {0.0F, 0.0F}} { } void Odometry::update(float elapsed_time) { - const float left_position = this->left_rotary_sensor.get_position(); - const float right_position = this->right_rotary_sensor.get_position(); + const float left_position = this->left_rotary_sensor->get_position(); + const float right_position = this->right_rotary_sensor->get_position(); const float left_distance = this->wheel_radius * (left_position - this->left_last_position); const float right_distance = this->wheel_radius * (right_position - this->right_last_position); @@ -35,7 +36,7 @@ void Odometry::update(float elapsed_time) { const float linear_distance = (left_distance + right_distance) / 2; this->state.velocity.linear = this->linear_filter.update(linear_distance / elapsed_time); - this->state.velocity.angular = this->imu.get_angular_velocity(proxy::Imu::Axis::Z); + this->state.velocity.angular = this->imu->get_angular_velocity(proxy::Imu::Axis::Z); const float angular_distance = this->state.velocity.angular * elapsed_time; @@ -50,8 +51,8 @@ void Odometry::update(float elapsed_time) { } void Odometry::reset() { - this->left_last_position = this->left_rotary_sensor.get_position(); - this->right_last_position = this->right_rotary_sensor.get_position(); + this->left_last_position = this->left_rotary_sensor->get_position(); + this->right_last_position = this->right_rotary_sensor->get_position(); } const nav::State& Odometry::get_state() const { From 6a1f50b4f9f9d1aba9c6484cb02da75e4f8aa704 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 4 Apr 2025 23:10:48 -0300 Subject: [PATCH 277/561] =?UTF-8?q?=E2=9C=A8=20Add=20extra=20colors=20arra?= =?UTF-8?q?y=20to=20argb?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/argb.hpp | 12 ++++++ micras_proxy/src/argb.cpp | 44 +++++++++++----------- 2 files changed, 33 insertions(+), 23 deletions(-) diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index 79c6ead..ba31624 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -100,6 +100,13 @@ class TArgb { */ void turn_off(); + /** + * @brief Send the colors to the addressable RGB LED. + * + * This function is called automatically when the colors are set. + */ + void update(); + private: /** * @brief Encode a color into the data buffer. @@ -139,6 +146,11 @@ class TArgb { */ float brightness; + /** + * @brief Array to store the color of each LED. + */ + std::array colors{}; + /** * @brief Data buffer to send to the addressable RGB LED. */ diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index cc201ce..bad7e37 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -14,48 +14,33 @@ TArgb::TArgb(const Config& config) : low_bit{pwm.get_compare(config.low_duty_cycle)}, high_bit{pwm.get_compare(config.high_duty_cycle)}, brightness{config.max_brightness / 100.0F} { - this->set_color({0, 0, 0}); + this->turn_off(); } template void TArgb::set_color(const Color& color, uint8_t index) { - if (index >= num_of_leds or this->pwm.is_busy()) { - return; - } - - this->encode_color(color * this->brightness, index); - this->pwm.start_dma(this->buffer); + this->colors.at(index) = color; + this->update(); } template void TArgb::set_color(const Color& color) { - if (this->pwm.is_busy()) { - return; - } - for (uint8_t i = 0; i < num_of_leds; i++) { - this->encode_color(color * this->brightness, i); + this->colors.at(i) = color; } - this->pwm.start_dma(this->buffer); + this->update(); } template void TArgb::set_colors(const std::array& colors) { - if (this->pwm.is_busy()) { - return; - } - - for (uint8_t i = 0; i < num_of_leds; i++) { - this->encode_color(colors[i] * this->brightness, i); - } - - this->pwm.start_dma(this->buffer); + this->colors = colors; + this->update(); } template void TArgb::turn_off(uint8_t index) { - this->set_color(index, {0, 0, 0}); + this->set_color({0, 0, 0}, index); } template @@ -63,6 +48,19 @@ void TArgb::turn_off() { this->set_color({0, 0, 0}); } +template +void TArgb::update() { + if (this->pwm.is_busy()) { + return; + } + + for (uint8_t i = 0; i < num_of_leds; i++) { + this->encode_color(this->colors.at(i) * this->brightness, i); + } + + this->pwm.start_dma(this->buffer); +} + template void TArgb::encode_color(const Color& color, uint8_t index) { uint32_t data = (color.green << (2 * bits_per_color)) | (color.red << bits_per_color) | color.blue; From 5488c4c3202a4c1f08f20fd3631f1a14face8df3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 5 Apr 2025 01:31:04 -0300 Subject: [PATCH 278/561] =?UTF-8?q?=E2=8F=AA=20Revert=20old=20changes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/follow_wall.cpp | 1 - micras_proxy/src/rotary_sensor.cpp | 46 +++++++++++++++--------------- 2 files changed, 23 insertions(+), 24 deletions(-) diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index bc7299b..4775793 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -35,7 +35,6 @@ float FollowWall::action(core::FollowWallType follow_wall_type, float elapsed_ti } float response = this->pid.update(error, elapsed_time); - return response; } diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index 2a7b91b..a00e178 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -10,36 +10,36 @@ namespace micras::proxy { RotarySensor::RotarySensor(const Config& config) : spi{config.spi}, encoder{config.encoder}, crc{config.crc}, resolution{config.resolution} { - // CommandFrame command_frame{}; - // DataFrame data_frame{}; + CommandFrame command_frame{}; + DataFrame data_frame{}; - // command_frame.fields.address = Registers::disable_addr; - // data_frame.fields.data = config.registers.disable.raw; - // this->write_register(command_frame, data_frame); + command_frame.fields.address = Registers::disable_addr; + data_frame.fields.data = config.registers.disable.raw; + this->write_register(command_frame, data_frame); - // command_frame.fields.address = Registers::zposm_addr; - // data_frame.fields.data = config.registers.zposm.raw; - // this->write_register(command_frame, data_frame); + command_frame.fields.address = Registers::zposm_addr; + data_frame.fields.data = config.registers.zposm.raw; + this->write_register(command_frame, data_frame); - // command_frame.fields.address = Registers::zposl_addr; - // data_frame.fields.data = config.registers.zposl.raw; - // this->write_register(command_frame, data_frame); + command_frame.fields.address = Registers::zposl_addr; + data_frame.fields.data = config.registers.zposl.raw; + this->write_register(command_frame, data_frame); - // command_frame.fields.address = Registers::settings1_addr; - // data_frame.fields.data = config.registers.settings1.raw; - // this->write_register(command_frame, data_frame); + command_frame.fields.address = Registers::settings1_addr; + data_frame.fields.data = config.registers.settings1.raw; + this->write_register(command_frame, data_frame); - // command_frame.fields.address = Registers::settings2_addr; - // data_frame.fields.data = config.registers.settings2.raw; - // this->write_register(command_frame, data_frame); + command_frame.fields.address = Registers::settings2_addr; + data_frame.fields.data = config.registers.settings2.raw; + this->write_register(command_frame, data_frame); - // command_frame.fields.address = Registers::settings3_addr; - // data_frame.fields.data = config.registers.settings3.raw; - // this->write_register(command_frame, data_frame); + command_frame.fields.address = Registers::settings3_addr; + data_frame.fields.data = config.registers.settings3.raw; + this->write_register(command_frame, data_frame); - // command_frame.fields.address = Registers::ecc_addr; - // data_frame.fields.data = config.registers.ecc.raw; - // this->write_register(command_frame, data_frame); + command_frame.fields.address = Registers::ecc_addr; + data_frame.fields.data = config.registers.ecc.raw; + this->write_register(command_frame, data_frame); } float RotarySensor::get_position() const { From 25458832f6e20726291f3750c74d63aa4ed4315a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 6 Apr 2025 16:04:13 -0300 Subject: [PATCH 279/561] =?UTF-8?q?=F0=9F=9A=A8=20Add=20shared=20pointers?= =?UTF-8?q?=20to=20micras?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 5 +---- micras_core/src/butterworth_filter.cpp | 4 ++-- micras_nav/src/mapping.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 10 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 936865b..2c4c494 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -21,6 +21,7 @@ Checks: performance-*, readability-*, -readability-duplicate-include, + -readability-magic-numbers, WarningsAsErrors: '*' HeaderFileExtensions: - '' @@ -36,10 +37,6 @@ ImplementationFileExtensions: HeaderFilterRegex: '(include|config)/.*.hpp' FormatStyle: none CheckOptions: - readability-magic-numbers.IgnoredIntegerValues: '1;2;3;4;5;10;100;255;1000;65535;1000000' - readability-magic-numbers.IgnorePowersOf2IntegerValues: true - readability-magic-numbers.IgnoreUserDefinedLiterals: true - readability-magic-numbers.IgnoredFloatingPointValues: '1.0;2.0;5.0;10.0;50.0;100.0;1000.0;1000000.0' readability-identifier-length.MinimumVariableNameLength: 2 readability-identifier-length.IgnoredVariableNames: ^[nxyz]$ readability-identifier-length.MinimumParameterNameLength: 2 diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index e623d80..3fe4650 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -37,13 +37,13 @@ float ButterworthFilter::update(float x0) { float x_b_dot = 0; for (uint8_t i = 0; i < filter_order + 1; i++) { - x_b_dot += this->x_array[i] * this->b_array[i]; // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index) + x_b_dot += this->x_array.at(i) * this->b_array.at(i); } float y_a_dot = 0; for (uint8_t i = 0; i < filter_order; i++) { - y_a_dot += this->y_array[i] * this->a_array[i]; // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index) + y_a_dot += this->y_array.at(i) * this->a_array.at(i); } const float y0 = x_b_dot - y_a_dot; diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index bdfab82..17b5f63 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -171,10 +171,10 @@ Pose TMapping::correct_pose(const Pose& pose, core::FollowWallTyp template void TMapping::calibrate_front() { - this->front_distance_reading[0] = this->wall_sensors.get_reading(0); - this->front_orientation_reading[0] = this->wall_sensors.get_reading(1); - this->front_orientation_reading[1] = this->wall_sensors.get_reading(2); - this->front_distance_reading[1] = this->wall_sensors.get_reading(3); + this->front_distance_reading[0] = this->wall_sensors->get_reading(0); + this->front_orientation_reading[0] = this->wall_sensors->get_reading(1); + this->front_orientation_reading[1] = this->wall_sensors->get_reading(2); + this->front_distance_reading[1] = this->wall_sensors->get_reading(3); } template From 2552c536c9ce44943eaa98966e96b4d7c2a78fe5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 6 Apr 2025 16:42:03 -0300 Subject: [PATCH 280/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= =?UTF-8?q?=20on=20tests?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 1 + micras_nav/src/mapping.cpp | 1 + micras_proxy/src/storage.cpp | 2 ++ 3 files changed, 4 insertions(+) diff --git a/.clang-tidy b/.clang-tidy index 2c4c494..38e56e5 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -15,6 +15,7 @@ Checks: -llvm-header-guard, misc-*, -misc-include-cleaner, + -misc-use-anonymous-namespace, modernize-*, -modernize-use-nodiscard, -modernize-use-trailing-return-type, diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 17b5f63..6137dc7 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -275,6 +275,7 @@ void TMapping::deserialize(const uint8_t* buffer, uint16_t size) for (uint32_t i = 0; i < size; i += 3) { this->best_route.emplace_back( + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) Point::from_grid({buffer[i], buffer[i + 1]}, this->cell_size), static_cast(2 * buffer[i + 2]) ); } diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index b4d2445..b528471 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -54,6 +54,8 @@ void Storage::save() { const auto* aux = std::bit_cast(variable.ram_pointer); variable.buffer_address = buffer.size(); + + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) this->buffer.insert(this->buffer.end(), aux, aux + variable.size); it++; } From dd23beef98620ceea5ead38532909b2655d60831 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 8 Apr 2025 00:41:00 -0300 Subject: [PATCH 281/561] =?UTF-8?q?=E2=9C=A8=20Add=20stopwatch=20proxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/timer.hpp | 47 +---------- micras_hal/stm32/src/timer.cpp | 42 ---------- micras_nav/include/micras/nav/odometry.hpp | 1 - micras_proxy/include/micras/proxy/button.hpp | 10 +-- micras_proxy/include/micras/proxy/buzzer.hpp | 6 +- micras_proxy/include/micras/proxy/fan.hpp | 6 +- .../include/micras/proxy/stopwatch.hpp | 83 +++++++++++++++++++ micras_proxy/src/button.cpp | 10 +-- micras_proxy/src/buzzer.cpp | 10 +-- micras_proxy/src/fan.cpp | 4 +- micras_proxy/src/imu.cpp | 4 +- micras_proxy/src/stopwatch.cpp | 51 ++++++++++++ micras_proxy/src/torque_sensors.cpp | 1 - 13 files changed, 160 insertions(+), 115 deletions(-) create mode 100644 micras_proxy/include/micras/proxy/stopwatch.hpp create mode 100644 micras_proxy/src/stopwatch.cpp diff --git a/micras_hal/include/micras/hal/timer.hpp b/micras_hal/include/micras/hal/timer.hpp index f166ffb..0ad0ada 100644 --- a/micras_hal/include/micras/hal/timer.hpp +++ b/micras_hal/include/micras/hal/timer.hpp @@ -25,7 +25,7 @@ class Timer { /** * @brief Construct a new Timer object. */ - Timer(); + Timer() = default; /** * @brief Construct a new Timer object. @@ -34,45 +34,6 @@ class Timer { */ explicit Timer(const Config& config); - /** - * @brief Reset the milliseconds timer counter. - */ - void reset_ms(); - - /** - * @brief Reset the microseconds timer counter. - */ - void reset_us(); - - /** - * @brief Get the time elapsed since the last reset. - * - * @return Time elapsed in miliseconds. - */ - uint32_t elapsed_time_ms() const; - - /** - * @brief Get the time elapsed since the last reset. - * - * @return Time elapsed in microseconds. - */ - uint32_t elapsed_time_us() const; - - /** - * @brief Sleep for a given amount of time. - * - * @param time Time to sleep in milliseconds. - */ - static void sleep_ms(uint32_t time); - - /** - * @brief Sleep for a given amount of time. - * - * @param time Time to sleep in microseconds. - */ - void sleep_us(uint32_t time) const; - -private: /** * @brief Get the current timer counter. * @@ -87,16 +48,12 @@ class Timer { */ uint32_t get_counter_us() const; +private: /** * @brief Timer handle. */ TIM_HandleTypeDef* handle{}; - /** - * @brief Timer counter. - */ - uint32_t counter{}; - /** * @brief Flag to enable microseconds. */ diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index 43e7e2c..f5e532b 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -2,15 +2,9 @@ * @file */ -#include - #include "micras/hal/timer.hpp" namespace micras::hal { -Timer::Timer() { - this->reset_ms(); -} - Timer::Timer(const Config& config) : handle{config.handle} { config.init_function(); @@ -21,42 +15,6 @@ Timer::Timer(const Config& config) : handle{config.handle} { this->enable_microseconds = true; HAL_TIM_Base_Start(this->handle); } - - this->reset_us(); -} - -void Timer::reset_ms() { - this->counter = get_counter_ms(); -} - -void Timer::reset_us() { - this->counter = this->get_counter_us(); -} - -uint32_t Timer::elapsed_time_ms() const { - return get_counter_ms() - this->counter; -} - -uint32_t Timer::elapsed_time_us() const { - const uint32_t counter = this->get_counter_us(); - - if (counter < this->counter) { - return std::numeric_limits::max() - this->counter + counter; - } - - return counter - this->counter; -} - -void Timer::sleep_ms(uint32_t time) { - const uint32_t start = HAL_GetTick(); - - while (HAL_GetTick() - start < time) { } -} - -void Timer::sleep_us(uint32_t time) const { - const uint32_t start = this->get_counter_us(); - - while (this->get_counter_us() - start < time) { } } uint32_t Timer::get_counter_ms() { diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 7846265..7346705 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -9,7 +9,6 @@ #include #include "micras/core/butterworth_filter.hpp" -#include "micras/hal/timer.hpp" #include "micras/nav/state.hpp" #include "micras/proxy/imu.hpp" #include "micras/proxy/rotary_sensor.hpp" diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index 709d371..8a0578d 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -8,7 +8,7 @@ #include #include "micras/hal/gpio.hpp" -#include "micras/hal/timer.hpp" +#include "micras/proxy/stopwatch.hpp" namespace micras::proxy { /** @@ -115,14 +115,14 @@ class Button { PullResistor pull_resistor; /** - * @brief Timer to check if button is debouncing. + * @brief Stopwatch to check if button is debouncing. */ - hal::Timer debounce_timer; + proxy::Stopwatch debounce_stopwatch; /** - * @brief Timer to determine type of button press. + * @brief Stopwatch to determine type of button press. */ - hal::Timer status_timer; + proxy::Stopwatch status_stopwatch; /** * @brief Flag to know when button is debouncing. diff --git a/micras_proxy/include/micras/proxy/buzzer.hpp b/micras_proxy/include/micras/proxy/buzzer.hpp index a2f9d01..8aa18ca 100644 --- a/micras_proxy/include/micras/proxy/buzzer.hpp +++ b/micras_proxy/include/micras/proxy/buzzer.hpp @@ -8,7 +8,7 @@ #include #include "micras/hal/pwm.hpp" -#include "micras/hal/timer.hpp" +#include "micras/proxy/stopwatch.hpp" namespace micras::proxy { /** @@ -62,9 +62,9 @@ class Buzzer { hal::Pwm pwm; /** - * @brief Timer to play the sound. + * @brief Stopwatch to play the sound. */ - hal::Timer timer; + proxy::Stopwatch stopwatch; /** * @brief Flag to check if the buzzer is playing. diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index 3a784a2..8879831 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -9,7 +9,7 @@ #include "micras/hal/gpio.hpp" #include "micras/hal/pwm.hpp" -#include "micras/hal/timer.hpp" +#include "micras/proxy/stopwatch.hpp" namespace micras::proxy { /** @@ -108,9 +108,9 @@ class Fan { float max_acceleration; /** - * @brief Timer for limiting the acceleration of the fan. + * @brief Stopwatch for limiting the acceleration of the fan. */ - hal::Timer acceleration_timer; + proxy::Stopwatch acceleration_stopwatch; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/stopwatch.hpp b/micras_proxy/include/micras/proxy/stopwatch.hpp new file mode 100644 index 0000000..5b01a70 --- /dev/null +++ b/micras_proxy/include/micras/proxy/stopwatch.hpp @@ -0,0 +1,83 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_STOPWATCH_HPP +#define MICRAS_PROXY_STOPWATCH_HPP + +#include "micras/hal/timer.hpp" + +namespace micras::proxy { +/** + * @brief Class to measure the time elapsed between two events. + */ +class Stopwatch { +public: + /** + * @brief Stopwatch configuration struct. + */ + struct Config { + hal::Timer::Config timer; + }; + + /** + * @brief Construct a new Stopwatch object. + */ + Stopwatch(); + + /** + * @brief Construct a new Stopwatch object. + * + * @param config Configuration for the timer. + */ + explicit Stopwatch(const Config& config); + + /** + * @brief Reset the milliseconds timer counter. + */ + void reset_ms(); + + /** + * @brief Reset the microseconds timer counter. + */ + void reset_us(); + + /** + * @brief Get the time elapsed since the last reset. + * + * @return Time elapsed in miliseconds. + */ + uint32_t elapsed_time_ms() const; + + /** + * @brief Get the time elapsed since the last reset. + * + * @return Time elapsed in microseconds. + */ + uint32_t elapsed_time_us() const; + + /** + * @brief Sleep for a given amount of time. + * + * @param time Time to sleep in milliseconds. + */ + static void sleep_ms(uint32_t time); + + /** + * @brief Sleep for a given amount of time. + * + * @param time Time to sleep in microseconds. + */ + void sleep_us(uint32_t time) const; + +private: + hal::Timer timer; + + /** + * @brief Stopwatch counter. + */ + uint32_t counter{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_STOPWATCH_HPP diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp index 1173ed2..1e83642 100644 --- a/micras_proxy/src/button.cpp +++ b/micras_proxy/src/button.cpp @@ -21,13 +21,13 @@ Button::Status Button::get_status() { this->current_state = this->update_state(); if (this->is_rising_edge()) { - this->status_timer.reset_ms(); + this->status_stopwatch.reset_ms(); } else if (this->is_falling_edge()) { - if (this->status_timer.elapsed_time_ms() > extra_long_press_delay) { + if (this->status_stopwatch.elapsed_time_ms() > extra_long_press_delay) { return EXTRA_LONG_PRESS; } - if (this->status_timer.elapsed_time_ms() > long_press_delay) { + if (this->status_stopwatch.elapsed_time_ms() > long_press_delay) { return LONG_PRESS; } @@ -46,8 +46,8 @@ bool Button::update_state() { if ((raw_reading != this->current_state) and not this->is_debouncing) { this->is_debouncing = true; - this->debounce_timer.reset_ms(); - } else if ((this->debounce_timer.elapsed_time_ms() < debounce_delay) and this->is_debouncing) { + this->debounce_stopwatch.reset_ms(); + } else if ((this->debounce_stopwatch.elapsed_time_ms() < debounce_delay) and this->is_debouncing) { if (this->current_state == raw_reading) { this->is_debouncing = false; } diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp index ce36b51..0a2f035 100644 --- a/micras_proxy/src/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -16,12 +16,12 @@ void Buzzer::play(uint32_t frequency, uint32_t duration) { this->duration = duration; if (duration > 0) { - this->timer.reset_ms(); + this->stopwatch.reset_ms(); } } void Buzzer::update() { - if (this->is_playing and this->duration > 0 and this->timer.elapsed_time_ms() > this->duration) { + if (this->is_playing and this->duration > 0 and this->stopwatch.elapsed_time_ms() > this->duration) { this->stop(); } } @@ -31,11 +31,9 @@ void Buzzer::wait(uint32_t interval) { this->update(); } - const hal::Timer wait_timer; + this->stopwatch.reset_ms(); - while (wait_timer.elapsed_time_ms() < interval) { - this->update(); - } + while (this->stopwatch.elapsed_time_ms() < interval) { } } void Buzzer::stop() { diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp index 34bc3df..1dfe504 100644 --- a/micras_proxy/src/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -30,10 +30,10 @@ void Fan::set_speed(float speed) { float Fan::update() { this->current_speed = core::move_towards( - this->current_speed, this->target_speed, this->acceleration_timer.elapsed_time_ms() * this->max_acceleration + this->current_speed, this->target_speed, this->acceleration_stopwatch.elapsed_time_ms() * this->max_acceleration ); - this->acceleration_timer.reset_ms(); + this->acceleration_stopwatch.reset_ms(); if (this->current_speed > 0.0F) { this->set_direction(RotationDirection::FORWARD); diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index d9e5a0e..0733cd7 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -5,8 +5,8 @@ #include #include -#include "micras/hal/timer.hpp" #include "micras/proxy/imu.hpp" +#include "micras/proxy/stopwatch.hpp" namespace micras::proxy { Imu::Imu(const Config& config) : @@ -20,7 +20,7 @@ Imu::Imu(const Config& config) : this->dev_ctx.write_reg = platform_write; this->dev_ctx.handle = &this->spi; - hal::Timer::sleep_ms(100); + proxy::Stopwatch::sleep_ms(100); if (not this->check_whoami()) { return; diff --git a/micras_proxy/src/stopwatch.cpp b/micras_proxy/src/stopwatch.cpp new file mode 100644 index 0000000..f1c27ae --- /dev/null +++ b/micras_proxy/src/stopwatch.cpp @@ -0,0 +1,51 @@ +/** + * @file + */ + +#include + +#include "micras/proxy/stopwatch.hpp" + +namespace micras::proxy { +Stopwatch::Stopwatch() { + this->reset_ms(); +} + +Stopwatch::Stopwatch(const Config& config) : timer{config.timer} { + this->reset_us(); +} + +void Stopwatch::reset_ms() { + this->counter = hal::Timer::get_counter_ms(); +} + +void Stopwatch::reset_us() { + this->counter = this->timer.get_counter_us(); +} + +uint32_t Stopwatch::elapsed_time_ms() const { + return hal::Timer::get_counter_ms() - this->counter; +} + +uint32_t Stopwatch::elapsed_time_us() const { + const uint32_t counter = this->timer.get_counter_us(); + + if (counter < this->counter) { + return std::numeric_limits::max() - this->counter + counter; + } + + return counter - this->counter; +} + +void Stopwatch::sleep_ms(uint32_t time) { + const uint32_t start = HAL_GetTick(); + + while (HAL_GetTick() - start < time) { } +} + +void Stopwatch::sleep_us(uint32_t time) const { + const uint32_t start = this->timer.get_counter_us(); + + while (this->timer.get_counter_us() - start < time) { } +} +} // namespace micras::proxy diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index d7f86b2..d362f8a 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -6,7 +6,6 @@ #define MICRAS_PROXY_TORQUE_SENSORS_CPP #include "micras/core/utils.hpp" -#include "micras/hal/timer.hpp" #include "micras/proxy/torque_sensors.hpp" namespace micras::proxy { From 937ab623e77896889b576fc9a3588140eda73823 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 8 Apr 2025 10:00:33 -0300 Subject: [PATCH 282/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20unnecessary=20N?= =?UTF-8?q?OLINT=20from=20pid=20controller?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/src/pid_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_core/src/pid_controller.cpp b/micras_core/src/pid_controller.cpp index 20d7698..b2464a7 100644 --- a/micras_core/src/pid_controller.cpp +++ b/micras_core/src/pid_controller.cpp @@ -7,14 +7,14 @@ #include "micras/core/pid_controller.hpp" -// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables, misc-use-anonymous-namespace) +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) static volatile float test_error; static volatile float test_proportional; static volatile float test_integrative; static volatile float test_derivative; static volatile float test_response; -// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables, misc-use-anonymous-namespace) +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) namespace micras::core { PidController::PidController(Config config) : From 4b0387940fec2a7e6a2f5ee3b7082d86fbe6558d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 11 Apr 2025 18:51:52 -0300 Subject: [PATCH 283/561] =?UTF-8?q?=E2=9E=95=20Add=20missing=20utils=20inc?= =?UTF-8?q?lude=20in=20mapping.cpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/mapping.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp index 6137dc7..0f8f97d 100644 --- a/micras_nav/src/mapping.cpp +++ b/micras_nav/src/mapping.cpp @@ -7,6 +7,7 @@ #include +#include "micras/core/utils.hpp" #include "micras/nav/mapping.hpp" namespace micras::nav { From 8168806b7e9abe369517a1331bf19474bed10eee Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 00:17:00 -0300 Subject: [PATCH 284/561] =?UTF-8?q?=F0=9F=94=A5=20Delete=20old=20classes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 93 ----- micras_nav/include/micras/nav/go_to_point.hpp | 127 ------- .../include/micras/nav/look_at_point.hpp | 82 ----- micras_nav/include/micras/nav/mapping.hpp | 290 --------------- micras_nav/src/follow_wall.cpp | 57 --- micras_nav/src/go_to_point.cpp | 71 ---- micras_nav/src/look_at_point.cpp | 34 -- micras_nav/src/mapping.cpp | 337 ------------------ 8 files changed, 1091 deletions(-) delete mode 100644 micras_nav/include/micras/nav/follow_wall.hpp delete mode 100644 micras_nav/include/micras/nav/go_to_point.hpp delete mode 100644 micras_nav/include/micras/nav/look_at_point.hpp delete mode 100644 micras_nav/include/micras/nav/mapping.hpp delete mode 100644 micras_nav/src/follow_wall.cpp delete mode 100644 micras_nav/src/go_to_point.cpp delete mode 100644 micras_nav/src/look_at_point.cpp delete mode 100644 micras_nav/src/mapping.cpp diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp deleted file mode 100644 index 9381e39..0000000 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ /dev/null @@ -1,93 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_FOLLOW_WALL -#define MICRAS_NAV_FOLLOW_WALL - -#include - -#include "micras/core/butterworth_filter.hpp" -#include "micras/core/pid_controller.hpp" -#include "micras/proxy/wall_sensors.hpp" - -namespace micras::nav { -/** - * @brief Class to follow the side walls using a PID controller. - */ -class FollowWall { -public: - /** - * @brief Configuration struct for the FollowWall class. - */ - struct Config { - core::PidController::Config pid; - float base_left_reading{}; - float base_right_reading{}; - }; - - /** - * @brief Construct a new FollowWall object. - * - * @param wall_sensors The wall sensors of the robot. - * @param config The configuration for the FollowWall class. - */ - FollowWall(const std::shared_ptr>& wall_sensors, const Config& config); - - /** - * @brief Update the PID controller and return the response. - * - * @param follow_wall_type The type of wall following to perform. - * @param elapsed_time The time elapsed since the last update. - * @return The response of the PID controller. - */ - float action(core::FollowWallType follow_wall_type, float elapsed_time); - - /** - * @brief Reset the PID controller. - */ - void reset(); - - /** - * @brief Reset the base readings of the wall sensors. - */ - void reset_base_readings(); - -private: - /** - * @brief Get the reading from the left wall sensor. - * - * @return The reading from the left wall sensor. - */ - float get_left_value() const; - - /** - * @brief Get the reading from the right wall sensor. - * - * @return The reading from the right wall sensor. - */ - float get_right_value() const; - - /** - * @brief Wall sensors of the robot. - */ - std::shared_ptr> wall_sensors; - - /** - * @brief PID controller for the wall following. - */ - core::PidController pid; - - /** - * @brief Base reading of the left wall sensor when aligned with the wall. - */ - float base_left_reading; - - /** - * @brief Base reading of the right wall sensor when aligned with the wall. - */ - float base_right_reading; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_FOLLOW_WALL diff --git a/micras_nav/include/micras/nav/go_to_point.hpp b/micras_nav/include/micras/nav/go_to_point.hpp deleted file mode 100644 index 5bf2240..0000000 --- a/micras_nav/include/micras/nav/go_to_point.hpp +++ /dev/null @@ -1,127 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_GO_TO_POINT_HPP -#define MICRAS_NAV_GO_TO_POINT_HPP - -#include "micras/core/pid_controller.hpp" -#include "micras/nav/follow_wall.hpp" -#include "micras/nav/state.hpp" - -namespace micras::nav { -/** - * @brief Class to calculate a command to go to a point. - */ -class GoToPoint { -public: - /** - * @brief Configuration struct for the GoToPoint class. - */ - struct Config { - core::PidController::Config stop_pid; - core::PidController::Config angular_pid; - float cell_size{}; - float min_linear_command{}; - float max_linear_command{}; - float deceleration_factor{}; - float distance_tolerance{}; - float velocity_tolerance{}; - }; - - /** - * @brief Construct a new GoToPoint object. - * - * @param wall_sensors The wall sensors of the robot. - * @param config The configuration for the GoToPoint class. - * @param follow_wall_config The configuration for the FollowWall class. - */ - GoToPoint( - const std::shared_ptr>& wall_sensors, const Config& config, - const FollowWall::Config& follow_wall_config - ); - - /** - * @brief Calculate the command to go to a point. - * - * @param state The current state of the robot. - * @param goal The goal point. - * @param follow_wall_type The type of wall following to perform. - * @param elapsed_time The time elapsed since the last update. - * @param stop Whether to stop at the goal. - * @return The command to go to the point. - */ - Twist action( - const State& state, const Point& goal, core::FollowWallType follow_wall_type, float elapsed_time, - bool stop = true - ); - - /** - * @brief Check if the robot has reached the goal. - * - * @param state The current state of the robot. - * @param goal The goal point. - * @param stop Whether to stop at the goal. - * @return True if the robot has reached the goal, false otherwise. - */ - bool finished(const State& state, const Point& goal, bool stop = true) const; - - /** - * @brief Reset the PID controllers. - */ - void reset(); - - /** - * @brief Calibrate the wall sensors. - */ - void calibrate(); - -private: - /** - * @brief PID controller for stopping at the goal. - */ - core::PidController stop_pid; - - /** - * @brief PID controller for the orientation. - */ - core::PidController angular_pid; - - /** - * @brief FollowWall controller. - */ - FollowWall follow_wall; - - /** - * @brief Size of the grid cells. - */ - float cell_size; - - /** - * @brief Minimum linear command. - */ - float min_linear_command; - - /** - * @brief Maximum linear command. - */ - float max_linear_command; - - /** - * @brief Deceleration factor for the linear command transition. - */ - float deceleration_factor; - - /** - * @brief Linear tolerance for the goal point. - */ - float distance_tolerance; - - /** - * @brief Velocity tolerance at the goal point. - */ - float velocity_tolerance; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_GO_TO_POINT_HPP diff --git a/micras_nav/include/micras/nav/look_at_point.hpp b/micras_nav/include/micras/nav/look_at_point.hpp deleted file mode 100644 index 4bfaacd..0000000 --- a/micras_nav/include/micras/nav/look_at_point.hpp +++ /dev/null @@ -1,82 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_LOOK_AT_POINT_HPP -#define MICRAS_NAV_LOOK_AT_POINT_HPP - -#include "micras/core/pid_controller.hpp" -#include "micras/nav/state.hpp" - -namespace micras::nav { -/** - * @brief Class to calculate a command to look at a point. - */ -class LookAtPoint { -public: - /** - * @brief Configuration struct for the LookAtPoint class. - */ - struct Config { - core::PidController::Config linear_pid; - core::PidController::Config angular_pid; - float distance_tolerance{}; - float velocity_tolerance{}; - }; - - /** - * @brief Construct a new LookAtPoint object. - * - * @param config The configuration for the LookAtPoint class. - */ - explicit LookAtPoint(Config config); - - /** - * @brief Calculate the command to look at a point. - * - * @param state The current state of the robot. - * @param goal The goal point. - * @param elapsed_time The time elapsed since the last update. - * @return The command to look at the point. - */ - Twist action(const State& state, const Point& goal, float elapsed_time); - - /** - * @brief Reset the PID controllers. - */ - void reset(); - - /** - * @brief Check if the robot has reached the goal. - * - * @param state The current state of the robot. - * @param goal The goal point. - * @param stop Whether to stop the robot when it reaches the goal. - * @return True if the robot has reached the goal, false otherwise. - */ - bool finished(const State& state, const Point& goal, bool stop = true) const; - -private: - /** - * @brief PID controller for the linear velocity. - */ - core::PidController linear_pid; - - /** - * @brief PID controller for the angular velocity. - */ - core::PidController angular_pid; - - /** - * @brief Angular tolerance for the goal. - */ - float distance_tolerance; - - /** - * @brief Velocity tolerance for the goal. - */ - float velocity_tolerance; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_LOOK_AT_POINT_HPP diff --git a/micras_nav/include/micras/nav/mapping.hpp b/micras_nav/include/micras/nav/mapping.hpp deleted file mode 100644 index 9416989..0000000 --- a/micras_nav/include/micras/nav/mapping.hpp +++ /dev/null @@ -1,290 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_MAPPING_HPP -#define MICRAS_NAV_MAPPING_HPP - -#include -#include -#include - -#include "micras/nav/maze.hpp" -#include "micras/nav/state.hpp" -#include "micras/proxy/serializable.hpp" -#include "micras/proxy/wall_sensors.hpp" - -namespace micras::nav { -/** - * @brief Class for mapping the maze. - * - * @tparam width The width of the maze. - * @tparam height The height of the maze. - */ -template -class TMapping : public proxy::ISerializable { -public: - /** - * @brief Configuration struct for the Mapping class. - */ - struct Config { - float wall_thickness{}; - float cell_size{}; - Pose front_sensor_pose{}; - Pose side_sensor_pose{}; - float front_distance_alignment_tolerance{}; - float side_distance_alignment_tolerance{}; - float front_orientation_alignment_tolerance{}; - float side_orientation_alignment_tolerance{}; - std::array front_distance_reading{}; - std::array front_orientation_reading{}; - std::array side_distance_reading{}; - - GridPose start{}; - std::unordered_set goal{ - {{width / 2, height / 2}, - {(width - 1) / 2, height / 2}, - {width / 2, (height - 1) / 2}, - {(width - 1) / 2, (height - 1) / 2}} - }; - }; - - /** - * @brief Type to identify the wall sensors. - */ - enum Sensor : uint8_t { - FRONT_LEFT, - LEFT, - RIGHT, - FRONT_RIGHT - }; - - /** - * @brief Type to store an action to be taken. - */ - struct Action { - /** - * @brief Possible types of actions. - */ - enum Type : uint8_t { - LOOK_AT, // Rotate the robot to look at a specific point. - GO_TO, // Move and turn the robot to go to a specific point. - ALIGN_BACK, // Align the back of the robot with a wall. - FINISHED, // The robot has reached the goal. - ERROR, // An error occurred. - }; - - /** - * @brief Type of the action. - */ - Type type; - - /** - * @brief The next goal point. - */ - Point point; - - /** - * @brief The orientation of the goal point. - */ - Direction direction; - }; - - /** - * @brief Construct a new Mapping object. - * - * @param wall_sensors The wall sensors. - * @param config The configuration for the mapping. - */ - TMapping(const std::shared_ptr>& wall_sensors, Config config); - - /** - * @brief Update the mapping of the maze using the current pose and wall sensors. - * - * @param pose The current pose of the robot. - */ - void update(const Pose& pose); - - /** - * @brief Get the action to be taken based on the current pose. - * - * @param pose The current pose of the robot. - * @param objective The curent objective of the robot. - * @return The action to be taken. - */ - Action get_action(const Pose& pose, core::Objective objective); - - /** - * @brief Fix the pose based on the mapping information. - * - * @param pose The current pose of the robot. - * @param follow_wall_type The type of wall following being performed. - * @return The fixed pose. - */ - Pose correct_pose(const Pose& pose, core::FollowWallType follow_wall_type) const; - - /** - * @brief Calibrate front alignment of the wall sensors. - */ - void calibrate_front(); - - /** - * @brief Calibrate side alignment of the wall sensors. - */ - void calibrate_side(); - - /** - * @brief Check if the robot is at a predefined distance from the wall at the front. - * - * @return True if the robot is aligned, false otherwise. - */ - bool is_distance_front_aligned() const; - - /** - * @brief Check if the robot orientation is aligned with walls at the front. - * - * @return True if the robot is aligned, false otherwise. - */ - bool is_orientation_front_aligned() const; - - /** - * @brief Check if the robot is at a predefined distance from the wallz at the sides. - * - * @return True if the robot is aligned, false otherwise. - */ - bool is_distance_side_aligned() const; - - /** - * @brief Check if the robot orientation is aligned with walls at the sides. - * - * @return True if the robot is aligned, false otherwise. - */ - bool is_orientation_side_aligned() const; - - /** - * @brief Get the type of wall following the robot can do. - * - * @param pose The current pose of the robot. - * @return The type of wall following. - */ - core::FollowWallType get_follow_wall_type(const Pose& pose) const; - - /** - * @brief Serialize the best route to the goal. - * - * @return The serialized data. - */ - std::vector serialize() const override; - - /** - * @brief Deserialize the best route to the goal. - * - * @param buffer The serialized data. - * @param size The size of the serialized data. - */ - void deserialize(const uint8_t* buffer, uint16_t size) override; - - /** - * @brief Check if the robot can align its back with a wall. - * - * @param pose The current pose of the robot. - * @return True if the robot can align its back, false otherwise. - */ - bool can_align_back(const Pose& pose) const; - - /** - * @brief Add diagonal movements to the best route. - */ - void diagonalize_best_route(); - -private: - /** - * @brief Wall sensors of the robot. - */ - std::shared_ptr> wall_sensors; - - /** - * @brief The maze information the robot has. - */ - TMaze maze; - - /** - * @brief Thickness of the maze walls. - */ - float wall_thickness; - - /** - * @brief Size of the cells in the maze. - */ - float cell_size; - - /** - * @brief Pose of the front wall sensors. - */ - Pose front_sensor_pose; - - /** - * @brief Pose of the side wall sensors. - */ - Pose side_sensor_pose; - - /** - * @brief Region of the cell to consider the information of the front sensors to the current cell. - */ - float front_sensors_region_division; - - /** - * @brief Region of the cell to consider the information of the side sensors to the current cell. - */ - float side_sensors_region_division; - - /** - * @brief Sensor readings tolerance for the front alignment of the robot. - */ - float front_distance_alignment_tolerance; - - /** - * @brief Sensor readings tolerance for the side alignment of the robot. - */ - float side_distance_alignment_tolerance; - - /** - * @brief Sensor readings for the front alignment of the robot at the front. - */ - float front_orientation_alignment_tolerance; - - /** - * @brief Sensor readings for the front alignment of the robot with walls at the side. - */ - float side_orientation_alignment_tolerance; - - /** - * @brief Sensor readings when the front of the robot is distance aligned. - */ - std::array front_distance_reading{}; - - /** - * @brief Sensor readings when the front of the robot is orientation aligned. - */ - std::array front_orientation_reading{}; - - /** - * @brief Sensor readings when the sides of the robot are distance aligned. - */ - std::array side_distance_reading{}; - - /** - * @brief List of points and directions that represent the best route to the goal. - */ - std::list> best_route; - - /** - * @brief Current iterator of the best route. - */ - std::list>::iterator best_route_iterator{best_route.begin()}; -}; -} // namespace micras::nav - -#include "../src/mapping.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) - -#endif // MICRAS_NAV_MAPPING_HPP diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp deleted file mode 100644 index 1788678..0000000 --- a/micras_nav/src/follow_wall.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/** - * @file - */ - -#include "micras/nav/follow_wall.hpp" - -namespace micras::nav { -FollowWall::FollowWall(const std::shared_ptr>& wall_sensors, const Config& config) : - wall_sensors{wall_sensors}, - pid{config.pid}, - base_left_reading{config.base_left_reading}, - base_right_reading{config.base_right_reading} { } - -float FollowWall::action(core::FollowWallType follow_wall_type, float elapsed_time) { - float error{}; - - switch (follow_wall_type) { - case core::FollowWallType::FRONT: - error = this->get_right_value() - this->get_left_value(); - break; - - case core::FollowWallType::LEFT: - error = 2.0F * this->get_left_value(); - break; - - case core::FollowWallType::RIGHT: - error = -2.0F * this->get_right_value(); - break; - - case core::FollowWallType::PARALLEL: - error = this->get_left_value() - this->get_right_value(); - break; - default: - return 0.0F; - } - - const float response = this->pid.update(error, elapsed_time); - return response; -} - -float FollowWall::get_left_value() const { - return this->wall_sensors->get_reading(1) - this->base_left_reading; -} - -float FollowWall::get_right_value() const { - return this->wall_sensors->get_reading(2) - this->base_right_reading; -} - -void FollowWall::reset() { - this->pid.reset(); -} - -void FollowWall::reset_base_readings() { - this->base_left_reading = this->wall_sensors->get_reading(1); - this->base_right_reading = this->wall_sensors->get_reading(2); -} -} // namespace micras::nav diff --git a/micras_nav/src/go_to_point.cpp b/micras_nav/src/go_to_point.cpp deleted file mode 100644 index ecac99f..0000000 --- a/micras_nav/src/go_to_point.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/** - * @file - */ - -#include - -#include "micras/core/utils.hpp" -#include "micras/nav/go_to_point.hpp" - -namespace micras::nav { -GoToPoint::GoToPoint( - const std::shared_ptr>& wall_sensors, const Config& config, - const FollowWall::Config& follow_wall_config -) : - stop_pid(config.stop_pid), - angular_pid(config.angular_pid), - follow_wall{wall_sensors, follow_wall_config}, - cell_size{config.cell_size}, - min_linear_command{config.min_linear_command}, - max_linear_command{config.max_linear_command}, - deceleration_factor{config.deceleration_factor}, - distance_tolerance{config.distance_tolerance}, - velocity_tolerance{config.velocity_tolerance} { } - -Twist GoToPoint::action( - const State& state, const Point& goal, core::FollowWallType follow_wall_type, float elapsed_time, bool stop -) { - float linear_command{}; - float angular_command{}; - const Point goal_distance = goal - state.pose.position; - - if (follow_wall_type != core::FollowWallType::NONE) { - angular_command = this->follow_wall.action(follow_wall_type, elapsed_time); - } else { - const float angular_error = - core::assert_angle(state.pose.orientation - std::atan2(this->cell_size, goal_distance.x)); - angular_command = this->angular_pid.update(angular_error, elapsed_time, state.velocity.angular); - } - - if (stop) { - if (std::abs(goal_distance.y) < this->cell_size) { - linear_command = this->stop_pid.update(-goal_distance.y, elapsed_time, state.velocity.linear); - } else { - linear_command = - core::transition(goal_distance.y, 0.0F, this->max_linear_command, this->deceleration_factor); - } - } else { - linear_command = core::transition( - goal_distance.y, this->min_linear_command, this->max_linear_command, this->deceleration_factor - ); - } - - angular_command *= linear_command / 50.0F; - return {linear_command, angular_command}; -} - -bool GoToPoint::finished(const State& state, const Point& goal, bool stop) const { - return std::abs((goal - state.pose.position).y) <= this->distance_tolerance and - (not stop or std::abs(state.velocity.linear) <= this->velocity_tolerance); -} - -void GoToPoint::reset() { - this->follow_wall.reset(); - this->stop_pid.reset(); - this->angular_pid.reset(); -} - -void GoToPoint::calibrate() { - this->follow_wall.reset_base_readings(); -} -} // namespace micras::nav diff --git a/micras_nav/src/look_at_point.cpp b/micras_nav/src/look_at_point.cpp deleted file mode 100644 index c2106de..0000000 --- a/micras_nav/src/look_at_point.cpp +++ /dev/null @@ -1,34 +0,0 @@ -/** - * @file - */ - -#include "micras/core/utils.hpp" -#include "micras/nav/look_at_point.hpp" - -namespace micras::nav { -LookAtPoint::LookAtPoint(Config config) : - linear_pid(config.linear_pid), - angular_pid{config.angular_pid}, - distance_tolerance{config.distance_tolerance}, - velocity_tolerance{config.velocity_tolerance} { } - -Twist LookAtPoint::action(const State& state, const Point& goal, float elapsed_time) { - const float angular_error = core::assert_angle(state.pose.orientation - state.pose.position.angle_between(goal)); - - return { - this->linear_pid.update(state.velocity.linear, elapsed_time), - this->angular_pid.update(angular_error, elapsed_time, state.velocity.angular) - }; -} - -void LookAtPoint::reset() { - this->linear_pid.reset(); - this->angular_pid.reset(); -} - -bool LookAtPoint::finished(const State& state, const Point& goal, bool stop) const { - return std::abs(core::assert_angle(state.pose.orientation - state.pose.position.angle_between(goal))) <= - this->distance_tolerance and - (not stop or std::abs(state.velocity.angular) <= this->velocity_tolerance); -} -} // namespace micras::nav diff --git a/micras_nav/src/mapping.cpp b/micras_nav/src/mapping.cpp deleted file mode 100644 index 0f8f97d..0000000 --- a/micras_nav/src/mapping.cpp +++ /dev/null @@ -1,337 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_MAPPING_CPP -#define MICRAS_NAV_MAPPING_CPP - -#include - -#include "micras/core/utils.hpp" -#include "micras/nav/mapping.hpp" - -namespace micras::nav { -template -TMapping::TMapping( - const std::shared_ptr>& wall_sensors, TMapping::Config config -) : - wall_sensors{wall_sensors}, - maze{config.start, config.goal}, - wall_thickness{config.wall_thickness}, - cell_size{config.cell_size}, - front_sensor_pose{config.front_sensor_pose}, - side_sensor_pose{config.side_sensor_pose}, - front_sensors_region_division{cell_size - wall_thickness / 2.0F - front_sensor_pose.position.y}, - side_sensors_region_division{ - cell_size - side_sensor_pose.position.y + - (side_sensor_pose.position.x + (wall_thickness - cell_size) / 2.0F) * std::tan(side_sensor_pose.orientation) - }, - front_distance_alignment_tolerance{config.front_distance_alignment_tolerance}, - side_distance_alignment_tolerance{config.side_distance_alignment_tolerance}, - front_orientation_alignment_tolerance{config.front_orientation_alignment_tolerance}, - side_orientation_alignment_tolerance{config.side_orientation_alignment_tolerance}, - front_distance_reading{config.front_distance_reading}, - front_orientation_reading{config.front_orientation_reading}, - side_distance_reading{config.side_distance_reading} { } - -template -void TMapping::update(const Pose& pose) { - const float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); - - if (reliability < 30.0F) { - return; - } - - Information information{}; - nav::Point cell_position = pose.to_cell(cell_size); - - if (cell_position.y < this->front_sensors_region_division) { - if (this->wall_sensors->get_observation(Sensor::FRONT_LEFT) == core::Observation::WALL and - this->wall_sensors->get_observation(Sensor::FRONT_RIGHT) == core::Observation::WALL) { - information.front = core::Observation::WALL; - } else if (this->wall_sensors->get_observation(Sensor::FRONT_LEFT) == core::Observation::FREE_SPACE and - this->wall_sensors->get_observation(Sensor::FRONT_RIGHT) == core::Observation::FREE_SPACE) { - information.front = core::Observation::FREE_SPACE; - } - } - - if (information.front != core::Observation::WALL and - cell_position.y > this->side_sensors_region_division + 2 * this->wall_thickness) { - information.front_left = this->wall_sensors->get_observation(Sensor::LEFT); - information.front_right = this->wall_sensors->get_observation(Sensor::RIGHT); - } - - this->maze.update(pose.to_grid(this->cell_size), information); -} - -template -TMapping::Action TMapping::get_action(const Pose& pose, core::Objective objective) { - GridPoint grid_position = pose.position.to_grid(this->cell_size); - GridPose current_grid_goal{}; - - Point current_goal{}; - Direction orientation{}; - - switch (objective) { - case core::Objective::EXPLORE: - if (this->maze.finished(grid_position)) { - return {Action::Type::FINISHED, pose.position, Direction::EAST}; - } - - current_grid_goal = this->maze.get_current_exploration_goal(grid_position); - current_goal = Point::from_grid(current_grid_goal.position, this->cell_size); - orientation = static_cast(2 * current_grid_goal.orientation); - break; - case core::Objective::RETURN: - if (this->maze.returned(grid_position)) { - this->maze.optimize_route(); - return {Action::Type::FINISHED, pose.position, Direction::EAST}; - } - - this->maze.calculate_best_route(); - current_grid_goal = this->maze.get_current_returning_goal(grid_position); - current_goal = Point::from_grid(current_grid_goal.position, this->cell_size); - orientation = static_cast(2 * current_grid_goal.orientation); - break; - case core::Objective::SOLVE: - if (this->maze.finished(grid_position)) { - return {Action::Type::FINISHED, pose.position, Direction::EAST}; - } - - this->best_route_iterator++; - current_goal = this->best_route_iterator->first; - orientation = this->best_route_iterator->second; - break; - } - - Point current_goal_rotated = current_goal.rotate(orientation); - - if (std::abs(core::assert_angle(pose.orientation - pose.position.angle_between(current_goal))) > - 0.375F * std::numbers::pi_v) { - if (objective == core::Objective::SOLVE) { - this->best_route_iterator--; - } - - return {Action::Type::LOOK_AT, current_goal_rotated, orientation}; - } - - return {Action::Type::GO_TO, current_goal_rotated, orientation}; -} - -template -Pose TMapping::correct_pose(const Pose& pose, core::FollowWallType follow_wall_type) const { - const float reliability = 50.0F * (std::cos(4 * pose.orientation) + 1); - - if (reliability < 20.0F) { - return pose; - } - - Point cell_position = pose.to_cell(this->cell_size); - Point pose_correction{}; - Pose corrected_pose = pose; - Side const direction = angle_to_grid(pose.orientation); - - switch (follow_wall_type) { - case core::FollowWallType::BACK: - pose_correction.y = cell_position.y - (0.04F + this->wall_thickness / 2); - corrected_pose.orientation = - core::assert_angle(static_cast(direction) * std::numbers::pi_v / 2.0F); - break; - - case core::FollowWallType::FRONT: - if (this->is_distance_front_aligned()) { - pose_correction.y = cell_position.y - this->cell_size / 2.0F; - } - - if (this->is_orientation_front_aligned()) { - corrected_pose.orientation = - core::assert_angle(static_cast(direction) * std::numbers::pi_v / 2.0F); - } - break; - case core::FollowWallType::PARALLEL: - if (this->is_distance_side_aligned()) { - pose_correction.x = cell_position.x; - pose_correction.y = cell_position.y - std::hypot(cell_position.x, cell_position.y); - - corrected_pose.orientation = - core::assert_angle(static_cast(direction) * std::numbers::pi_v / 2.0F); - } else if (this->is_orientation_side_aligned()) { - corrected_pose.orientation = - core::assert_angle(static_cast(direction) * std::numbers::pi_v / 2.0F); - } - break; - default: - return pose; - } - - pose_correction = pose_correction.rotate(static_cast((12 - 2 * direction) % 8)); - corrected_pose.position = corrected_pose.position - pose_correction; - - return corrected_pose; -} - -template -void TMapping::calibrate_front() { - this->front_distance_reading[0] = this->wall_sensors->get_reading(0); - this->front_orientation_reading[0] = this->wall_sensors->get_reading(1); - this->front_orientation_reading[1] = this->wall_sensors->get_reading(2); - this->front_distance_reading[1] = this->wall_sensors->get_reading(3); -} - -template -void TMapping::calibrate_side() { - this->side_distance_reading[0] = this->wall_sensors->get_reading(1); - this->side_distance_reading[1] = this->wall_sensors->get_reading(2); -} - -template -bool TMapping::is_distance_front_aligned() const { - return core::is_near( - this->wall_sensors->get_reading(0), this->front_distance_reading[0], - this->front_distance_alignment_tolerance - ) and - core::is_near( - this->wall_sensors->get_reading(3), this->front_distance_reading[1], - this->front_distance_alignment_tolerance - ); -} - -template -bool TMapping::is_orientation_front_aligned() const { - return std::abs( - (this->wall_sensors->get_reading(1) - this->front_orientation_reading[0]) + - (this->wall_sensors->get_reading(2) - this->front_orientation_reading[1]) - ) <= this->front_orientation_alignment_tolerance; -} - -template -bool TMapping::is_distance_side_aligned() const { - return core::is_near( - this->wall_sensors->get_reading(1), this->side_distance_reading[0], - this->side_distance_alignment_tolerance - ) and - core::is_near( - this->wall_sensors->get_reading(2), this->side_distance_reading[1], - this->side_distance_alignment_tolerance - ); -} - -template -bool TMapping::is_orientation_side_aligned() const { - return std::abs( - (this->wall_sensors->get_reading(1) - this->side_distance_reading[0]) + - (this->wall_sensors->get_reading(2) - this->side_distance_reading[1]) - ) <= this->side_orientation_alignment_tolerance; -} - -template -core::FollowWallType TMapping::get_follow_wall_type(const Pose& pose) const { - nav::Point cell_position = pose.to_cell(cell_size); - - core::FollowWallType follow_wall_type = this->maze.get_follow_wall_type(pose.to_grid(this->cell_size)); - - if (follow_wall_type == core::FollowWallType::FRONT and cell_position.y > this->side_sensors_region_division) { - return follow_wall_type; - } - - if (cell_position.y < this->side_sensors_region_division and follow_wall_type != core::FollowWallType::FRONT) { - return follow_wall_type; - } - - bool can_follow_left = this->wall_sensors->get_observation(Sensor::LEFT) == core::Observation::WALL; - bool can_follow_right = this->wall_sensors->get_observation(Sensor::RIGHT) == core::Observation::WALL; - - if (can_follow_left and can_follow_right) { - return core::FollowWallType::PARALLEL; - } - - if (can_follow_left) { - return core::FollowWallType::LEFT; - } - - if (can_follow_right) { - return core::FollowWallType::RIGHT; - } - - return core::FollowWallType::NONE; -} - -template -std::vector TMapping::serialize() const { - std::vector buffer; - buffer.reserve(3 * this->maze.get_best_route().size()); - - for (const auto& [cost, grid_pose] : this->maze.get_best_route()) { - buffer.emplace_back(grid_pose.position.x); - buffer.emplace_back(grid_pose.position.y); - buffer.emplace_back(grid_pose.orientation); - } - - return buffer; -} - -template -void TMapping::deserialize(const uint8_t* buffer, uint16_t size) { - this->best_route.clear(); - - for (uint32_t i = 0; i < size; i += 3) { - this->best_route.emplace_back( - // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) - Point::from_grid({buffer[i], buffer[i + 1]}, this->cell_size), static_cast(2 * buffer[i + 2]) - ); - } -} - -template -bool TMapping::can_align_back(const Pose& pose) const { - return this->maze.has_wall(pose.to_grid(this->cell_size).turned_back()); -} - -template -void TMapping::diagonalize_best_route() { - for (auto it = std::next(this->best_route.begin()); it != this->best_route.end(); it++) { - auto next_it = std::next(it); - - if (next_it == this->best_route.end()) { - break; - } - - auto diagonal_direction = static_cast((next_it->second + it->second) / 2); - - if (std::abs(next_it->second - it->second) > 2) { - diagonal_direction = Direction::SOUTHEAST; - } - - this->best_route.insert(it, {it->first.move_towards(std::prev(it)->first, this->cell_size / 2), it->second}); - this->best_route.insert(it, {it->first.move_towards(next_it->first, this->cell_size / 2), diagonal_direction}); - } - - uint8_t pos = 1; - - for (auto it = std::next(this->best_route.begin()); it != this->best_route.end(); pos++) { - if (pos % 3 == 0) { - it = this->best_route.erase(it); - } else { - it++; - } - } - - for (auto it = std::next(this->best_route.begin()); it != this->best_route.end();) { - auto next_it = std::next(it); - - if (next_it == this->best_route.end()) { - break; - } - - if (it->second == next_it->second) { - it = this->best_route.erase(it); - } else if (it->first == next_it->first) { - this->best_route.erase(next_it); - } else { - it++; - } - } -} -} // namespace micras::nav - -#endif // MICRAS_NAV_MAPPING_CPP From b064a7348abaeb9dad295482ef0ef7068ed4bf7a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 00:19:32 -0300 Subject: [PATCH 285/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Move=20base=20read?= =?UTF-8?q?ings=20to=20wall=20sensors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/types.hpp | 10 ++-- .../include/micras/proxy/wall_sensors.hpp | 57 +++++------------- micras_proxy/src/wall_sensors.cpp | 59 ++++--------------- 3 files changed, 30 insertions(+), 96 deletions(-) diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index 244d389..0f5c334 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -9,12 +9,12 @@ namespace micras::core { /** - * @brief Possible values for the observation of a wall. + * @brief Possible values for the observation from the sensors. */ -enum Observation : uint8_t { - UNKNOWN = 0, - FREE_SPACE = 1, - WALL = 2 +struct Observation { + bool left{}; + bool front{}; + bool right{}; }; /** diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 0b12e57..1385991 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -28,9 +28,8 @@ class TWallSensors { hal::Pwm::Config led_0_pwm; hal::Pwm::Config led_1_pwm; float filter_cutoff; + std::array base_readings; float uncertainty; - std::array wall_threshold; - std::array free_threshold; }; /** @@ -59,9 +58,16 @@ class TWallSensors { * @brief Get the observation from a sensor. * * @param sensor_index Index of the sensor. - * @return core::Observation Observation from the sensor. + * @return True if the sensor detects a wall, false otherwise. */ - core::Observation get_observation(uint8_t sensor_index) const; + bool get_wall(uint8_t sensor_index) const; + + /** + * @brief Get the observations from all sensors. + * + * @return Observations from all sensors. + */ + core::Observation get_observation() const; /** * @brief Get the reading from a sensor. @@ -94,26 +100,6 @@ class TWallSensors { */ void calibrate_right_wall(); - /** - * @brief Calibrate the wall sensors for free space at the front. - */ - void calibrate_front_free_space(); - - /** - * @brief Calibrate the wall sensors for free space at the left. - */ - void calibrate_left_free_space(); - - /** - * @brief Calibrate the wall sensors for free space at the right. - */ - void calibrate_right_free_space(); - - /** - * @brief Update the wall sensors thresholds. - */ - void update_thresholds(); - private: /** * @brief ADC DMA handle. @@ -141,29 +127,14 @@ class TWallSensors { std::array filters; /** - * @brief Uncertainty of the wall sensors. - */ - float uncertainty; - - /** - * @brief Measured wall value during calibration. - */ - std::array wall_calibration_measure; - - /** - * @brief Measured free space value during calibration. + * @brief Measured wall values during calibration. */ - std::array free_space_calibration_measure; + std::array base_readings; /** - * @brief Minimum reading value to identify a wall. - */ - std::array wall_threshold; - - /** - * @brief Maximum reading value to identify a free space. + * @brief Uncertainty of the wall sensors. */ - std::array free_space_threshold; + float uncertainty; }; } // namespace micras::proxy diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index c7f938b..a55aab6 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -15,9 +15,7 @@ TWallSensors::TWallSensors(const Config& config) : led_0_pwm{config.led_0_pwm}, led_1_pwm{config.led_1_pwm}, filters{core::make_array(config.filter_cutoff)}, - uncertainty{config.uncertainty}, - wall_threshold{config.wall_threshold}, - free_space_threshold{config.free_threshold} { + uncertainty{config.base_readings} { this->adc.start_dma(this->buffer); this->turn_off(); } @@ -42,18 +40,13 @@ void TWallSensors::update() { } template -core::Observation TWallSensors::get_observation(uint8_t sensor_index) const { - float reading = this->filters.at(sensor_index).get_last(); - - if (reading > this->wall_threshold.at(sensor_index)) { - return core::Observation::WALL; - } - - if (reading < this->free_space_threshold.at(sensor_index)) { - return core::Observation::FREE_SPACE; - } +bool TWallSensors::get_wall(uint8_t sensor_index) const { + return this->filters.at(sensor_index).get_last() > this->base_readings.at(sensor_index) * this->uncertainty; +} - return core::Observation::UNKNOWN; +template +core::Observation TWallSensors::get_observation() const { + return {this->get_wall(1), this->get_wall(0) && this->get_wall(3), this->get_wall(2)}; } template @@ -70,48 +63,18 @@ float TWallSensors::get_adc_reading(uint8_t sensor_index) const template void TWallSensors::calibrate_front_wall() { - this->wall_calibration_measure[0] = this->get_reading(0); - this->wall_calibration_measure[3] = this->get_reading(3); + this->base_readings[0] = this->get_reading(0); + this->base_readings[3] = this->get_reading(3); } template void TWallSensors::calibrate_left_wall() { - this->wall_calibration_measure[1] = this->get_reading(1); + this->base_readings[1] = this->get_reading(1); } template void TWallSensors::calibrate_right_wall() { - this->wall_calibration_measure[2] = this->get_reading(2); -} - -template -void TWallSensors::calibrate_front_free_space() { - this->free_space_calibration_measure[0] = this->get_reading(0); - this->free_space_calibration_measure[3] = this->get_reading(3); -} - -template -void TWallSensors::calibrate_left_free_space() { - this->free_space_calibration_measure[1] = this->get_reading(1); -} - -template -void TWallSensors::calibrate_right_free_space() { - this->free_space_calibration_measure[2] = this->get_reading(2); -} - -template -void TWallSensors::update_thresholds() { - for (uint8_t i = 0; i < num_of_sensors; i++) { - float mean_value = (this->wall_calibration_measure.at(i) + this->free_space_calibration_measure.at(i)) / 2.0F; - - float unknown_delta = this->uncertainty * - (this->wall_calibration_measure.at(i) - this->free_space_calibration_measure.at(i)) / - 2.0F; - - this->wall_threshold.at(i) = mean_value + unknown_delta; - this->free_space_threshold.at(i) = mean_value - unknown_delta; - } + this->base_readings[2] = this->get_reading(2); } } // namespace micras::proxy From 9acbd3c63de65c5ecb10b7df860c22bff54297d6 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 00:21:41 -0300 Subject: [PATCH 286/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20free=20space=20?= =?UTF-8?q?and=20wall=20count=20form=20cells?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 12 +++--- micras_nav/src/maze.cpp | 60 +++++--------------------- 2 files changed, 15 insertions(+), 57 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 24ebe2a..e0343f1 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -34,9 +34,9 @@ class TMaze { * @brief Update the maze walls with the current pose and new information. * * @param pose The pose of the robot. - * @param information The information from the wall sensors. + * @param observation The observation from the wall sensors. */ - void update(const GridPose& pose, Information information); + void update(const GridPose& pose, core::Observation observation); /** * @brief Return the next point the robot should go to when exploring. @@ -108,9 +108,8 @@ class TMaze { * @brief Type to store the information of a cell in the maze. */ struct Cell { - std::array wall_count{}; - std::array free_count{}; - uint16_t cost{0xFFFF}; + std::array walls{}; + uint16_t cost{0xFFFF}; }; /** @@ -139,9 +138,8 @@ class TMaze { * * @param pose The pose of the robot. * @param wall Whether there is a wall in front of the robot. - * @return True if the information is new, false otherwise. */ - bool update_wall(const GridPose& pose, bool wall); + void update_wall(const GridPose& pose, bool wall); /** * @brief Cells matrix representing the maze. diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index a852baa..3e98eee 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -56,34 +56,12 @@ TMaze::TMaze(const GridPose& start, const std::unordered_set -void TMaze::update(const GridPose& pose, Information information) { - bool new_information{}; +void TMaze::update(const GridPose& pose, core::Observation observation) { + this->update_wall(pose.turned_left(), information.left); + this->update_wall(pose, information.front); + this->update_wall(pose.turned_right(), information.right); - if (information.left != core::Observation::UNKNOWN) { - new_information |= this->update_wall(pose.turned_left(), information.left == core::Observation::WALL); - } - - if (information.front_left != core::Observation::UNKNOWN) { - new_information |= - this->update_wall(pose.front().turned_left(), information.front_left == core::Observation::WALL); - } - - if (information.front != core::Observation::UNKNOWN) { - new_information |= this->update_wall(pose, information.front == core::Observation::WALL); - } - - if (information.front_right != core::Observation::UNKNOWN) { - new_information |= - this->update_wall(pose.front().turned_right(), information.front_right == core::Observation::WALL); - } - - if (information.right != core::Observation::UNKNOWN) { - new_information |= this->update_wall(pose.turned_right(), information.right == core::Observation::WALL); - } - - if (new_information) { - this->calculate_costmap(); - } + this->calculate_costmap(); } template @@ -129,38 +107,20 @@ TMaze::Cell& TMaze::get_cell(const GridPoint& posi } template -bool TMaze::update_wall(const GridPose& pose, bool wall) { +void TMaze::update_wall(const GridPose& pose, bool wall) { if (pose.position.x >= width or pose.position.y >= height) { - return false; - } - - bool new_information = this->has_wall(pose); - - if (wall) { - this->get_cell(pose.position).wall_count[pose.orientation]++; - } else { - this->get_cell(pose.position).free_count[pose.orientation]++; + return; } - new_information ^= this->has_wall(pose); + this->get_cell(pose.position).walls[pose.orientation] = wall; GridPose front_pose = pose.front(); if (front_pose.position.x >= width or front_pose.position.y >= height) { - return new_information; + return; } - bool new_front_information = this->has_wall(front_pose); - - if (wall) { - this->get_cell(front_pose.position).wall_count[pose.turned_back().orientation]++; - } else { - this->get_cell(front_pose.position).free_count[pose.turned_back().orientation]++; - } - - new_front_information ^= this->has_wall(front_pose); - - return new_information or new_front_information; + this->get_cell(front_pose.position).wall_count[pose.turned_back().orientation] = wall; } template From dfaa5f50f61e2c45ba2626ca3d71301d3e22f0ce Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 00:22:09 -0300 Subject: [PATCH 287/561] =?UTF-8?q?=E2=9C=A8=20Add=20basic=20actions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/actions/base.hpp | 39 +++++++++++ .../include/micras/nav/actions/move.hpp | 64 +++++++++++++++++++ .../include/micras/nav/actions/turn.hpp | 34 ++++++++++ 3 files changed, 137 insertions(+) create mode 100644 micras_nav/include/micras/nav/actions/base.hpp create mode 100644 micras_nav/include/micras/nav/actions/move.hpp create mode 100644 micras_nav/include/micras/nav/actions/turn.hpp diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp new file mode 100644 index 0000000..9e46e9a --- /dev/null +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -0,0 +1,39 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_BASE_ACTION_HPP +#define MICRAS_NAV_BASE_ACTION_HPP + +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Class to follow the side walls using a PID controller. + */ +class BaseAction { +public: + /** + * @brief Virtual destructor for the BaseAction class. + */ + virtual ~BaseAction() = default; + + virtual Twist get_twist(const State& state) const = 0; + + virtual bool finished(const State& state) const = 0; + +protected: + /** + * @brief Special member functions declared as default. + */ + ///@{ + BaseAction() = default; + BaseAction(const BaseAction&) = default; + BaseAction(BaseAction&&) = default; + BaseAction& operator=(const BaseAction&) = default; + BaseAction& operator=(BaseAction&&) = default; + ///@} +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_BASE_ACTION_HPP diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp new file mode 100644 index 0000000..589dcfa --- /dev/null +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -0,0 +1,64 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MOVE_ACTION_HPP +#define MICRAS_NAV_MOVE_ACTION_HPP + +#include + +#include "micras/nav/actions/base.hpp" + +namespace micras::nav { +/** + * @brief Class to follow the side walls using a PID controller. + */ +class MoveAction : public BaseAction { +public: + MoveAction( + float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, + float max_deceleration + ) : + distance(distance), + start_speed_2(start_speed * start_speed), + end_speed_2(end_speed * end_speed), + max_speed{max_speed}, + max_acceleration_doubled{2.0F * max_acceleration}, + max_deceleration_doubled{2.0F * max_deceleration}, + decelerate_distance{ + (start_speed_2 - end_speed_2 - max_deceleration_doubled * distance) / + (max_acceleration_doubled - max_deceleration_doubled) + } { } + + Twist get_twist(const State& state) const override { + const float current_distance = state.pose.position.distance({0.0F, 0.0F}); + Twist twist{}; + + if (state.pose.position.distance({0.0F, 0.0F}) < this->decelerate_distance) { + twist = {std::sqrtf(this->start_speed_2 + this->max_acceleration_doubled * current_distance), 0.0F}; + } + + twist = { + std::sqrtf(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), 0.0F + }; + + twist.linear = std::fminf(twist.linear, this->max_speed); + } + + bool finished(const State& state) const override { + return state.pose.position.distance({0.0F, 0.0F}) >= this->distance; + } + +private: + float distance; + float start_speed_2; + float end_speed_2; + float max_speed; + float max_acceleration_doubled; + float max_deceleration_doubled; + + float decelerate_distance; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_MOVE_ACTION_HPP diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp new file mode 100644 index 0000000..4bc26d3 --- /dev/null +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -0,0 +1,34 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_TURN_ACTION_HPP +#define MICRAS_NAV_TURN_ACTION_HPP + +#include + +#include "micras/nav/actions/base.hpp" + +namespace micras::nav { +/** + * @brief Class to follow the side walls using a PID controller. + */ +class TurnAction : public BaseAction { +public: + TurnAction(float angle, float curve_radius, float max_centrifugal_acceleration) : + angle{angle}, + linear_speed{std::sqrtf(max_centrifugal_acceleration * curve_radius)}, + angular_speed{std::copysignf(linear_speed / curve_radius, angle)} { } + + Twist get_twist(const State& state) const override { return {linear_speed, angular_speed}; } + + bool finished(const State& state) const override { return state.pose.orientation >= this->angle; } + +private: + float angle; + float linear_speed; + float angular_speed; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_TURN_ACTION_HPP From f4fbf1c3c5603c8b917fd48f4260da44444e0025 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 01:19:01 -0300 Subject: [PATCH 288/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20Serializable=20to?= =?UTF-8?q?=20core?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/core}/serializable.hpp | 10 +++++----- micras_proxy/include/micras/proxy/storage.hpp | 12 ++++++------ micras_proxy/src/storage.cpp | 4 ++-- 3 files changed, 13 insertions(+), 13 deletions(-) rename {micras_proxy/include/micras/proxy => micras_core/include/micras/core}/serializable.hpp (85%) diff --git a/micras_proxy/include/micras/proxy/serializable.hpp b/micras_core/include/micras/core/serializable.hpp similarity index 85% rename from micras_proxy/include/micras/proxy/serializable.hpp rename to micras_core/include/micras/core/serializable.hpp index eff0aac..e49c612 100644 --- a/micras_proxy/include/micras/proxy/serializable.hpp +++ b/micras_core/include/micras/core/serializable.hpp @@ -2,13 +2,13 @@ * @file */ -#ifndef MICRAS_PROXY_SERIALIZABLE_HPP -#define MICRAS_PROXY_SERIALIZABLE_HPP +#ifndef MICRAS_CORE_SERIALIZABLE_HPP +#define MICRAS_CORE_SERIALIZABLE_HPP #include #include -namespace micras::proxy { +namespace micras::core { /** * @brief Interface class for serializable classes. */ @@ -46,6 +46,6 @@ class ISerializable { ISerializable& operator=(ISerializable&&) = default; ///@} }; -} // namespace micras::proxy +} // namespace micras::core -#endif // MICRAS_PROXY_SERIALIZABLE_HPP +#endif // MICRAS_CORE_SERIALIZABLE_HPP diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index 00c895e..2b24a21 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -11,7 +11,7 @@ #include #include -#include "serializable.hpp" +#include "micras/core/serializable.hpp" namespace micras::proxy { template @@ -56,7 +56,7 @@ class Storage { * @param name Name of the variable. * @param data Reference to the variable. */ - void create(const std::string& name, const ISerializable& data); + void create(const std::string& name, const core::ISerializable& data); /** * @brief Sync a primitive variable with the storage. @@ -81,7 +81,7 @@ class Storage { * @param name Name of the variable. * @param data Reference to the variable. */ - void sync(const std::string& name, ISerializable& data); + void sync(const std::string& name, core::ISerializable& data); /** * @brief Save the storage to the flash. @@ -102,9 +102,9 @@ class Storage { * @brief Struct for serializable variables. */ struct SerializableVariable { - const ISerializable* ram_pointer{nullptr}; - uint16_t buffer_address{}; - uint16_t size{}; + const core::ISerializable* ram_pointer{nullptr}; + uint16_t buffer_address{}; + uint16_t size{}; }; /** diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index b528471..2072609 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -27,11 +27,11 @@ Storage::Storage(const Config& config) : start_page{config.start_page}, number_o this->serializables = deserialize_var_map(this->buffer, num_serializables); } -void Storage::create(const std::string& name, const ISerializable& data) { +void Storage::create(const std::string& name, const core::ISerializable& data) { this->serializables[name].ram_pointer = &data; } -void Storage::sync(const std::string& name, ISerializable& data) { +void Storage::sync(const std::string& name, core::ISerializable& data) { if (this->serializables.contains(name) and this->serializables.at(name).ram_pointer == nullptr) { const auto& serializable = this->serializables.at(name); data.deserialize(&this->buffer.at(serializable.buffer_address), serializable.size); From a643c9ef22b7e8276e776be59f4d1ae59aee46e4 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 01:19:37 -0300 Subject: [PATCH 289/561] =?UTF-8?q?=E2=9C=A8=20Add=20serialization=20to=20?= =?UTF-8?q?maze?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 47 +++++------ micras_nav/src/maze.cpp | 103 +++++++++++-------------- 2 files changed, 68 insertions(+), 82 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index e0343f1..74fc45f 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -10,6 +10,7 @@ #include #include +#include "micras/core/serializable.hpp" #include "micras/nav/grid_pose.hpp" namespace micras::nav { @@ -20,7 +21,7 @@ namespace micras::nav { * @tparam height The height of the maze. */ template -class TMaze { +class TMaze : public core::ISerializable { public: /** * @brief Construct a new Maze object. @@ -36,7 +37,7 @@ class TMaze { * @param pose The pose of the robot. * @param observation The observation from the wall sensors. */ - void update(const GridPose& pose, core::Observation observation); + void update_walls(const GridPose& pose, const core::Observation& observation); /** * @brief Return the next point the robot should go to when exploring. @@ -44,15 +45,7 @@ class TMaze { * @param position The current position of the robot. * @return The next point the robot should go to when exploring. */ - GridPose get_current_exploration_goal(const GridPoint& position) const; - - /** - * @brief Return the next point the robot should go to when returning. - * - * @param position The current position of the robot. - * @return The next point the robot should go to when returning. - */ - GridPose get_current_returning_goal(const GridPoint& position) const; + GridPose get_next_goal(const GridPoint& position, bool returning) const; /** * @brief Check the type of wall following the robot can do. @@ -60,7 +53,7 @@ class TMaze { * @param pose The pose of the robot. * @return The type of wall following. */ - core::FollowWallType get_follow_wall_type(const GridPose& pose) const; + core::Observation get_observation(const GridPose& pose) const; /** * @brief Check whether the robot has finished the maze. @@ -68,7 +61,7 @@ class TMaze { * @param position The current position of the robot. * @return True if the robot has finished the maze, false otherwise. */ - bool finished(const GridPoint& position) const; + bool finished(const GridPoint& position, bool returning) const; /** * @brief Check whether the robot has returned to the start. @@ -83,11 +76,6 @@ class TMaze { */ void calculate_best_route(); - /** - * @brief Improve the route to the goal. - */ - void optimize_route(); - /** * @brief Return the best route to the goal. * @@ -96,12 +84,19 @@ class TMaze { const std::map>& get_best_route() const; /** - * @brief Check whether there is a wall at the front of a given pose. + * @brief Serialize the best route to the goal. * - * @param pose The pose to check. - * @return True if there is a wall, false otherwise. + * @return The serialized data. */ - bool has_wall(const GridPose& pose) const; + std::vector serialize() const override; + + /** + * @brief Deserialize the best route to the goal. + * + * @param buffer The serialized data. + * @param size The size of the serialized data. + */ + void deserialize(const uint8_t* buffer, uint16_t size) override; private: /** @@ -141,6 +136,14 @@ class TMaze { */ void update_wall(const GridPose& pose, bool wall); + /** + * @brief Check whether there is a wall at the front of a given pose. + * + * @param pose The pose to check. + * @return True if there is a wall, false otherwise. + */ + bool has_wall(const GridPose& pose) const; + /** * @brief Cells matrix representing the maze. */ diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 3e98eee..dc627ca 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -56,7 +56,7 @@ TMaze::TMaze(const GridPose& start, const std::unordered_set -void TMaze::update(const GridPose& pose, core::Observation observation) { +void TMaze::update_walls(const GridPose& pose, const core::Observation& observation) { this->update_wall(pose.turned_left(), information.left); this->update_wall(pose, information.front); this->update_wall(pose.turned_right(), information.right); @@ -65,8 +65,18 @@ void TMaze::update(const GridPose& pose, core::Observation observ } template -GridPose TMaze::get_current_exploration_goal(const GridPoint& position) const { - uint16_t current_cost = this->get_cell(position).cost; +GridPose TMaze::get_next_goal(const GridPoint& position, bool returning) const { + uint16_t current_cost = this->get_cell(position).cost; + + if (returning) { + if (this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { + auto current = this->best_route.find(current_cost); + return {std::prev(current)->second.position, current->second.turned_back().orientation}; + } + + return get_current_exploration_goal(position); + } + GridPoint next_position = position; Side next_side{}; @@ -84,18 +94,6 @@ GridPose TMaze::get_current_exploration_goal(const GridPoint& pos return {next_position, next_side}; } -template -GridPose TMaze::get_current_returning_goal(const GridPoint& position) const { - uint16_t current_cost = this->get_cell(position).cost; - - if (this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { - auto current = this->best_route.find(current_cost); - return {std::prev(current)->second.position, current->second.turned_back().orientation}; - } - - return get_current_exploration_goal(position); -} - template const TMaze::Cell& TMaze::get_cell(const GridPoint& position) const { return this->cells.at(position.y).at(position.x); @@ -130,38 +128,13 @@ bool TMaze::has_wall(const GridPose& pose) const { } template -core::FollowWallType TMaze::get_follow_wall_type(const GridPose& pose) const { - if (pose.position.x >= width or pose.position.y >= height) { - return core::FollowWallType::NONE; - } - - if (this->has_wall(pose)) { - return core::FollowWallType::FRONT; - } - - if (this->has_wall(pose.turned_left()) and this->has_wall(pose.turned_right())) { - return core::FollowWallType::PARALLEL; - } - - if (this->has_wall(pose.turned_left())) { - return core::FollowWallType::LEFT; - } - - if (this->has_wall(pose.turned_right())) { - return core::FollowWallType::RIGHT; - } - - return core::FollowWallType::NONE; -} - -template -bool TMaze::finished(const GridPoint& position) const { - return this->goal.contains(position); +core::Observation TMaze::get_observation(const GridPose& pose) const { + return {this->has_wall(pose.turned_left()), this->has_wall(pose), this->has_wall(pose.turned_right())}; } template -bool TMaze::returned(const GridPoint& position) const { - return this->start.position == position; +bool TMaze::finished(const GridPoint& position, bool returning) const { + return returning ? this->start.position == position : this->goal.contains(position); } template @@ -176,22 +149,6 @@ void TMaze::calculate_best_route() { } } -template -void TMaze::optimize_route() { - for (auto it = std::next(this->best_route.begin()); it != this->best_route.end();) { - auto next = std::next(it); - - if (next != this->best_route.end()) { - if (it->second.orientation == next->second.orientation) { - it = this->best_route.erase(it); - continue; - } - } - - it++; - } -} - template const std::map>& TMaze::get_best_route() const { return this->best_route; @@ -233,6 +190,32 @@ void TMaze::calculate_costmap() { } } } + +template +std::vector TMaze::serialize() const { + std::vector buffer; + buffer.reserve(3 * this->best_route.size()); + + for (const auto& [cost, grid_pose] : this->best_route) { + buffer.emplace_back(grid_pose.position.x); + buffer.emplace_back(grid_pose.position.y); + buffer.emplace_back(grid_pose.orientation); + } + + return buffer; +} + +template +void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { + this->best_route.clear(); + + for (uint32_t i = 0; i < size; i += 3) { + this->best_route.emplace_back( + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) + Point::from_grid({buffer[i], buffer[i + 1]}, this->cell_size), static_cast(2 * buffer[i + 2]) + ); + } +} } // namespace micras::nav #endif // MICRAS_NAV_MAZE_CPP From 904ab09b1eba728b6c73f4cf2a33cf37cf1a9104 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 01:52:36 -0300 Subject: [PATCH 290/561] =?UTF-8?q?=F0=9F=9A=A7=20Add=20ActionQueuer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 60 +++++++++++++++++++ .../include/micras/nav/actions/base.hpp | 16 ++--- .../include/micras/nav/actions/move.hpp | 6 +- .../include/micras/nav/actions/turn.hpp | 2 +- micras_nav/src/action_queuer.cpp | 43 +++++++++++++ 5 files changed, 115 insertions(+), 12 deletions(-) create mode 100644 micras_nav/include/micras/nav/action_queuer.hpp create mode 100644 micras_nav/src/action_queuer.cpp diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp new file mode 100644 index 0000000..1c8a541 --- /dev/null +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -0,0 +1,60 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_ACTION_QUEUER_HPP +#define MICRAS_NAV_ACTION_QUEUER_HPP + +#include +#include +#include + +#include "micras/nav/actions/move.hpp" +#include "micras/nav/actions/turn.hpp" + +namespace micras::nav { +/** + * @brief Class to follow the side walls using a PID controller. + */ +class ActionQueuer { +public: + struct Config { + struct Dynamic { + float max_speed; + float max_acceleration; + float max_deceleration; + float curve_radius; + float max_centrifugal_acceleration; + }; + + float cell_size; + float start_offset; + Dynamic exploring; + Dynamic solving; + }; + + ActionQueuer(Config config); + + void push(const GridPose& current_pose, const GridPose& target_pose); + + const std::shared_ptr& pop(); + + void recalculate(const std::map>& best_route); + +private: + float cell_size; + float start_offset; + Config::Dynamic exploring_params; + Config::Dynamic solving_params; + + std::shared_ptr start; + std::shared_ptr move_forward; + std::shared_ptr turn_left; + std::shared_ptr turn_right; + std::shared_ptr turn_back; + + std::queue> action_queue; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_ACTION_QUEUER_HPP diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 9e46e9a..a9dbf47 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -11,12 +11,12 @@ namespace micras::nav { /** * @brief Class to follow the side walls using a PID controller. */ -class BaseAction { +class Action { public: /** - * @brief Virtual destructor for the BaseAction class. + * @brief Virtual destructor for the Action class. */ - virtual ~BaseAction() = default; + virtual ~Action() = default; virtual Twist get_twist(const State& state) const = 0; @@ -27,11 +27,11 @@ class BaseAction { * @brief Special member functions declared as default. */ ///@{ - BaseAction() = default; - BaseAction(const BaseAction&) = default; - BaseAction(BaseAction&&) = default; - BaseAction& operator=(const BaseAction&) = default; - BaseAction& operator=(BaseAction&&) = default; + Action() = default; + Action(const Action&) = default; + Action(Action&&) = default; + Action& operator=(const Action&) = default; + Action& operator=(Action&&) = default; ///@} }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 589dcfa..f14d815 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -13,7 +13,7 @@ namespace micras::nav { /** * @brief Class to follow the side walls using a PID controller. */ -class MoveAction : public BaseAction { +class MoveAction : public Action { public: MoveAction( float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, @@ -26,8 +26,8 @@ class MoveAction : public BaseAction { max_acceleration_doubled{2.0F * max_acceleration}, max_deceleration_doubled{2.0F * max_deceleration}, decelerate_distance{ - (start_speed_2 - end_speed_2 - max_deceleration_doubled * distance) / - (max_acceleration_doubled - max_deceleration_doubled) + (end_speed_2 - start_speed_2 + max_deceleration_doubled * distance) / + (max_acceleration_doubled + max_deceleration_doubled) } { } Twist get_twist(const State& state) const override { diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 4bc26d3..8454fb9 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -13,7 +13,7 @@ namespace micras::nav { /** * @brief Class to follow the side walls using a PID controller. */ -class TurnAction : public BaseAction { +class TurnAction : public Action { public: TurnAction(float angle, float curve_radius, float max_centrifugal_acceleration) : angle{angle}, diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp new file mode 100644 index 0000000..6b2cea6 --- /dev/null +++ b/micras_nav/src/action_queuer.cpp @@ -0,0 +1,43 @@ +/** + * @file + */ + +#include + +#include "micras/nav/action_queuer.hpp" + +namespace micras::nav { +ActionQueuer::ActionQueuer(Config config) : + cell_size{config.cell_size}, + start_offset{config.start_offset}, + exploring_params{config.exploring}, + solving_params{config.solving}, + start{std::make_shared( + cell_size - start_offset, 0.0F, exploring_params.max_speed, exploring_params.max_speed, + exploring_params.max_acceleration, exploring_params.max_deceleration + )}, + move_forward{std::make_shared( + cell_size, exploring_params.max_speed, exploring_params.max_speed, exploring_params.max_speed, + exploring_params.max_acceleration, exploring_params.max_deceleration + )}, + turn_left{std::make_shared( + std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration + )}, + turn_right{std::make_shared( + -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration + )}, + turn_back{std::make_shared( + std::numbers::pi_v, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration + )}, + action_queue{{start}} { } + +void ActionQueuer::push(const GridPose& current_pose, const GridPose& target_pose) { } + +const std::shared_ptr& ActionQueuer::pop() { + auto action = this->action_queue.front(); + this->action_queue.pop(); + return action; +} + +void ActionQueuer::recalculate(const std::map>& best_route) { } +} // namespace micras::nav From 272278cca047cd21e430e026aeba16bcd52530ef Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 17:16:11 -0300 Subject: [PATCH 291/561] =?UTF-8?q?=E2=8F=AA=20Add=20follow=20wall=20back?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 64 +++++++++++++++++++ micras_nav/src/follow_wall.cpp | 34 ++++++++++ .../include/micras/proxy/wall_sensors.hpp | 8 +++ micras_proxy/src/wall_sensors.cpp | 5 ++ 4 files changed, 111 insertions(+) create mode 100644 micras_nav/include/micras/nav/follow_wall.hpp create mode 100644 micras_nav/src/follow_wall.cpp diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp new file mode 100644 index 0000000..cee2648 --- /dev/null +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -0,0 +1,64 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_FOLLOW_WALL +#define MICRAS_NAV_FOLLOW_WALL + +#include + +#include "micras/core/pid_controller.hpp" +#include "micras/proxy/wall_sensors.hpp" + +namespace micras::nav { +/** + * @brief Class to follow the side walls using a PID controller. + */ +class FollowWall { +public: + /** + * @brief Configuration struct for the FollowWall class. + */ + struct Config { + core::PidController::Config pid; + float max_linear_speed; + }; + + /** + * @brief Construct a new FollowWall object. + * + * @param wall_sensors The wall sensors of the robot. + * @param config The configuration for the FollowWall class. + */ + FollowWall(const std::shared_ptr>& wall_sensors, const Config& config); + + /** + * @brief Update the PID controller and return the response. + * + * @param follow_wall_type The type of wall following to perform. + * @param elapsed_time The time elapsed since the last update. + * @return The response of the PID controller. + */ + float action(core::Observation observation, float elapsed_time, float linear_speed); + + /** + * @brief Reset the PID controller. + */ + void reset(); + +private: + /** + * @brief Wall sensors of the robot. + */ + std::shared_ptr> wall_sensors; + + /** + * @brief PID controller for the wall following. + */ + core::PidController pid; + + float max_linear_speed; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_FOLLOW_WALL diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp new file mode 100644 index 0000000..bf8d647 --- /dev/null +++ b/micras_nav/src/follow_wall.cpp @@ -0,0 +1,34 @@ +/** + * @file + */ + +#include "micras/nav/follow_wall.hpp" + +namespace micras::nav { +FollowWall::FollowWall(const std::shared_ptr>& wall_sensors, const Config& config) : + wall_sensors{wall_sensors}, pid{config.pid}, max_linear_speed{config.max_linear_speed} { } + +float FollowWall::action(core::Observation observation, float elapsed_time, float linear_speed) { + float error{}; + + if (observation.front) { + error = this->wall_sensors->get_sensor_error(0) - this->wall_sensors->get_sensor_error(3); + } else if (observation.left and observation.right) { + error = this->wall_sensors->get_sensor_error(1) - this->wall_sensors->get_sensor_error(2); + } else if (observation.left) { + error = 2.0F * this->wall_sensors->get_sensor_error(1); + } else if (observation.right) { + error = 2.0F * this->wall_sensors->get_sensor_error(2); + } else { + error = 0.0F; + } + + const float response = this->pid.update(error, elapsed_time); + + return response * linear_speed / this->max_linear_speed; +} + +void FollowWall::reset() { + this->pid.reset(); +} +} // namespace micras::nav diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 1385991..f45c73f 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -85,6 +85,14 @@ class TWallSensors { */ float get_adc_reading(uint8_t sensor_index) const; + /** + * @brief Get the error of a sensor. + * + * @param sensor_index Index of the sensor. + * @return Error of the sensor. + */ + float get_sensor_error(uint8_t sensor_index) const; + /** * @brief Calibrate the wall sensors for a wall at the front. */ diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index a55aab6..f29ddf2 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -61,6 +61,11 @@ float TWallSensors::get_adc_reading(uint8_t sensor_index) const this->adc.get_max_reading(); } +template +float TWallSensors::get_sensor_error(uint8_t sensor_index) const { + return this->get_reading(sensor_index) - this->base_readings.at(sensor_index); +} + template void TWallSensors::calibrate_front_wall() { this->base_readings[0] = this->get_reading(0); From 0bd530a9a016043d671eb49c98f12e7381f36e9d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 17:16:40 -0300 Subject: [PATCH 292/561] =?UTF-8?q?=E2=9C=A8=20Add=20push=20function=20to?= =?UTF-8?q?=20action=20queuer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 2 ++ micras_nav/src/action_queuer.cpp | 31 +++++++++++++++++-- 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 1c8a541..02b474b 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -39,6 +39,8 @@ class ActionQueuer { const std::shared_ptr& pop(); + bool empty() const; + void recalculate(const std::map>& best_route); private: diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 6b2cea6..29534a7 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -31,7 +31,28 @@ ActionQueuer::ActionQueuer(Config config) : )}, action_queue{{start}} { } -void ActionQueuer::push(const GridPose& current_pose, const GridPose& target_pose) { } +void ActionQueuer::push(const GridPose& current_pose, const GridPose& target_pose) { + if (current_pose.front() == target_pose) { + this->action_queue.emplace(move_forward); + return; + } + + if (current_pose.turned_left().front() == target_pose) { + this->action_queue.emplace(turn_left); + return; + } + + if (current_pose.turned_right().front() == target_pose) { + this->action_queue.emplace(turn_right); + return; + } + + if (current_pose.turned_back().front() == target_pose) { + this->action_queue.emplace(turn_back); + this->action_queue.emplace(move_forward); + return; + } +} const std::shared_ptr& ActionQueuer::pop() { auto action = this->action_queue.front(); @@ -39,5 +60,11 @@ const std::shared_ptr& ActionQueuer::pop() { return action; } -void ActionQueuer::recalculate(const std::map>& best_route) { } +bool ActionQueuer::empty() const { + return this->action_queue.empty(); +} + +void ActionQueuer::recalculate(const std::map>& best_route) { + return; +} } // namespace micras::nav From a71f53bce3039ef9ba1a522f85e7706f29461829 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 17:17:28 -0300 Subject: [PATCH 293/561] =?UTF-8?q?=F0=9F=A5=85=20Fix=20remaining=20change?= =?UTF-8?q?s=20on=20maze?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 10 +--------- micras_nav/src/maze.cpp | 16 +++++++++------- 2 files changed, 10 insertions(+), 16 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 74fc45f..85043cf 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -45,7 +45,7 @@ class TMaze : public core::ISerializable { * @param position The current position of the robot. * @return The next point the robot should go to when exploring. */ - GridPose get_next_goal(const GridPoint& position, bool returning) const; + GridPoint get_next_goal(const GridPoint& position, bool returning) const; /** * @brief Check the type of wall following the robot can do. @@ -63,14 +63,6 @@ class TMaze : public core::ISerializable { */ bool finished(const GridPoint& position, bool returning) const; - /** - * @brief Check whether the robot has returned to the start. - * - * @param position The current position of the robot. - * @return True if the robot has returned to the start, false otherwise. - */ - bool returned(const GridPoint& position) const; - /** * @brief Calculate the best route to the goal using the current costmap. */ diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index dc627ca..8b537cc 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -65,16 +65,16 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ } template -GridPose TMaze::get_next_goal(const GridPoint& position, bool returning) const { +GridPoint TMaze::get_next_goal(const GridPoint& position, bool returning) const { uint16_t current_cost = this->get_cell(position).cost; if (returning) { if (this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { auto current = this->best_route.find(current_cost); - return {std::prev(current)->second.position, current->second.turned_back().orientation}; + return std::prev(current)->second.position; } - return get_current_exploration_goal(position); + return get_next_goal(position, false); } GridPoint next_position = position; @@ -87,11 +87,10 @@ GridPose TMaze::get_next_goal(const GridPoint& position, bool ret if (not this->has_wall({position, side}) and this->get_cell(front_position).cost < current_cost) { current_cost = this->get_cell(front_position).cost; next_position = front_position; - next_side = side; } } - return {next_position, next_side}; + return next_position; } template @@ -123,8 +122,11 @@ void TMaze::update_wall(const GridPose& pose, bool wall) { template bool TMaze::has_wall(const GridPose& pose) const { - return this->get_cell(pose.position).wall_count[pose.orientation] > - this->get_cell(pose.position).free_count[pose.orientation]; + if (pose.position.x >= width or pose.position.y >= height) { + return false; + } + + return this->get_cell(pose.position).walls[pose.orientation]; } template From d3cb863e3edfa3b2d62d001289125b4162278a96 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 17:18:05 -0300 Subject: [PATCH 294/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20deadzone=20from?= =?UTF-8?q?=20motor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/motor.hpp | 12 ------------ micras_proxy/src/motor.cpp | 12 +----------- 2 files changed, 1 insertion(+), 23 deletions(-) diff --git a/micras_proxy/include/micras/proxy/motor.hpp b/micras_proxy/include/micras/proxy/motor.hpp index 8d1b2a7..cf1bd01 100644 --- a/micras_proxy/include/micras/proxy/motor.hpp +++ b/micras_proxy/include/micras/proxy/motor.hpp @@ -19,8 +19,6 @@ class Motor { struct Config { hal::Pwm::Config backwards_pwm; hal::Pwm::Config forward_pwm; - float max_stopped_command; - float deadzone; }; /** @@ -47,16 +45,6 @@ class Motor { * @brief PWM object for controlling the motor in the forward direction. */ hal::Pwm forward_pwm; - - /** - * @brief Maximum command value for the motor to be considered stopped. - */ - float max_stopped_command; - - /** - * @brief Deadzone for the motor. - */ - float deadzone; }; } // namespace micras::proxy diff --git a/micras_proxy/src/motor.cpp b/micras_proxy/src/motor.cpp index 97b3afb..17c0a02 100644 --- a/micras_proxy/src/motor.cpp +++ b/micras_proxy/src/motor.cpp @@ -8,11 +8,7 @@ #include "micras/proxy/motor.hpp" namespace micras::proxy { -Motor::Motor(const Config& config) : - backwards_pwm{config.backwards_pwm}, - forward_pwm{config.forward_pwm}, - max_stopped_command{config.max_stopped_command}, - deadzone{config.deadzone} { +Motor::Motor(const Config& config) : backwards_pwm{config.backwards_pwm}, forward_pwm{config.forward_pwm} { this->set_command(0.0F); } @@ -20,12 +16,6 @@ void Motor::set_command(float command) { const bool is_positive = (command >= 0.0F); command = std::abs(command); - if (command <= max_stopped_command) { - command = 0.0F; - } else { - command = core::remap(command, 0.0F, 100.0F, this->deadzone, 100.0F); - } - if (is_positive) { this->forward_pwm.set_duty_cycle(command); this->backwards_pwm.set_duty_cycle(0.0F); From 65cb58d9056e268d67cd99b57c475973a18b8624 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 17:18:30 -0300 Subject: [PATCH 295/561] =?UTF-8?q?=E2=9C=A8=20Add=20Speed=20Controller?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/speed_controller.hpp | 72 +++++++++++++++++++ micras_nav/src/speed_controller.cpp | 44 ++++++++++++ 2 files changed, 116 insertions(+) create mode 100644 micras_nav/include/micras/nav/speed_controller.hpp create mode 100644 micras_nav/src/speed_controller.cpp diff --git a/micras_nav/include/micras/nav/speed_controller.hpp b/micras_nav/include/micras/nav/speed_controller.hpp new file mode 100644 index 0000000..17ddfd2 --- /dev/null +++ b/micras_nav/include/micras/nav/speed_controller.hpp @@ -0,0 +1,72 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_SPEED_CONTROLLER_HPP +#define MICRAS_NAV_SPEED_CONTROLLER_HPP + +#include "micras/core/pid_controller.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Class to calculate a command to go to a point. + */ +class SpeedController { +public: + /** + * @brief Configuration struct for the SpeedController class. + */ + struct Config { + struct FeedForward { + float bias; + float speed; + float acceleration; + }; + + core::PidController::Config linear_pid; + core::PidController::Config angular_pid; + FeedForward left_feed_forward; + FeedForward right_feed_forward; + }; + + /** + * @brief Construct a new SpeedController object. + * + * @param config The configuration for the SpeedController class. + */ + SpeedController(const Config& config); + + /** + * @brief Calculate the command to go to a point. + * + * @param state The current state of the robot. + * @param goal The goal point. + * @param elapsed_time The time elapsed since the last update. + * @return The command to go to the point. + */ + Twist action(const Twist& current_twist, const Twist& desired_twist, float elapsed_time); + + /** + * @brief Reset the PID controllers. + */ + void reset(); + +private: + /** + * @brief PID controller for stopping at the goal. + */ + core::PidController linear_pid; + + /** + * @brief PID controller for the orientation. + */ + core::PidController angular_pid; + + Config::FeedForward left_feed_forward; + + Config::FeedForward right_feed_forward; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_SPEED_CONTROLLER_HPP diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp new file mode 100644 index 0000000..8b67be9 --- /dev/null +++ b/micras_nav/src/speed_controller.cpp @@ -0,0 +1,44 @@ +/** + * @file + */ + +#include + +#include "micras/core/utils.hpp" +#include "micras/nav/speed_controller.hpp" + +namespace micras::nav { +SpeedController::SpeedController(const Config& config) : + linear_pid(config.linear_pid), + angular_pid(config.angular_pid), + left_feed_forward{config.left_feed_forward}, + right_feed_forward{config.right_feed_forward} { } + +Twist SpeedController::action(const Twist& current_twist, const Twist& desired_twist, float elapsed_time) { + this->linear_pid.set_setpoint(desired_twist.linear); + this->angular_pid.set_setpoint(desired_twist.angular); + + float linear_command = this->linear_pid.update(current_twist.linear, elapsed_time); + float angular_command = this->angular_pid.update(current_twist.angular, elapsed_time); + + float left_command = linear_command - angular_command; + float right_command = linear_command + angular_command; + + float current_left_speed = current_twist.linear - current_twist.angular; + float desired_left_speed = desired_twist.linear - desired_twist.angular; + float left_ff = this->left_feed_forward.bias + this->left_feed_forward.speed * desired_left_speed + + this->left_feed_forward.acceleration * (desired_left_speed - current_left_speed) / elapsed_time; + + float current_right_speed = current_twist.linear + current_twist.angular; + float desired_right_speed = desired_twist.linear + desired_twist.angular; + float right_ff = this->right_feed_forward.bias + this->right_feed_forward.speed * desired_right_speed + + this->right_feed_forward.acceleration * (desired_right_speed - current_right_speed) / elapsed_time; + + return {left_command + left_ff, right_command + right_ff}; +} + +void SpeedController::reset() { + this->linear_pid.reset(); + this->angular_pid.reset(); +} +} // namespace micras::nav From 9ee98800fe33ddd37f1347d93f0b3ea90554c870 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 14 Apr 2025 21:52:40 -0300 Subject: [PATCH 296/561] =?UTF-8?q?=F0=9F=A5=85=20Fix=20errors=20in=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/types.hpp | 12 --- .../include/micras/nav/action_queuer.hpp | 12 +-- .../include/micras/nav/actions/move.hpp | 6 +- .../include/micras/nav/actions/turn.hpp | 4 +- micras_nav/include/micras/nav/follow_wall.hpp | 2 +- micras_nav/include/micras/nav/maze.hpp | 18 +++-- .../include/micras/nav/speed_controller.hpp | 6 +- micras_nav/src/action_queuer.cpp | 20 +++-- micras_nav/src/maze.cpp | 73 +++++++++---------- micras_nav/src/speed_controller.cpp | 32 ++++---- micras_proxy/src/wall_sensors.cpp | 3 +- 11 files changed, 87 insertions(+), 101 deletions(-) diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index 0f5c334..326c76a 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -17,18 +17,6 @@ struct Observation { bool right{}; }; -/** - * @brief Types of wall following the robot is able to do. - */ -enum FollowWallType : uint8_t { - NONE = 0, - FRONT = 1, - LEFT = 2, - RIGHT = 3, - PARALLEL = 4, - BACK = 5, -}; - /** * @brief Possible objectives of the robot. */ diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 02b474b..97436b7 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -5,7 +5,7 @@ #ifndef MICRAS_NAV_ACTION_QUEUER_HPP #define MICRAS_NAV_ACTION_QUEUER_HPP -#include +#include #include #include @@ -33,21 +33,21 @@ class ActionQueuer { Dynamic solving; }; - ActionQueuer(Config config); + explicit ActionQueuer(Config config); - void push(const GridPose& current_pose, const GridPose& target_pose); + void push(const GridPose& current_pose, const GridPoint& target_position); - const std::shared_ptr& pop(); + std::shared_ptr pop(); bool empty() const; - void recalculate(const std::map>& best_route); + void recalculate(const std::list& best_route); private: float cell_size; float start_offset; Config::Dynamic exploring_params; - Config::Dynamic solving_params; + // Config::Dynamic solving_params; std::shared_ptr start; std::shared_ptr move_forward; diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index f14d815..8f2ff6d 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -35,14 +35,16 @@ class MoveAction : public Action { Twist twist{}; if (state.pose.position.distance({0.0F, 0.0F}) < this->decelerate_distance) { - twist = {std::sqrtf(this->start_speed_2 + this->max_acceleration_doubled * current_distance), 0.0F}; + twist = {std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance), 0.0F}; } twist = { - std::sqrtf(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), 0.0F + std::sqrt(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), 0.0F }; twist.linear = std::fminf(twist.linear, this->max_speed); + + return twist; } bool finished(const State& state) const override { diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 8454fb9..b62bad0 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -17,10 +17,10 @@ class TurnAction : public Action { public: TurnAction(float angle, float curve_radius, float max_centrifugal_acceleration) : angle{angle}, - linear_speed{std::sqrtf(max_centrifugal_acceleration * curve_radius)}, + linear_speed{std::sqrt(max_centrifugal_acceleration * curve_radius)}, angular_speed{std::copysignf(linear_speed / curve_radius, angle)} { } - Twist get_twist(const State& state) const override { return {linear_speed, angular_speed}; } + Twist get_twist(const State& /*state*/) const override { return {linear_speed, angular_speed}; } bool finished(const State& state) const override { return state.pose.orientation >= this->angle; } diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index cee2648..0e5fb0c 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -21,7 +21,7 @@ class FollowWall { */ struct Config { core::PidController::Config pid; - float max_linear_speed; + float max_linear_speed{}; }; /** diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 85043cf..338cfd2 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include "micras/core/serializable.hpp" @@ -23,13 +23,15 @@ namespace micras::nav { template class TMaze : public core::ISerializable { public: + struct Config { + GridPose start{}; + std::unordered_set goal; + }; + /** * @brief Construct a new Maze object. - * - * @param start The start pose of the robot. - * @param goal The goal points in the maze. */ - TMaze(const GridPose& start, const std::unordered_set& goal); + explicit TMaze(Config config); /** * @brief Update the maze walls with the current pose and new information. @@ -45,7 +47,7 @@ class TMaze : public core::ISerializable { * @param position The current position of the robot. * @return The next point the robot should go to when exploring. */ - GridPoint get_next_goal(const GridPoint& position, bool returning) const; + GridPose get_next_goal(const GridPoint& position, bool returning) const; /** * @brief Check the type of wall following the robot can do. @@ -73,7 +75,7 @@ class TMaze : public core::ISerializable { * * @return The best route to the goal. */ - const std::map>& get_best_route() const; + const std::list& get_best_route() const; /** * @brief Serialize the best route to the goal. @@ -154,7 +156,7 @@ class TMaze : public core::ISerializable { /** * @brief Current best found route to the goal. */ - std::map> best_route; + std::list best_route; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/speed_controller.hpp b/micras_nav/include/micras/nav/speed_controller.hpp index 17ddfd2..56ad68c 100644 --- a/micras_nav/include/micras/nav/speed_controller.hpp +++ b/micras_nav/include/micras/nav/speed_controller.hpp @@ -26,8 +26,8 @@ class SpeedController { core::PidController::Config linear_pid; core::PidController::Config angular_pid; - FeedForward left_feed_forward; - FeedForward right_feed_forward; + FeedForward left_feed_forward{}; + FeedForward right_feed_forward{}; }; /** @@ -35,7 +35,7 @@ class SpeedController { * * @param config The configuration for the SpeedController class. */ - SpeedController(const Config& config); + explicit SpeedController(const Config& config); /** * @brief Calculate the command to go to a point. diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 29534a7..3e0cc58 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -11,7 +11,7 @@ ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, start_offset{config.start_offset}, exploring_params{config.exploring}, - solving_params{config.solving}, + // solving_params{config.solving}, start{std::make_shared( cell_size - start_offset, 0.0F, exploring_params.max_speed, exploring_params.max_speed, exploring_params.max_acceleration, exploring_params.max_deceleration @@ -31,31 +31,31 @@ ActionQueuer::ActionQueuer(Config config) : )}, action_queue{{start}} { } -void ActionQueuer::push(const GridPose& current_pose, const GridPose& target_pose) { - if (current_pose.front() == target_pose) { +void ActionQueuer::push(const GridPose& current_pose, const GridPoint& target_position) { + if (current_pose.front().position == target_position) { this->action_queue.emplace(move_forward); return; } - if (current_pose.turned_left().front() == target_pose) { + if (current_pose.turned_left().front().position == target_position) { this->action_queue.emplace(turn_left); return; } - if (current_pose.turned_right().front() == target_pose) { + if (current_pose.turned_right().front().position == target_position) { this->action_queue.emplace(turn_right); return; } - if (current_pose.turned_back().front() == target_pose) { + if (current_pose.turned_back().front().position == target_position) { this->action_queue.emplace(turn_back); this->action_queue.emplace(move_forward); return; } } -const std::shared_ptr& ActionQueuer::pop() { - auto action = this->action_queue.front(); +std::shared_ptr ActionQueuer::pop() { + const auto& action = this->action_queue.front(); this->action_queue.pop(); return action; } @@ -64,7 +64,5 @@ bool ActionQueuer::empty() const { return this->action_queue.empty(); } -void ActionQueuer::recalculate(const std::map>& best_route) { - return; -} +void ActionQueuer::recalculate(const std::list& /*best_route*/) { } } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 8b537cc..21fcaf7 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -12,40 +12,37 @@ namespace micras::nav { template -TMaze::TMaze(const GridPose& start, const std::unordered_set& goal) : - start{start}, goal{goal} { +TMaze::TMaze(Config config) : start{config.start}, goal{config.goal} { for (uint8_t row = 0; row < height; row++) { - this->cells[row][0].wall_count[Side::LEFT] = 0xFFFF; - this->cells[row][width - 1].wall_count[Side::RIGHT] = 0xFFFF; + this->cells[row][0].walls[Side::LEFT] = true; + this->cells[row][width - 1].walls[Side::RIGHT] = true; } for (uint8_t col = 0; col < width; col++) { - this->cells[0][col].wall_count[Side::DOWN] = 0xFFFF; - this->cells[height - 1][col].wall_count[Side::UP] = 0xFFFF; + this->cells[0][col].walls[Side::DOWN] = true; + this->cells[height - 1][col].walls[Side::UP] = true; } - this->cells[start.position.y][start.position.x].wall_count[start.turned_right().orientation] = 0xFFFF; + this->cells[start.position.y][start.position.x].walls[start.turned_right().orientation] = true; GridPose right_pose = start.turned_right().front(); - this->get_cell(right_pose.position).wall_count[right_pose.turned_back().orientation] = 0xFFFF; + this->get_cell(right_pose.position).walls[right_pose.turned_back().orientation] = true; // Hardcoded walls at the end region if (width == 16 and height == 16) { - this->cells.at(7).at(7).wall_count[Side::DOWN] = 0xFFFF; - this->cells.at(7).at(7).wall_count[Side::LEFT] = 0xFFFF; - this->cells.at(6).at(7).wall_count[Side::UP] = 0xFFFF; - this->cells.at(7).at(6).wall_count[Side::RIGHT] = 0xFFFF; - this->cells.at(7).at(8).wall_count[Side::DOWN] = 0xFFFF; - this->cells.at(6).at(8).wall_count[Side::UP] = 0xFFFF; - this->cells.at(7).at(8).free_count[Side::RIGHT] = 0xFFFF; - this->cells.at(7).at(9).free_count[Side::LEFT] = 0xFFFF; - this->cells.at(8).at(7).wall_count[Side::LEFT] = 0xFFFF; - this->cells.at(8).at(7).wall_count[Side::UP] = 0xFFFF; - this->cells.at(8).at(6).wall_count[Side::RIGHT] = 0xFFFF; - this->cells.at(9).at(7).wall_count[Side::DOWN] = 0xFFFF; - this->cells.at(8).at(8).wall_count[Side::UP] = 0xFFFF; - this->cells.at(8).at(8).wall_count[Side::RIGHT] = 0xFFFF; - this->cells.at(9).at(8).wall_count[Side::DOWN] = 0xFFFF; - this->cells.at(8).at(9).wall_count[Side::LEFT] = 0xFFFF; + this->cells.at(7).at(7).walls[Side::DOWN] = true; + this->cells.at(7).at(7).walls[Side::LEFT] = true; + this->cells.at(6).at(7).walls[Side::UP] = true; + this->cells.at(7).at(6).walls[Side::RIGHT] = true; + this->cells.at(7).at(8).walls[Side::DOWN] = true; + this->cells.at(6).at(8).walls[Side::UP] = true; + this->cells.at(8).at(7).walls[Side::LEFT] = true; + this->cells.at(8).at(7).walls[Side::UP] = true; + this->cells.at(8).at(6).walls[Side::RIGHT] = true; + this->cells.at(9).at(7).walls[Side::DOWN] = true; + this->cells.at(8).at(8).walls[Side::UP] = true; + this->cells.at(8).at(8).walls[Side::RIGHT] = true; + this->cells.at(9).at(8).walls[Side::DOWN] = true; + this->cells.at(8).at(9).walls[Side::LEFT] = true; } for (const auto& position : this->goal) { @@ -57,23 +54,18 @@ TMaze::TMaze(const GridPose& start, const std::unordered_set void TMaze::update_walls(const GridPose& pose, const core::Observation& observation) { - this->update_wall(pose.turned_left(), information.left); - this->update_wall(pose, information.front); - this->update_wall(pose.turned_right(), information.right); + this->update_wall(pose.turned_left(), observation.left); + this->update_wall(pose, observation.front); + this->update_wall(pose.turned_right(), observation.right); this->calculate_costmap(); } template -GridPoint TMaze::get_next_goal(const GridPoint& position, bool returning) const { +GridPose TMaze::get_next_goal(const GridPoint& position, bool returning) const { uint16_t current_cost = this->get_cell(position).cost; if (returning) { - if (this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { - auto current = this->best_route.find(current_cost); - return std::prev(current)->second.position; - } - return get_next_goal(position, false); } @@ -87,10 +79,11 @@ GridPoint TMaze::get_next_goal(const GridPoint& position, bool re if (not this->has_wall({position, side}) and this->get_cell(front_position).cost < current_cost) { current_cost = this->get_cell(front_position).cost; next_position = front_position; + next_side = side; } } - return next_position; + return {next_position, next_side}; } template @@ -117,7 +110,7 @@ void TMaze::update_wall(const GridPose& pose, bool wall) { return; } - this->get_cell(front_pose.position).wall_count[pose.turned_back().orientation] = wall; + this->get_cell(front_pose.position).walls[pose.turned_back().orientation] = wall; } template @@ -143,16 +136,16 @@ template void TMaze::calculate_best_route() { GridPose current_pose = this->start; this->best_route.clear(); - this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start); + this->best_route.emplace_back(this->start); while (not this->goal.contains(current_pose.position)) { current_pose = this->get_current_exploration_goal(current_pose.position); - this->best_route.try_emplace(this->get_cell(current_pose.position).cost, current_pose); + this->best_route.emplace_back(current_pose); } } template -const std::map>& TMaze::get_best_route() const { +const std::list& TMaze::get_best_route() const { return this->best_route; } @@ -198,7 +191,7 @@ std::vector TMaze::serialize() const { std::vector buffer; buffer.reserve(3 * this->best_route.size()); - for (const auto& [cost, grid_pose] : this->best_route) { + for (const auto& grid_pose : this->best_route) { buffer.emplace_back(grid_pose.position.x); buffer.emplace_back(grid_pose.position.y); buffer.emplace_back(grid_pose.orientation); @@ -214,7 +207,7 @@ void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { for (uint32_t i = 0; i < size; i += 3) { this->best_route.emplace_back( // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) - Point::from_grid({buffer[i], buffer[i + 1]}, this->cell_size), static_cast(2 * buffer[i + 2]) + GridPoint{buffer[i], buffer[i + 1]}, static_cast(buffer[i + 2]) ); } } diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp index 8b67be9..784ce1f 100644 --- a/micras_nav/src/speed_controller.cpp +++ b/micras_nav/src/speed_controller.cpp @@ -18,21 +18,23 @@ Twist SpeedController::action(const Twist& current_twist, const Twist& desired_t this->linear_pid.set_setpoint(desired_twist.linear); this->angular_pid.set_setpoint(desired_twist.angular); - float linear_command = this->linear_pid.update(current_twist.linear, elapsed_time); - float angular_command = this->angular_pid.update(current_twist.angular, elapsed_time); - - float left_command = linear_command - angular_command; - float right_command = linear_command + angular_command; - - float current_left_speed = current_twist.linear - current_twist.angular; - float desired_left_speed = desired_twist.linear - desired_twist.angular; - float left_ff = this->left_feed_forward.bias + this->left_feed_forward.speed * desired_left_speed + - this->left_feed_forward.acceleration * (desired_left_speed - current_left_speed) / elapsed_time; - - float current_right_speed = current_twist.linear + current_twist.angular; - float desired_right_speed = desired_twist.linear + desired_twist.angular; - float right_ff = this->right_feed_forward.bias + this->right_feed_forward.speed * desired_right_speed + - this->right_feed_forward.acceleration * (desired_right_speed - current_right_speed) / elapsed_time; + const float linear_command = this->linear_pid.update(current_twist.linear, elapsed_time); + const float angular_command = this->angular_pid.update(current_twist.angular, elapsed_time); + + const float left_command = linear_command - angular_command; + const float right_command = linear_command + angular_command; + + const float current_left_speed = current_twist.linear - current_twist.angular; + const float desired_left_speed = desired_twist.linear - desired_twist.angular; + const float left_ff = + this->left_feed_forward.bias + this->left_feed_forward.speed * desired_left_speed + + this->left_feed_forward.acceleration * (desired_left_speed - current_left_speed) / elapsed_time; + + const float current_right_speed = current_twist.linear + current_twist.angular; + const float desired_right_speed = desired_twist.linear + desired_twist.angular; + const float right_ff = + this->right_feed_forward.bias + this->right_feed_forward.speed * desired_right_speed + + this->right_feed_forward.acceleration * (desired_right_speed - current_right_speed) / elapsed_time; return {left_command + left_ff, right_command + right_ff}; } diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index f29ddf2..431cef2 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -15,7 +15,8 @@ TWallSensors::TWallSensors(const Config& config) : led_0_pwm{config.led_0_pwm}, led_1_pwm{config.led_1_pwm}, filters{core::make_array(config.filter_cutoff)}, - uncertainty{config.base_readings} { + base_readings{config.base_readings}, + uncertainty{config.uncertainty} { this->adc.start_dma(this->buffer); this->turn_off(); } From bbda3281167709145d3d3b96b71f3e2c91b2ab4e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 15 Apr 2025 00:39:42 -0300 Subject: [PATCH 297/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Initial=20constant?= =?UTF-8?q?s=20calibration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/odometry.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 673f952..a97c8c6 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -53,6 +53,7 @@ void Odometry::update(float elapsed_time) { void Odometry::reset() { this->left_last_position = this->left_rotary_sensor->get_position(); this->right_last_position = this->right_rotary_sensor->get_position(); + this->state = {{{0.0F, 0.0F}, 0.0F}, {0.0F, 0.0F}}; } const nav::State& Odometry::get_state() const { From f1601ce66457befe55900890194809fd809388d5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 15 Apr 2025 01:28:57 -0300 Subject: [PATCH 298/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20command=20set=20on?= =?UTF-8?q?=20motors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/speed_controller.hpp | 4 +++- micras_nav/src/speed_controller.cpp | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/micras_nav/include/micras/nav/speed_controller.hpp b/micras_nav/include/micras/nav/speed_controller.hpp index 56ad68c..659a013 100644 --- a/micras_nav/include/micras/nav/speed_controller.hpp +++ b/micras_nav/include/micras/nav/speed_controller.hpp @@ -5,6 +5,8 @@ #ifndef MICRAS_NAV_SPEED_CONTROLLER_HPP #define MICRAS_NAV_SPEED_CONTROLLER_HPP +#include + #include "micras/core/pid_controller.hpp" #include "micras/nav/state.hpp" @@ -45,7 +47,7 @@ class SpeedController { * @param elapsed_time The time elapsed since the last update. * @return The command to go to the point. */ - Twist action(const Twist& current_twist, const Twist& desired_twist, float elapsed_time); + std::pair action(const Twist& current_twist, const Twist& desired_twist, float elapsed_time); /** * @brief Reset the PID controllers. diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp index 784ce1f..b00f891 100644 --- a/micras_nav/src/speed_controller.cpp +++ b/micras_nav/src/speed_controller.cpp @@ -14,7 +14,8 @@ SpeedController::SpeedController(const Config& config) : left_feed_forward{config.left_feed_forward}, right_feed_forward{config.right_feed_forward} { } -Twist SpeedController::action(const Twist& current_twist, const Twist& desired_twist, float elapsed_time) { +std::pair + SpeedController::action(const Twist& current_twist, const Twist& desired_twist, float elapsed_time) { this->linear_pid.set_setpoint(desired_twist.linear); this->angular_pid.set_setpoint(desired_twist.angular); From da56792962848f5839b90f65ec76689ab7d1d953 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 15 Apr 2025 02:05:03 -0300 Subject: [PATCH 299/561] =?UTF-8?q?=F0=9F=90=9B=20Keep=20discovered=20wall?= =?UTF-8?q?s=20in=20maze?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 21fcaf7..539477c 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -102,7 +102,7 @@ void TMaze::update_wall(const GridPose& pose, bool wall) { return; } - this->get_cell(pose.position).walls[pose.orientation] = wall; + this->get_cell(pose.position).walls[pose.orientation] |= wall; GridPose front_pose = pose.front(); @@ -110,15 +110,11 @@ void TMaze::update_wall(const GridPose& pose, bool wall) { return; } - this->get_cell(front_pose.position).walls[pose.turned_back().orientation] = wall; + this->get_cell(front_pose.position).walls[pose.turned_back().orientation] |= wall; } template bool TMaze::has_wall(const GridPose& pose) const { - if (pose.position.x >= width or pose.position.y >= height) { - return false; - } - return this->get_cell(pose.position).walls[pose.orientation]; } From a452897d13cc9a388372d54a91ca04f08f23cfba Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 15 Apr 2025 02:18:04 -0300 Subject: [PATCH 300/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20angle=20check=20on?= =?UTF-8?q?=20turn=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/turn.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index b62bad0..1ae4705 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -22,7 +22,9 @@ class TurnAction : public Action { Twist get_twist(const State& /*state*/) const override { return {linear_speed, angular_speed}; } - bool finished(const State& state) const override { return state.pose.orientation >= this->angle; } + bool finished(const State& state) const override { + return std::abs(state.pose.orientation) >= std::abs(this->angle); + } private: float angle; From 97ebe0b6e2510d3529cb8f6a0abb1cd79fc4604f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 22 Apr 2025 09:38:24 -0300 Subject: [PATCH 301/561] =?UTF-8?q?=E2=8F=AA=20Add=20deadzone=20back=20to?= =?UTF-8?q?=20motors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/motor.hpp | 12 ++++++++++++ micras_proxy/src/motor.cpp | 12 +++++++++++- 2 files changed, 23 insertions(+), 1 deletion(-) diff --git a/micras_proxy/include/micras/proxy/motor.hpp b/micras_proxy/include/micras/proxy/motor.hpp index cf1bd01..8d1b2a7 100644 --- a/micras_proxy/include/micras/proxy/motor.hpp +++ b/micras_proxy/include/micras/proxy/motor.hpp @@ -19,6 +19,8 @@ class Motor { struct Config { hal::Pwm::Config backwards_pwm; hal::Pwm::Config forward_pwm; + float max_stopped_command; + float deadzone; }; /** @@ -45,6 +47,16 @@ class Motor { * @brief PWM object for controlling the motor in the forward direction. */ hal::Pwm forward_pwm; + + /** + * @brief Maximum command value for the motor to be considered stopped. + */ + float max_stopped_command; + + /** + * @brief Deadzone for the motor. + */ + float deadzone; }; } // namespace micras::proxy diff --git a/micras_proxy/src/motor.cpp b/micras_proxy/src/motor.cpp index 17c0a02..97b3afb 100644 --- a/micras_proxy/src/motor.cpp +++ b/micras_proxy/src/motor.cpp @@ -8,7 +8,11 @@ #include "micras/proxy/motor.hpp" namespace micras::proxy { -Motor::Motor(const Config& config) : backwards_pwm{config.backwards_pwm}, forward_pwm{config.forward_pwm} { +Motor::Motor(const Config& config) : + backwards_pwm{config.backwards_pwm}, + forward_pwm{config.forward_pwm}, + max_stopped_command{config.max_stopped_command}, + deadzone{config.deadzone} { this->set_command(0.0F); } @@ -16,6 +20,12 @@ void Motor::set_command(float command) { const bool is_positive = (command >= 0.0F); command = std::abs(command); + if (command <= max_stopped_command) { + command = 0.0F; + } else { + command = core::remap(command, 0.0F, 100.0F, this->deadzone, 100.0F); + } + if (is_positive) { this->forward_pwm.set_duty_cycle(command); this->backwards_pwm.set_duty_cycle(0.0F); From 4be57e2ef93f451a2adb4eb6d8ff58d6f138e9d4 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 22 Apr 2025 09:40:14 -0300 Subject: [PATCH 302/561] =?UTF-8?q?=E2=9C=A8=20Add=20angular=20acceleratio?= =?UTF-8?q?n=20ramp=20to=20turn=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/actions/turn.hpp | 36 ++++++++++++++++--- micras_nav/src/action_queuer.cpp | 19 ++++++---- 2 files changed, 44 insertions(+), 11 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 1ae4705..4852ccb 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -5,22 +5,49 @@ #ifndef MICRAS_NAV_TURN_ACTION_HPP #define MICRAS_NAV_TURN_ACTION_HPP +#include #include #include "micras/nav/actions/base.hpp" namespace micras::nav { +static constexpr float correction_factor{0.95F}; + /** * @brief Class to follow the side walls using a PID controller. */ class TurnAction : public Action { public: - TurnAction(float angle, float curve_radius, float max_centrifugal_acceleration) : + TurnAction(float angle, float curve_radius, float max_centrifugal_acceleration, float max_angular_acceleration) : angle{angle}, linear_speed{std::sqrt(max_centrifugal_acceleration * curve_radius)}, - angular_speed{std::copysignf(linear_speed / curve_radius, angle)} { } + acceleration{max_angular_acceleration}, + max_speed{ + correction_factor * max_angular_acceleration * + (curve_radius / linear_speed - + std::sqrt( + std::pow(curve_radius / linear_speed, 2.0F) - 2.0F / (correction_factor * max_angular_acceleration) + )) + } { } + + Twist get_twist(const State& state) const override { + Twist twist{}; + const float current_orientation = std::abs(state.pose.orientation); + + if (current_orientation < std::abs(this->angle)) { + twist = { + linear_speed, std::sqrt(2.0F * this->acceleration * (current_orientation + this->acceleration / 2e6F)) + }; + } else { + twist = { + linear_speed, std::sqrt(2.0F * this->acceleration * (std::abs(this->angle) - current_orientation)) + }; + } - Twist get_twist(const State& /*state*/) const override { return {linear_speed, angular_speed}; } + twist.angular = std::copysign(std::clamp(twist.angular, -this->max_speed, this->max_speed), this->angle); + + return twist; + } bool finished(const State& state) const override { return std::abs(state.pose.orientation) >= std::abs(this->angle); @@ -29,7 +56,8 @@ class TurnAction : public Action { private: float angle; float linear_speed; - float angular_speed; + float acceleration; + float max_speed; }; } // namespace micras::nav diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 3e0cc58..d12c902 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -13,21 +13,26 @@ ActionQueuer::ActionQueuer(Config config) : exploring_params{config.exploring}, // solving_params{config.solving}, start{std::make_shared( - cell_size - start_offset, 0.0F, exploring_params.max_speed, exploring_params.max_speed, - exploring_params.max_acceleration, exploring_params.max_deceleration + cell_size - start_offset, 0.001F * exploring_params.max_linear_acceleration, exploring_params.max_linear_speed, + exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, + exploring_params.max_linear_deceleration )}, move_forward{std::make_shared( - cell_size, exploring_params.max_speed, exploring_params.max_speed, exploring_params.max_speed, - exploring_params.max_acceleration, exploring_params.max_deceleration + cell_size, exploring_params.max_linear_speed, exploring_params.max_linear_speed, + exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, + exploring_params.max_linear_deceleration )}, turn_left{std::make_shared( - std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration + std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration, + exploring_params.max_angular_acceleration )}, turn_right{std::make_shared( - -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration + -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration, + exploring_params.max_angular_acceleration )}, turn_back{std::make_shared( - std::numbers::pi_v, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration + std::numbers::pi_v, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration, + exploring_params.max_angular_acceleration )}, action_queue{{start}} { } From 1a52ae13287a3d830e760b7fe6f16ef4f6ca72a4 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 22 Apr 2025 09:41:27 -0300 Subject: [PATCH 303/561] =?UTF-8?q?=E2=9C=A8=20Add=20feed=20forward=20for?= =?UTF-8?q?=20linear=20and=20angular=20speeds?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 7 ++-- .../include/micras/nav/actions/move.hpp | 11 +++--- .../include/micras/nav/speed_controller.hpp | 17 ++++++++-- micras_nav/src/speed_controller.cpp | 34 ++++++++++++------- 4 files changed, 45 insertions(+), 24 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 97436b7..c099741 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -20,11 +20,12 @@ class ActionQueuer { public: struct Config { struct Dynamic { - float max_speed; - float max_acceleration; - float max_deceleration; + float max_linear_speed; + float max_linear_acceleration; + float max_linear_deceleration; float curve_radius; float max_centrifugal_acceleration; + float max_angular_acceleration; }; float cell_size; diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 8f2ff6d..beadd63 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -34,14 +34,15 @@ class MoveAction : public Action { const float current_distance = state.pose.position.distance({0.0F, 0.0F}); Twist twist{}; - if (state.pose.position.distance({0.0F, 0.0F}) < this->decelerate_distance) { + if (current_distance < this->decelerate_distance) { twist = {std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance), 0.0F}; + } else { + twist = { + std::sqrt(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), + 0.0F + }; } - twist = { - std::sqrt(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), 0.0F - }; - twist.linear = std::fminf(twist.linear, this->max_speed); return twist; diff --git a/micras_nav/include/micras/nav/speed_controller.hpp b/micras_nav/include/micras/nav/speed_controller.hpp index 659a013..dd56ab1 100644 --- a/micras_nav/include/micras/nav/speed_controller.hpp +++ b/micras_nav/include/micras/nav/speed_controller.hpp @@ -21,11 +21,14 @@ class SpeedController { */ struct Config { struct FeedForward { - float bias; - float speed; - float acceleration; + float linear_speed; + float linear_acceleration; + float angular_speed; + float angular_acceleration; }; + float max_linear_acceleration{}; + float max_angular_acceleration{}; core::PidController::Config linear_pid; core::PidController::Config angular_pid; FeedForward left_feed_forward{}; @@ -55,6 +58,14 @@ class SpeedController { void reset(); private: + static float feed_forward(const Twist& speed, const Twist& acceleration, const Config::FeedForward& config); + + float max_linear_acceleration; + float max_angular_acceleration; + + float last_linear_speed{}; + float last_angular_speed{}; + /** * @brief PID controller for stopping at the goal. */ diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp index b00f891..e68d57d 100644 --- a/micras_nav/src/speed_controller.cpp +++ b/micras_nav/src/speed_controller.cpp @@ -2,6 +2,7 @@ * @file */ +#include #include #include "micras/core/utils.hpp" @@ -9,6 +10,8 @@ namespace micras::nav { SpeedController::SpeedController(const Config& config) : + max_linear_acceleration{config.max_linear_acceleration}, + max_angular_acceleration{config.max_angular_acceleration}, linear_pid(config.linear_pid), angular_pid(config.angular_pid), left_feed_forward{config.left_feed_forward}, @@ -22,22 +25,27 @@ std::pair const float linear_command = this->linear_pid.update(current_twist.linear, elapsed_time); const float angular_command = this->angular_pid.update(current_twist.angular, elapsed_time); - const float left_command = linear_command - angular_command; - const float right_command = linear_command + angular_command; + const float linear_acceleration = (desired_twist.linear - this->last_linear_speed) / elapsed_time; + const float angular_acceleration = (desired_twist.angular - this->last_angular_speed) / elapsed_time; + const Twist acceleration_twist{ + std::clamp(linear_acceleration, -this->max_linear_acceleration, this->max_linear_acceleration), + std::clamp(angular_acceleration, -this->max_angular_acceleration, this->max_angular_acceleration) + }; - const float current_left_speed = current_twist.linear - current_twist.angular; - const float desired_left_speed = desired_twist.linear - desired_twist.angular; - const float left_ff = - this->left_feed_forward.bias + this->left_feed_forward.speed * desired_left_speed + - this->left_feed_forward.acceleration * (desired_left_speed - current_left_speed) / elapsed_time; + this->last_linear_speed = desired_twist.linear; + this->last_angular_speed = desired_twist.angular; - const float current_right_speed = current_twist.linear + current_twist.angular; - const float desired_right_speed = desired_twist.linear + desired_twist.angular; - const float right_ff = - this->right_feed_forward.bias + this->right_feed_forward.speed * desired_right_speed + - this->right_feed_forward.acceleration * (desired_right_speed - current_right_speed) / elapsed_time; + const float left_feed_forward = feed_forward(desired_twist, acceleration_twist, this->left_feed_forward); + const float right_feed_forward = feed_forward(desired_twist, acceleration_twist, this->right_feed_forward); - return {left_command + left_ff, right_command + right_ff}; + return { + left_feed_forward + linear_command - angular_command, right_feed_forward + linear_command + angular_command + }; +} + +float SpeedController::feed_forward(const Twist& speed, const Twist& acceleration, const Config::FeedForward& config) { + return config.linear_speed * speed.linear + config.linear_acceleration * acceleration.linear + + config.angular_speed * speed.angular + config.angular_acceleration * acceleration.angular; } void SpeedController::reset() { From 28c8591ae1ec64774e08e16646224b74abeb68fb Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 22 Apr 2025 09:41:52 -0300 Subject: [PATCH 304/561] =?UTF-8?q?=E2=9C=A8=20Add=20initialized=20paramet?= =?UTF-8?q?er=20to=20imu?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/imu.hpp | 18 +++++++++++------- micras_proxy/src/imu.cpp | 5 +++++ 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 5ce0d54..37afcf9 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -49,13 +49,6 @@ class Imu { */ explicit Imu(const Config& config); - /** - * @brief Check the IMU device. - * - * @return True if the device is correct, false otherwise. - */ - bool check_whoami(); - /** * @brief Update the IMU data. */ @@ -82,7 +75,16 @@ class Imu { */ void calibrate(); + bool was_initialized() const; + private: + /** + * @brief Check the IMU device. + * + * @return True if the device is correct, false otherwise. + */ + bool check_whoami(); + /** * @brief Read data from the IMU. * @@ -150,6 +152,8 @@ class Imu { * @brief Flag to check if the IMU is calibrated. */ bool calibrated{}; + + bool initialized{}; }; } // namespace micras::proxy diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 0733cd7..305336e 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -48,6 +48,7 @@ Imu::Imu(const Config& config) : lsm6dsv_filt_gy_lp1_bandwidth_set(&(this->dev_ctx), config.gyroscope_filter); lsm6dsv_filt_xl_lp2_set(&(this->dev_ctx), PROPERTY_ENABLE); lsm6dsv_filt_xl_lp2_bandwidth_set(&(this->dev_ctx), config.accelerometer_filter); + this->initialized = true; } bool Imu::check_whoami() { @@ -142,4 +143,8 @@ int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint void Imu::calibrate() { this->calibrated = true; } + +bool Imu::was_initialized() const { + return this->initialized; +} } // namespace micras::proxy From 2b21d1058e949f6ca326932e09e0518fe62e3248 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 22 Apr 2025 09:42:33 -0300 Subject: [PATCH 305/561] =?UTF-8?q?=E2=9C=A8=20Add=20disturbance=20to=20wa?= =?UTF-8?q?ll=20sensors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/wall_sensors.hpp | 2 +- micras_proxy/src/wall_sensors.cpp | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index f45c73f..48fdbf4 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -60,7 +60,7 @@ class TWallSensors { * @param sensor_index Index of the sensor. * @return True if the sensor detects a wall, false otherwise. */ - bool get_wall(uint8_t sensor_index) const; + bool get_wall(uint8_t sensor_index, bool disturbed = false) const; /** * @brief Get the observations from all sensors. diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index 431cef2..b4f2949 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -41,13 +41,18 @@ void TWallSensors::update() { } template -bool TWallSensors::get_wall(uint8_t sensor_index) const { - return this->filters.at(sensor_index).get_last() > this->base_readings.at(sensor_index) * this->uncertainty; +bool TWallSensors::get_wall(uint8_t sensor_index, bool disturbed) const { + return this->filters.at(sensor_index).get_last() > + this->base_readings.at(sensor_index) * this->uncertainty * (disturbed ? 1.2F : 1.0F); } template core::Observation TWallSensors::get_observation() const { - return {this->get_wall(1), this->get_wall(0) && this->get_wall(3), this->get_wall(2)}; + if (this->get_wall(0) && this->get_wall(3)) { + return {this->get_wall(1, true), true, this->get_wall(2, true)}; + } + + return {this->get_wall(1), false, this->get_wall(2)}; } template From 6a1accfd06f729e00c0716073f4a5c02b3afb569 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 22 Apr 2025 22:22:09 -0300 Subject: [PATCH 306/561] =?UTF-8?q?=F0=9F=8E=A8=20Update=20button=20proxy?= =?UTF-8?q?=20to=20update-getter=20pattern?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/button.hpp | 18 ++++++++---- micras_proxy/src/button.cpp | 31 ++++++++++++-------- 2 files changed, 31 insertions(+), 18 deletions(-) diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index 8a0578d..8f68af1 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -57,14 +57,19 @@ class Button { * * @return True if button is pressed, false otherwise. */ - bool is_pressed(); + bool is_pressed() const; /** * @brief Get button status. * * @return Current button status. */ - Status get_status(); + Status get_status() const; + + /** + * @brief Update the status of the button. + */ + void update(); private: /** @@ -76,10 +81,8 @@ class Button { /** * @brief Update button state. - * - * @return Current state of the button. */ - bool update_state(); + void update_state(); /** * @brief Check if button was just pressed. @@ -138,6 +141,11 @@ class Button { * @brief Flag to know if button is being pressed. */ bool current_state{false}; + + /** + * @brief Current status of the button. + */ + Status current_status{NO_PRESS}; }; } // namespace micras::proxy diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp index 1e83642..162e547 100644 --- a/micras_proxy/src/button.cpp +++ b/micras_proxy/src/button.cpp @@ -12,36 +12,43 @@ Button::Button(const Config& config) : gpio{config.gpio}, pull_resistor{config.pull_resistor} { } -bool Button::is_pressed() { - return this->update_state(); +bool Button::is_pressed() const { + return this->current_state; } -Button::Status Button::get_status() { - this->previous_state = this->current_state; - this->current_state = this->update_state(); +Button::Status Button::get_status() const { + return this->current_status; +} + +void Button::update() { + this->update_state(); if (this->is_rising_edge()) { this->status_stopwatch.reset_ms(); } else if (this->is_falling_edge()) { if (this->status_stopwatch.elapsed_time_ms() > extra_long_press_delay) { - return EXTRA_LONG_PRESS; + this->current_status = EXTRA_LONG_PRESS; + return; } if (this->status_stopwatch.elapsed_time_ms() > long_press_delay) { - return LONG_PRESS; + this->current_status = LONG_PRESS; + return; } - return SHORT_PRESS; + this->current_status = SHORT_PRESS; + return; } - return NO_PRESS; + this->current_status = NO_PRESS; } bool Button::get_raw_reading() const { return this->gpio.read() == static_cast(this->pull_resistor); } -bool Button::update_state() { +void Button::update_state() { + this->previous_state = this->current_state; const bool raw_reading = this->get_raw_reading(); if ((raw_reading != this->current_state) and not this->is_debouncing) { @@ -52,10 +59,8 @@ bool Button::update_state() { this->is_debouncing = false; } } else { - return raw_reading; + this->current_state = raw_reading; } - - return this->current_state; } bool Button::is_rising_edge() const { From bf2b62d0e35a2d9b71409118d0560c761ae9a9b6 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 24 Apr 2025 11:19:37 -0300 Subject: [PATCH 307/561] =?UTF-8?q?=F0=9F=94=A5=20Delete=20look=20at=20poi?= =?UTF-8?q?nt=20test?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/button.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp index 162e547..ae0dab2 100644 --- a/micras_proxy/src/button.cpp +++ b/micras_proxy/src/button.cpp @@ -21,6 +21,7 @@ Button::Status Button::get_status() const { } void Button::update() { + this->previous_state = this->current_state; this->update_state(); if (this->is_rising_edge()) { @@ -48,7 +49,6 @@ bool Button::get_raw_reading() const { } void Button::update_state() { - this->previous_state = this->current_state; const bool raw_reading = this->get_raw_reading(); if ((raw_reading != this->current_state) and not this->is_debouncing) { From 7b2da9ec6e4fb20591d95601d81d1d52f026e989 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Apr 2025 19:02:22 -0300 Subject: [PATCH 308/561] =?UTF-8?q?=E2=9C=A8=20Add=20relative=20state?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/actions/base.hpp | 6 ++-- .../include/micras/nav/actions/move.hpp | 10 +++--- .../include/micras/nav/actions/turn.hpp | 8 ++--- micras_nav/include/micras/nav/state.hpp | 33 +++++++++++++++++++ micras_nav/src/state.cpp | 14 ++++++++ 5 files changed, 59 insertions(+), 12 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index a9dbf47..3d938c7 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -18,9 +18,11 @@ class Action { */ virtual ~Action() = default; - virtual Twist get_twist(const State& state) const = 0; + virtual Twist get_twist(const Pose& pose) const = 0; - virtual bool finished(const State& state) const = 0; + virtual bool finished(const Pose& pose) const = 0; + + static constexpr bool allow_follow_wall{}; protected: /** diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index beadd63..ad36eec 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -30,8 +30,8 @@ class MoveAction : public Action { (max_acceleration_doubled + max_deceleration_doubled) } { } - Twist get_twist(const State& state) const override { - const float current_distance = state.pose.position.distance({0.0F, 0.0F}); + Twist get_twist(const Pose& pose) const override { + const float current_distance = pose.position.distance({0.0F, 0.0F}); Twist twist{}; if (current_distance < this->decelerate_distance) { @@ -48,9 +48,9 @@ class MoveAction : public Action { return twist; } - bool finished(const State& state) const override { - return state.pose.position.distance({0.0F, 0.0F}) >= this->distance; - } + bool finished(const Pose& pose) const override { return pose.position.distance({0.0F, 0.0F}) >= this->distance; } + + static constexpr bool allow_follow_wall{true}; private: float distance; diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 4852ccb..adaa007 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -30,9 +30,9 @@ class TurnAction : public Action { )) } { } - Twist get_twist(const State& state) const override { + Twist get_twist(const Pose& pose) const override { Twist twist{}; - const float current_orientation = std::abs(state.pose.orientation); + const float current_orientation = std::abs(pose.orientation); if (current_orientation < std::abs(this->angle)) { twist = { @@ -49,9 +49,7 @@ class TurnAction : public Action { return twist; } - bool finished(const State& state) const override { - return std::abs(state.pose.orientation) >= std::abs(this->angle); - } + bool finished(const Pose& pose) const override { return std::abs(pose.orientation) >= std::abs(this->angle); } private: float angle; diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index 7cc62e9..d849eb6 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -171,6 +171,39 @@ struct State { */ Twist velocity; }; + +class RelativePose { +public: + /** + * @brief Construct a new Relative Pose object. + * + * @param absolute_pose The absolute pose to be used as a reference. + */ + RelativePose(const Pose& absolute_pose); + + /** + * @brief Get the relative pose. + * + * @return The pose relative to the reference. + */ + Pose get() const; + + /** + * @brief Reset the reference to the current absolute pose. + */ + void reset_reference(); + +private: + /** + * @brief A reference to the absolute pose. + */ + const Pose& absolute_pose; + + /** + * @brief The reference pose to be used for calculations. + */ + Pose reference_pose{}; +}; } // namespace micras::nav #endif // MICRAS_NAV_POSE_HPP diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 5190c58..ba6e2d9 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -5,6 +5,7 @@ #include #include +#include "micras/core/utils.hpp" #include "micras/nav/state.hpp" namespace micras::nav { @@ -97,4 +98,17 @@ Point Pose::to_cell(float cell_size) const { return {cell_alignment, cell_advance}; } + +RelativePose::RelativePose(const Pose& absolute_pose) : absolute_pose{absolute_pose} { } + +Pose RelativePose::get() const { + return { + this->absolute_pose.position - this->reference_pose.position, + core::assert_angle(this->absolute_pose.orientation - this->reference_pose.orientation) + }; +} + +void RelativePose::reset_reference() { + this->reference_pose = this->absolute_pose; +} } // namespace micras::nav From 7d6746a626cf150ccde1d1d02378803be2b8654a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Apr 2025 21:52:55 -0300 Subject: [PATCH 309/561] =?UTF-8?q?=E2=9C=A8=20Add=20post=20detection=20to?= =?UTF-8?q?=20follow=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/actions/turn.hpp | 16 ++-- micras_nav/include/micras/nav/follow_wall.hpp | 30 ++++++- micras_nav/src/action_queuer.cpp | 11 ++- micras_nav/src/follow_wall.cpp | 83 +++++++++++++++++-- 4 files changed, 116 insertions(+), 24 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index adaa007..91fc0f9 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -18,16 +18,18 @@ static constexpr float correction_factor{0.95F}; */ class TurnAction : public Action { public: - TurnAction(float angle, float curve_radius, float max_centrifugal_acceleration, float max_angular_acceleration) : + TurnAction(float angle, float curve_radius, float linear_speed, float max_angular_acceleration) : angle{angle}, - linear_speed{std::sqrt(max_centrifugal_acceleration * curve_radius)}, + linear_speed{linear_speed}, acceleration{max_angular_acceleration}, max_speed{ - correction_factor * max_angular_acceleration * - (curve_radius / linear_speed - - std::sqrt( - std::pow(curve_radius / linear_speed, 2.0F) - 2.0F / (correction_factor * max_angular_acceleration) - )) + (linear_speed == 0.0F) ? + (max_angular_acceleration * 0.1F) : + (correction_factor * max_angular_acceleration * + (curve_radius / linear_speed - std::sqrt( + std::pow(curve_radius / linear_speed, 2.0F) - + 2.0F / (correction_factor * max_angular_acceleration) + ))) } { } Twist get_twist(const Pose& pose) const override { diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 0e5fb0c..479a09d 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -8,6 +8,7 @@ #include #include "micras/core/pid_controller.hpp" +#include "micras/nav/state.hpp" #include "micras/proxy/wall_sensors.hpp" namespace micras::nav { @@ -22,6 +23,9 @@ class FollowWall { struct Config { core::PidController::Config pid; float max_linear_speed{}; + float post_threshold{}; + float cell_size{}; + float post_margin{}; }; /** @@ -30,7 +34,9 @@ class FollowWall { * @param wall_sensors The wall sensors of the robot. * @param config The configuration for the FollowWall class. */ - FollowWall(const std::shared_ptr>& wall_sensors, const Config& config); + FollowWall( + const std::shared_ptr>& wall_sensors, const Pose& absolute_pose, const Config& config + ); /** * @brief Update the PID controller and return the response. @@ -39,7 +45,7 @@ class FollowWall { * @param elapsed_time The time elapsed since the last update. * @return The response of the PID controller. */ - float action(core::Observation observation, float elapsed_time, float linear_speed); + float action(float elapsed_time, float linear_speed); /** * @brief Reset the PID controller. @@ -47,6 +53,10 @@ class FollowWall { void reset(); private: + void reset_displacement(bool reseted_by_post = false); + + bool check_posts(); + /** * @brief Wall sensors of the robot. */ @@ -58,6 +68,22 @@ class FollowWall { core::PidController pid; float max_linear_speed; + + float post_threshold; + + RelativePose blind_pose; + float last_blind_distance; + + float cell_size; + float post_margin; + + bool left_wall{true}; + bool right_wall{true}; + + float last_right_error{}; + float last_left_error{}; + + bool reseted_by_post{}; }; } // namespace micras::nav diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index d12c902..125bec7 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -23,17 +23,16 @@ ActionQueuer::ActionQueuer(Config config) : exploring_params.max_linear_deceleration )}, turn_left{std::make_shared( - std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration, + std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, exploring_params.max_angular_acceleration )}, turn_right{std::make_shared( - -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration, - exploring_params.max_angular_acceleration - )}, - turn_back{std::make_shared( - std::numbers::pi_v, cell_size / 2.0F, exploring_params.max_centrifugal_acceleration, + -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, exploring_params.max_angular_acceleration )}, + turn_back{ + std::make_shared(std::numbers::pi_v, 0.0F, 0.0F, exploring_params.max_angular_acceleration) + }, action_queue{{start}} { } void ActionQueuer::push(const GridPose& current_pose, const GridPoint& target_position) { diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index bf8d647..f69a7cb 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -5,22 +5,42 @@ #include "micras/nav/follow_wall.hpp" namespace micras::nav { -FollowWall::FollowWall(const std::shared_ptr>& wall_sensors, const Config& config) : - wall_sensors{wall_sensors}, pid{config.pid}, max_linear_speed{config.max_linear_speed} { } +FollowWall::FollowWall( + const std::shared_ptr>& wall_sensors, const Pose& absolute_pose, const Config& config +) : + wall_sensors{wall_sensors}, + pid{config.pid}, + max_linear_speed{config.max_linear_speed}, + post_threshold{config.post_threshold}, + blind_pose{absolute_pose}, + cell_size{config.cell_size}, + post_margin{config.post_margin} { } + +float FollowWall::action(float elapsed_time, float linear_speed) { + if (this->check_posts()) { + return 0.0F; + } + + this->last_left_error = this->wall_sensors->get_sensor_error(0); + this->last_right_error = this->wall_sensors->get_sensor_error(3); + + if ((not this->left_wall or not this->right_wall) and + (this->last_blind_distance >= (this->cell_size + (this->reseted_by_post ? this->post_margin : 0.0F)))) { + this->left_wall = this->wall_sensors->get_wall(0); + this->right_wall = this->wall_sensors->get_wall(3); + this->reset_displacement(); + } -float FollowWall::action(core::Observation observation, float elapsed_time, float linear_speed) { float error{}; - if (observation.front) { - error = this->wall_sensors->get_sensor_error(0) - this->wall_sensors->get_sensor_error(3); - } else if (observation.left and observation.right) { + if (this->left_wall and this->right_wall) { error = this->wall_sensors->get_sensor_error(1) - this->wall_sensors->get_sensor_error(2); - } else if (observation.left) { + } else if (this->left_wall) { error = 2.0F * this->wall_sensors->get_sensor_error(1); - } else if (observation.right) { + } else if (this->right_wall) { error = 2.0F * this->wall_sensors->get_sensor_error(2); } else { - error = 0.0F; + return 0.0F; } const float response = this->pid.update(error, elapsed_time); @@ -28,7 +48,52 @@ float FollowWall::action(core::Observation observation, float elapsed_time, floa return response * linear_speed / this->max_linear_speed; } +bool FollowWall::check_posts() { + const float current_distance = this->blind_pose.get().position.distance({0.0F, 0.0F}); + const float delta_distance = current_distance - this->last_blind_distance; + + if (delta_distance <= 0.0F) { + return false; + } + + this->last_blind_distance = current_distance; + bool found_posts = false; + + if (this->left_wall and + (this->wall_sensors->get_sensor_error(0) - this->last_left_error) / delta_distance >= this->post_threshold) { + this->left_wall = false; + + if (this->right_wall) { + this->reset_displacement(true); + } + + found_posts = true; + } + + if (this->right_wall and + (this->wall_sensors->get_sensor_error(3) - this->last_right_error) / delta_distance >= this->post_threshold) { + this->right_wall = false; + + if (this->left_wall) { + this->reset_displacement(true); + } + + found_posts = true; + } + + return found_posts; +} + +void FollowWall::reset_displacement(bool reseted_by_post) { + this->blind_pose.reset_reference(); + this->last_blind_distance = 0.0F; + this->reseted_by_post = reseted_by_post; +} + void FollowWall::reset() { this->pid.reset(); + this->reset_displacement(); + this->left_wall = this->wall_sensors->get_wall(0); + this->right_wall = this->wall_sensors->get_wall(3); } } // namespace micras::nav From 5d56e690d18d5a304a9620f8be998b9132d49583 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Apr 2025 22:04:26 -0300 Subject: [PATCH 310/561] =?UTF-8?q?=F0=9F=90=9B=20Make=20allow=20follow=20?= =?UTF-8?q?wall=20a=20base=20state=20function?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/base.hpp | 2 +- micras_nav/include/micras/nav/actions/move.hpp | 2 +- micras_nav/include/micras/nav/actions/turn.hpp | 2 ++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 3d938c7..ae2790a 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -22,7 +22,7 @@ class Action { virtual bool finished(const Pose& pose) const = 0; - static constexpr bool allow_follow_wall{}; + virtual constexpr bool allow_follow_wall() const = 0; protected: /** diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index ad36eec..32d5e82 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -50,7 +50,7 @@ class MoveAction : public Action { bool finished(const Pose& pose) const override { return pose.position.distance({0.0F, 0.0F}) >= this->distance; } - static constexpr bool allow_follow_wall{true}; + constexpr bool allow_follow_wall() const override { return true; } private: float distance; diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 91fc0f9..043bec5 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -53,6 +53,8 @@ class TurnAction : public Action { bool finished(const Pose& pose) const override { return std::abs(pose.orientation) >= std::abs(this->angle); } + constexpr bool allow_follow_wall() const override { return false; } + private: float angle; float linear_speed; From 3a67b2d2ed2a481176a2662733fac398b4789e36 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Apr 2025 22:15:31 -0300 Subject: [PATCH 311/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20sensors=20numbers?= =?UTF-8?q?=20on=20follow=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/follow_wall.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index f69a7cb..3d83bfa 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -21,9 +21,6 @@ float FollowWall::action(float elapsed_time, float linear_speed) { return 0.0F; } - this->last_left_error = this->wall_sensors->get_sensor_error(0); - this->last_right_error = this->wall_sensors->get_sensor_error(3); - if ((not this->left_wall or not this->right_wall) and (this->last_blind_distance >= (this->cell_size + (this->reseted_by_post ? this->post_margin : 0.0F)))) { this->left_wall = this->wall_sensors->get_wall(0); @@ -60,7 +57,7 @@ bool FollowWall::check_posts() { bool found_posts = false; if (this->left_wall and - (this->wall_sensors->get_sensor_error(0) - this->last_left_error) / delta_distance >= this->post_threshold) { + (this->wall_sensors->get_sensor_error(1) - this->last_left_error) / delta_distance >= this->post_threshold) { this->left_wall = false; if (this->right_wall) { @@ -71,7 +68,7 @@ bool FollowWall::check_posts() { } if (this->right_wall and - (this->wall_sensors->get_sensor_error(3) - this->last_right_error) / delta_distance >= this->post_threshold) { + (this->wall_sensors->get_sensor_error(2) - this->last_right_error) / delta_distance >= this->post_threshold) { this->right_wall = false; if (this->left_wall) { @@ -81,6 +78,9 @@ bool FollowWall::check_posts() { found_posts = true; } + this->last_left_error = this->wall_sensors->get_sensor_error(1); + this->last_right_error = this->wall_sensors->get_sensor_error(2); + return found_posts; } @@ -93,7 +93,7 @@ void FollowWall::reset_displacement(bool reseted_by_post) { void FollowWall::reset() { this->pid.reset(); this->reset_displacement(); - this->left_wall = this->wall_sensors->get_wall(0); - this->right_wall = this->wall_sensors->get_wall(3); + this->left_wall = this->wall_sensors->get_wall(1); + this->right_wall = this->wall_sensors->get_wall(2); } } // namespace micras::nav From 7592a23a9e3c2eb1daa0fb6cd0caeaebdf10ffc0 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Apr 2025 22:26:59 -0300 Subject: [PATCH 312/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20threshold=20compar?= =?UTF-8?q?ison=20on=20follow=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/follow_wall.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 3d83bfa..bab01e8 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -57,7 +57,7 @@ bool FollowWall::check_posts() { bool found_posts = false; if (this->left_wall and - (this->wall_sensors->get_sensor_error(1) - this->last_left_error) / delta_distance >= this->post_threshold) { + (this->wall_sensors->get_sensor_error(1) - this->last_left_error) / delta_distance <= this->post_threshold) { this->left_wall = false; if (this->right_wall) { @@ -68,7 +68,7 @@ bool FollowWall::check_posts() { } if (this->right_wall and - (this->wall_sensors->get_sensor_error(2) - this->last_right_error) / delta_distance >= this->post_threshold) { + (this->wall_sensors->get_sensor_error(2) - this->last_right_error) / delta_distance <= this->post_threshold) { this->right_wall = false; if (this->left_wall) { From a3fabcbfad7f9f0cd7e1d0fae6a27bfd2da4db2b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Apr 2025 22:41:07 -0300 Subject: [PATCH 313/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20sign=20of=20right?= =?UTF-8?q?=20reading=20on=20follow=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/follow_wall.cpp | 2 +- micras_nav/src/state.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index bab01e8..face27f 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -35,7 +35,7 @@ float FollowWall::action(float elapsed_time, float linear_speed) { } else if (this->left_wall) { error = 2.0F * this->wall_sensors->get_sensor_error(1); } else if (this->right_wall) { - error = 2.0F * this->wall_sensors->get_sensor_error(2); + error = -2.0F * this->wall_sensors->get_sensor_error(2); } else { return 0.0F; } diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index ba6e2d9..644ae16 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -5,7 +5,6 @@ #include #include -#include "micras/core/utils.hpp" #include "micras/nav/state.hpp" namespace micras::nav { @@ -104,7 +103,7 @@ RelativePose::RelativePose(const Pose& absolute_pose) : absolute_pose{absolute_p Pose RelativePose::get() const { return { this->absolute_pose.position - this->reference_pose.position, - core::assert_angle(this->absolute_pose.orientation - this->reference_pose.orientation) + this->absolute_pose.orientation - this->reference_pose.orientation }; } From d036cfd5e0786ce9a1a50ea80c5a5b5c0bbb217e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Apr 2025 23:44:05 -0300 Subject: [PATCH 314/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20180=20turn=20speed?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/turn.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 043bec5..a520bd3 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -24,7 +24,7 @@ class TurnAction : public Action { acceleration{max_angular_acceleration}, max_speed{ (linear_speed == 0.0F) ? - (max_angular_acceleration * 0.1F) : + (max_angular_acceleration * 0.01F) : (correction_factor * max_angular_acceleration * (curve_radius / linear_speed - std::sqrt( std::pow(curve_radius / linear_speed, 2.0F) - From 12544e02efde25b2f3b53e336a1c7d25a2157dc1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Apr 2025 00:08:10 -0300 Subject: [PATCH 315/561] =?UTF-8?q?=E2=9C=A8=20Add=20move=5Fhalf=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/action_queuer.hpp | 2 ++ micras_nav/src/action_queuer.cpp | 12 +++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index c099741..97afb93 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -52,6 +52,8 @@ class ActionQueuer { std::shared_ptr start; std::shared_ptr move_forward; + std::shared_ptr stop; + std::shared_ptr move_half; std::shared_ptr turn_left; std::shared_ptr turn_right; std::shared_ptr turn_back; diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 125bec7..333517a 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -22,6 +22,15 @@ ActionQueuer::ActionQueuer(Config config) : exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration )}, + stop{std::make_shared( + cell_size / 2.0F, exploring_params.max_linear_speed, 0.0F, exploring_params.max_linear_speed, + exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration + )}, + move_half{std::make_shared( + cell_size / 2.0F, exploring_params.max_linear_speed, exploring_params.max_linear_speed, + exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, + exploring_params.max_linear_deceleration + )}, turn_left{std::make_shared( std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, exploring_params.max_angular_acceleration @@ -52,8 +61,9 @@ void ActionQueuer::push(const GridPose& current_pose, const GridPoint& target_po } if (current_pose.turned_back().front().position == target_position) { + this->action_queue.emplace(stop); this->action_queue.emplace(turn_back); - this->action_queue.emplace(move_forward); + this->action_queue.emplace(move_half); return; } } From 7165bcb4bce7bfe5633165d4a91195aee010d315 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Apr 2025 00:49:41 -0300 Subject: [PATCH 316/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20move=20half=20star?= =?UTF-8?q?t=20speed?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 333517a..fa3312d 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -27,7 +27,7 @@ ActionQueuer::ActionQueuer(Config config) : exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration )}, move_half{std::make_shared( - cell_size / 2.0F, exploring_params.max_linear_speed, exploring_params.max_linear_speed, + cell_size / 2.0F, 0.001F * exploring_params.max_linear_acceleration, exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration )}, From 00bd6b8ac655f12d012c1deb18f77cd9c17fa31d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Apr 2025 01:46:41 -0300 Subject: [PATCH 317/561] =?UTF-8?q?=E2=9C=A8=20Remove=20follow=5Fwall=20fr?= =?UTF-8?q?om=20stop=20and=20move=5Fhalf?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/base.hpp | 2 +- micras_nav/include/micras/nav/actions/move.hpp | 6 ++++-- micras_nav/include/micras/nav/actions/turn.hpp | 2 +- micras_nav/src/action_queuer.cpp | 4 ++-- 4 files changed, 8 insertions(+), 6 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index ae2790a..dc8f614 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -22,7 +22,7 @@ class Action { virtual bool finished(const Pose& pose) const = 0; - virtual constexpr bool allow_follow_wall() const = 0; + virtual bool allow_follow_wall() const = 0; protected: /** diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 32d5e82..6e31e8e 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -17,7 +17,7 @@ class MoveAction : public Action { public: MoveAction( float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, - float max_deceleration + float max_deceleration, bool follow_wall = true ) : distance(distance), start_speed_2(start_speed * start_speed), @@ -25,6 +25,7 @@ class MoveAction : public Action { max_speed{max_speed}, max_acceleration_doubled{2.0F * max_acceleration}, max_deceleration_doubled{2.0F * max_deceleration}, + follow_wall(follow_wall), decelerate_distance{ (end_speed_2 - start_speed_2 + max_deceleration_doubled * distance) / (max_acceleration_doubled + max_deceleration_doubled) @@ -50,7 +51,7 @@ class MoveAction : public Action { bool finished(const Pose& pose) const override { return pose.position.distance({0.0F, 0.0F}) >= this->distance; } - constexpr bool allow_follow_wall() const override { return true; } + bool allow_follow_wall() const override { return this->follow_wall; } private: float distance; @@ -59,6 +60,7 @@ class MoveAction : public Action { float max_speed; float max_acceleration_doubled; float max_deceleration_doubled; + bool follow_wall; float decelerate_distance; }; diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index a520bd3..c1bf43c 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -53,7 +53,7 @@ class TurnAction : public Action { bool finished(const Pose& pose) const override { return std::abs(pose.orientation) >= std::abs(this->angle); } - constexpr bool allow_follow_wall() const override { return false; } + bool allow_follow_wall() const override { return false; } private: float angle; diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index fa3312d..d5e7c52 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -24,12 +24,12 @@ ActionQueuer::ActionQueuer(Config config) : )}, stop{std::make_shared( cell_size / 2.0F, exploring_params.max_linear_speed, 0.0F, exploring_params.max_linear_speed, - exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration + exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false )}, move_half{std::make_shared( cell_size / 2.0F, 0.001F * exploring_params.max_linear_acceleration, exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, - exploring_params.max_linear_deceleration + exploring_params.max_linear_deceleration, false )}, turn_left{std::make_shared( std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, From a5bbc41f4a152eb8631c95abae01f3030948c09f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Apr 2025 02:39:39 -0300 Subject: [PATCH 318/561] =?UTF-8?q?=E2=9C=A8=20Add=20return=20back?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 6 +++--- micras_nav/src/maze.cpp | 19 ++++++++++++------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 338cfd2..539a626 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include "micras/core/serializable.hpp" @@ -75,7 +75,7 @@ class TMaze : public core::ISerializable { * * @return The best route to the goal. */ - const std::list& get_best_route() const; + const std::map>& get_best_route() const; /** * @brief Serialize the best route to the goal. @@ -156,7 +156,7 @@ class TMaze : public core::ISerializable { /** * @brief Current best found route to the goal. */ - std::list best_route; + std::map> best_route; }; } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 539477c..fa9e1d6 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -66,7 +66,12 @@ GridPose TMaze::get_next_goal(const GridPoint& position, bool ret uint16_t current_cost = this->get_cell(position).cost; if (returning) { - return get_next_goal(position, false); + if (this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { + auto current = this->best_route.find(current_cost); + return {std::prev(current)->second.position, current->second.turned_back().orientation}; + } + + return get_next_goal(position, true); } GridPoint next_position = position; @@ -132,16 +137,16 @@ template void TMaze::calculate_best_route() { GridPose current_pose = this->start; this->best_route.clear(); - this->best_route.emplace_back(this->start); + this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start); while (not this->goal.contains(current_pose.position)) { current_pose = this->get_current_exploration_goal(current_pose.position); - this->best_route.emplace_back(current_pose); + this->best_route.try_emplace(this->get_cell(current_pose.position).cost, current_pose); } } template -const std::list& TMaze::get_best_route() const { +const std::map>& TMaze::get_best_route() const { return this->best_route; } @@ -187,7 +192,7 @@ std::vector TMaze::serialize() const { std::vector buffer; buffer.reserve(3 * this->best_route.size()); - for (const auto& grid_pose : this->best_route) { + for (const auto& [cost, grid_pose] : this->best_route) { buffer.emplace_back(grid_pose.position.x); buffer.emplace_back(grid_pose.position.y); buffer.emplace_back(grid_pose.orientation); @@ -201,9 +206,9 @@ void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { this->best_route.clear(); for (uint32_t i = 0; i < size; i += 3) { - this->best_route.emplace_back( + this->best_route.try_emplace( // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) - GridPoint{buffer[i], buffer[i + 1]}, static_cast(buffer[i + 2]) + (size - i) / 3 - 1, GridPose{{buffer[i], buffer[i + 1]}, static_cast(buffer[i + 2])} ); } } From 8e29552a36ff22ed24c66215994843ea84f135f5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Apr 2025 02:53:33 -0300 Subject: [PATCH 319/561] =?UTF-8?q?=F0=9F=90=9B=20Add=20calculate=5Fbest?= =?UTF-8?q?=5Froute=20to=20maze=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index fa9e1d6..c05555e 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -66,6 +66,8 @@ GridPose TMaze::get_next_goal(const GridPoint& position, bool ret uint16_t current_cost = this->get_cell(position).cost; if (returning) { + this->calculate_best_route(); + if (this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { auto current = this->best_route.find(current_cost); return {std::prev(current)->second.position, current->second.turned_back().orientation}; From 0be01d614cd6b1df4fda086244b1aeaf23beb4a1 Mon Sep 17 00:00:00 2001 From: dardengo Date: Sun, 27 Apr 2025 03:18:31 -0300 Subject: [PATCH 320/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20calculate=20best?= =?UTF-8?q?=20route?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index c05555e..a3616ea 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -66,14 +66,12 @@ GridPose TMaze::get_next_goal(const GridPoint& position, bool ret uint16_t current_cost = this->get_cell(position).cost; if (returning) { - this->calculate_best_route(); - if (this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { auto current = this->best_route.find(current_cost); return {std::prev(current)->second.position, current->second.turned_back().orientation}; } - return get_next_goal(position, true); + return get_next_goal(position, false); } GridPoint next_position = position; @@ -142,7 +140,7 @@ void TMaze::calculate_best_route() { this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start); while (not this->goal.contains(current_pose.position)) { - current_pose = this->get_current_exploration_goal(current_pose.position); + current_pose = this->get_next_goal(current_pose.position, false); this->best_route.try_emplace(this->get_cell(current_pose.position).cost, current_pose); } } From 371c0cc22e4d804e9513d8248ca69338cd33f128 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Apr 2025 13:21:00 -0300 Subject: [PATCH 321/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 2 +- micras_nav/include/micras/nav/state.hpp | 4 ++-- micras_nav/src/state.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 479a09d..2496e64 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -72,7 +72,7 @@ class FollowWall { float post_threshold; RelativePose blind_pose; - float last_blind_distance; + float last_blind_distance{}; float cell_size; float post_margin; diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index d849eb6..7c5113e 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -179,7 +179,7 @@ class RelativePose { * * @param absolute_pose The absolute pose to be used as a reference. */ - RelativePose(const Pose& absolute_pose); + explicit RelativePose(const Pose& absolute_pose); /** * @brief Get the relative pose. @@ -197,7 +197,7 @@ class RelativePose { /** * @brief A reference to the absolute pose. */ - const Pose& absolute_pose; + const Pose* absolute_pose; /** * @brief The reference pose to be used for calculations. diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 644ae16..8694877 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -98,16 +98,16 @@ Point Pose::to_cell(float cell_size) const { return {cell_alignment, cell_advance}; } -RelativePose::RelativePose(const Pose& absolute_pose) : absolute_pose{absolute_pose} { } +RelativePose::RelativePose(const Pose& absolute_pose) : absolute_pose{&absolute_pose} { } Pose RelativePose::get() const { return { - this->absolute_pose.position - this->reference_pose.position, - this->absolute_pose.orientation - this->reference_pose.orientation + this->absolute_pose->position - this->reference_pose.position, + this->absolute_pose->orientation - this->reference_pose.orientation }; } void RelativePose::reset_reference() { - this->reference_pose = this->absolute_pose; + this->reference_pose = *(this->absolute_pose); } } // namespace micras::nav From 6c0b783bd7febe23b332c3ff2a0112864686479a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Apr 2025 14:56:36 -0300 Subject: [PATCH 322/561] =?UTF-8?q?=E2=9C=A8=20Add=20recalculate=20functio?= =?UTF-8?q?n=20implementation=20to=20action=20queuer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/action_queuer.hpp | 4 ++-- micras_nav/src/action_queuer.cpp | 12 +++++++++++- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 97afb93..8d9ee5b 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -5,7 +5,7 @@ #ifndef MICRAS_NAV_ACTION_QUEUER_HPP #define MICRAS_NAV_ACTION_QUEUER_HPP -#include +#include #include #include @@ -42,7 +42,7 @@ class ActionQueuer { bool empty() const; - void recalculate(const std::list& best_route); + void recalculate(const std::map>& best_route); private: float cell_size; diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index d5e7c52..e16d95d 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -78,5 +78,15 @@ bool ActionQueuer::empty() const { return this->action_queue.empty(); } -void ActionQueuer::recalculate(const std::list& /*best_route*/) { } +void ActionQueuer::recalculate(const std::map>& best_route) { + this->action_queue = {}; + + for (const auto& [cost, pose] : best_route) { + this->push(pose, pose.front().position); + } + + if (not this->action_queue.empty()) { + this->action_queue.pop(); + } +} } // namespace micras::nav From a8ea05edc7f47714f362a151f95a8df5d7810f09 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Apr 2025 15:17:29 -0300 Subject: [PATCH 323/561] =?UTF-8?q?=F0=9F=90=9B=20Add=20start=20action=20t?= =?UTF-8?q?o=20recalculate?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index e16d95d..71bac86 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -80,13 +80,10 @@ bool ActionQueuer::empty() const { void ActionQueuer::recalculate(const std::map>& best_route) { this->action_queue = {}; + this->action_queue.emplace(start); - for (const auto& [cost, pose] : best_route) { - this->push(pose, pose.front().position); - } - - if (not this->action_queue.empty()) { - this->action_queue.pop(); + for (auto it = std::next(best_route.begin()); std::next(it) != best_route.end(); it++) { + this->push(it->second, std::next(it)->second.position); } } } // namespace micras::nav From cfa43d8d24c49fe45846d8d0c42ea7b62690f7cf Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Apr 2025 17:05:38 -0300 Subject: [PATCH 324/561] =?UTF-8?q?=F0=9F=93=9D=20Fix=20documentation=20fo?= =?UTF-8?q?r=20modified=20classes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 51 ++++++++++++++- .../include/micras/nav/actions/base.hpp | 19 +++++- .../include/micras/nav/actions/move.hpp | 64 ++++++++++++++++++- .../include/micras/nav/actions/turn.hpp | 47 +++++++++++++- micras_nav/include/micras/nav/follow_wall.hpp | 59 +++++++++++++++-- micras_nav/include/micras/nav/maze.hpp | 2 + .../include/micras/nav/speed_controller.hpp | 40 ++++++++++-- micras_nav/src/action_queuer.cpp | 5 +- micras_nav/src/maze.cpp | 22 +++---- micras_proxy/include/micras/proxy/imu.hpp | 5 +- 10 files changed, 279 insertions(+), 35 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 8d9ee5b..d4fb889 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -14,10 +14,13 @@ namespace micras::nav { /** - * @brief Class to follow the side walls using a PID controller. + * @brief Class to queue actions for the robot. */ class ActionQueuer { public: + /** + * @brief Configuration struct for the ActionQueuer class. + */ struct Config { struct Dynamic { float max_linear_speed; @@ -34,22 +37,60 @@ class ActionQueuer { Dynamic solving; }; + /** + * @brief Construct a new ActionQueuer object. + * + * @param config Configuration for the ActionQueuer. + */ explicit ActionQueuer(Config config); + /** + * @brief Push an action to the queue. + * + * @param current_pose Current pose of the robot. + * @param target_position Target position to move to. + */ void push(const GridPose& current_pose, const GridPoint& target_position); + /** + * @brief Pop an action from the queue. + * + * @return Shared pointer to the action. + */ std::shared_ptr pop(); + /** + * @brief Check if the action queue is empty. + * + * @return True if the action queue is empty, false otherwise. + */ bool empty() const; + /** + * @brief Fill the action action queue with a sequence of actions to the end. + */ void recalculate(const std::map>& best_route); private: - float cell_size; - float start_offset; + /** + * @brief Size of the cells in the grid. + */ + float cell_size; + + /** + * @brief Dynamic exploring parameters. + */ Config::Dynamic exploring_params; + + /** + * @brief Dynamic solving parameters. + */ // Config::Dynamic solving_params; + /** + * @brief Pre-built actions to use in the exploration. + */ + ///@{ std::shared_ptr start; std::shared_ptr move_forward; std::shared_ptr stop; @@ -57,7 +98,11 @@ class ActionQueuer { std::shared_ptr turn_left; std::shared_ptr turn_right; std::shared_ptr turn_back; + ///@} + /** + * @brief Queue of actions to be performed. + */ std::queue> action_queue; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index dc8f614..ca6794d 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -9,7 +9,7 @@ namespace micras::nav { /** - * @brief Class to follow the side walls using a PID controller. + * @brief Base class for actions. */ class Action { public: @@ -18,10 +18,27 @@ class Action { */ virtual ~Action() = default; + /** + * @brief Get the desired speeds for the robot. + * + * @param pose The current pose of the robot. + * @return The desired speeds for the robot. + */ virtual Twist get_twist(const Pose& pose) const = 0; + /** + * @brief Check if the action is finished. + * + * @param pose The current pose of the robot. + * @return True if the action is finished, false otherwise. + */ virtual bool finished(const Pose& pose) const = 0; + /** + * @brief Check if the action allow the robot to follow walls. + * + * @return True if the action allows the robot to follow walls, false otherwise. + */ virtual bool allow_follow_wall() const = 0; protected: diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 6e31e8e..67bd41e 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -11,10 +11,21 @@ namespace micras::nav { /** - * @brief Class to follow the side walls using a PID controller. + * @brief Action to move the robot a certain distance forward. */ class MoveAction : public Action { public: + /** + * @brief Construct a new Move Action object. + * + * @param distance Distance to move in meters. + * @param start_speed Initial speed in m/s. + * @param end_speed Final speed in m/s. + * @param max_speed Maximum speed in m/s. + * @param max_acceleration Maximum acceleration in m/s^2. + * @param max_deceleration Maximum deceleration in m/s^2. + * @param follow_wall Whether the robot can follow wall while executing this action. + */ MoveAction( float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, float max_deceleration, bool follow_wall = true @@ -31,6 +42,14 @@ class MoveAction : public Action { (max_acceleration_doubled + max_deceleration_doubled) } { } + /** + * @brief Get the desired speeds for the robot to compete the action. + * + * @param pose The current pose of the robot. + * @return The desired speeds for the robot to complete the action. + * + * @details The desired velocity is calculated from the linear displacement based on the Torricelli equation. + */ Twist get_twist(const Pose& pose) const override { const float current_distance = pose.position.distance({0.0F, 0.0F}); Twist twist{}; @@ -49,19 +68,60 @@ class MoveAction : public Action { return twist; } + /** + * @brief Check if the action is finished. + * + * @param pose The current pose of the robot. + * @return True if the action is finished, false otherwise. + */ bool finished(const Pose& pose) const override { return pose.position.distance({0.0F, 0.0F}) >= this->distance; } + /** + * @brief Check if the action allows the robot to follow walls. + * + * @return True if the action allows the robot to follow walls, false otherwise. + */ bool allow_follow_wall() const override { return this->follow_wall; } private: + /** + * @brief Distance to move in meters. + */ float distance; + + /** + * @brief Initial speed squared. + */ float start_speed_2; + + /** + * @brief Final speed squared. + */ float end_speed_2; + + /** + * @brief Maximum linear speed in m/s. + */ float max_speed; + + /** + * @brief Maximum linear acceleration multiplied by 2. + */ float max_acceleration_doubled; + + /** + * @brief Maximum linear deceleration multiplied by 2. + */ float max_deceleration_doubled; - bool follow_wall; + /** + * @brief Whether the robot can follow walls while executing this action. + */ + bool follow_wall; + + /** + * @brief Distance from the start where the robot should start to decelerate in meters. + */ float decelerate_distance; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index c1bf43c..572caef 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -14,10 +14,21 @@ namespace micras::nav { static constexpr float correction_factor{0.95F}; /** - * @brief Class to follow the side walls using a PID controller. + * @brief Action to turn the robot following a curve radius. */ class TurnAction : public Action { public: + /** + * @brief Construct a new Turn Action object. + * + * @param angle Angle to turn in radians. + * @param curve_radius Radius of the curve in meters. + * @param linear_speed Linear speed in m/s. + * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * + * @details The angular max speed is calculated to generate a curve equivalent in displacement to one that + * maintains the desired radius of curvature throughout the entire turn. + */ TurnAction(float angle, float curve_radius, float linear_speed, float max_angular_acceleration) : angle{angle}, linear_speed{linear_speed}, @@ -32,6 +43,14 @@ class TurnAction : public Action { ))) } { } + /** + * @brief Get the desired speeds for the robot to complete the action. + * + * @param pose Current pose of the robot. + * @return The desired speeds for the robot to complete the action. + * + * @details The desired velocity is calculated from the angular displacement based on the Torricelli equation. + */ Twist get_twist(const Pose& pose) const override { Twist twist{}; const float current_orientation = std::abs(pose.orientation); @@ -51,14 +70,40 @@ class TurnAction : public Action { return twist; } + /** + * @brief Check if the action is finished. + * + * @param pose Current pose of the robot. + * @return True if the action is finished, false otherwise. + */ bool finished(const Pose& pose) const override { return std::abs(pose.orientation) >= std::abs(this->angle); } + /** + * @brief Check if the action allows following the wall. + * + * @return True if the action allows following the wall, false otherwise. + */ bool allow_follow_wall() const override { return false; } private: + /** + * @brief Angle to turn in radians. + */ float angle; + + /** + * @brief Linear speed in m/s while turning. + */ float linear_speed; + + /** + * @brief Maximum angular acceleration in rad/s^2. + */ float acceleration; + + /** + * @brief Maximum angular speed in rad/s while turning. + */ float max_speed; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 2496e64..8fbfcfe 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -32,6 +32,7 @@ class FollowWall { * @brief Construct a new FollowWall object. * * @param wall_sensors The wall sensors of the robot. + * @param absolute_pose Reference to the absolute pose of the robot. * @param config The configuration for the FollowWall class. */ FollowWall( @@ -41,20 +42,33 @@ class FollowWall { /** * @brief Update the PID controller and return the response. * - * @param follow_wall_type The type of wall following to perform. * @param elapsed_time The time elapsed since the last update. + * @param linear_speed Current linear speed of the robot. * @return The response of the PID controller. */ float action(float elapsed_time, float linear_speed); /** - * @brief Reset the PID controller. + * @brief Reset the PID controller and the relative pose. */ void reset(); private: + /** + * @brief Reset the displacement of the robot. + * + * @param reseted_by_post Whether the reset was triggered by a post. + */ void reset_displacement(bool reseted_by_post = false); + /** + * @brief Check if the robot is seeing a post. + * + * @return True if the robot is seeing a post, false otherwise. + * + * @details This function uses the derivative of the distance sensors reading in relation to the distance + * traveled by the robot, comparing this value to the threshold to detect posts. + */ bool check_posts(); /** @@ -67,22 +81,59 @@ class FollowWall { */ core::PidController pid; + /** + * @brief Maximum linear speed of the robot. + */ float max_linear_speed; + /** + * @brief Derivative threshold for detecting posts. + */ float post_threshold; + /** + * @brief Current pose of the robot relative to the last time it was reset. + */ RelativePose blind_pose; - float last_blind_distance{}; + /** + * @brief Last distance of the robot to the start point, used to compute the derivative of the distance sensors. + */ + float last_blind_distance{}; + + /** + * @brief Size of the cells in the map. + */ float cell_size; + + /** + * @brief Margin in front of a post to stop seeing it. + */ float post_margin; + /** + * @brief Flag to indicate if the robot is currently following the left wall. + */ bool left_wall{true}; + + /** + * @brief Flag to indicate if the robot is currently following the right wall. + */ bool right_wall{true}; - float last_right_error{}; + /** + * @brief Last error measured by the left wall sensor, used to compute the derivative of the distance sensors. + */ float last_left_error{}; + /** + * @brief Last error measured by the right wall sensor, used to compute the derivative of the distance sensors. + */ + float last_right_error{}; + + /** + * @brief Flag to indicate if relative pose was reset by a post. + */ bool reseted_by_post{}; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 539a626..5a18070 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -45,6 +45,7 @@ class TMaze : public core::ISerializable { * @brief Return the next point the robot should go to when exploring. * * @param position The current position of the robot. + * @param returning Whether the robot is returning to the start position. * @return The next point the robot should go to when exploring. */ GridPose get_next_goal(const GridPoint& position, bool returning) const; @@ -61,6 +62,7 @@ class TMaze : public core::ISerializable { * @brief Check whether the robot has finished the maze. * * @param position The current position of the robot. + * @param returning Whether the robot is returning to the start position. * @return True if the robot has finished the maze, false otherwise. */ bool finished(const GridPoint& position, bool returning) const; diff --git a/micras_nav/include/micras/nav/speed_controller.hpp b/micras_nav/include/micras/nav/speed_controller.hpp index dd56ab1..952ce93 100644 --- a/micras_nav/include/micras/nav/speed_controller.hpp +++ b/micras_nav/include/micras/nav/speed_controller.hpp @@ -12,7 +12,7 @@ namespace micras::nav { /** - * @brief Class to calculate a command to go to a point. + * @brief Class to calculate a command to follow a pair of linear and angular speeds. */ class SpeedController { public: @@ -43,12 +43,12 @@ class SpeedController { explicit SpeedController(const Config& config); /** - * @brief Calculate the command to go to a point. + * @brief Calculate the command to achieve the desired speeds. * - * @param state The current state of the robot. - * @param goal The goal point. - * @param elapsed_time The time elapsed since the last update. - * @return The command to go to the point. + * @param current_twist The current speeds of the robot. + * @param desired_twist The desired speeds of the robot. + * @param elapsed_time The time since the last update. + * @return A pair of floats representing the left and right wheel speeds. */ std::pair action(const Twist& current_twist, const Twist& desired_twist, float elapsed_time); @@ -58,12 +58,34 @@ class SpeedController { void reset(); private: + /** + * @brief Calculate the feed-forward term for a motor. + * + * @param speed The current speeds of the robot. + * @param acceleration The current accelerations of the robot. + * @param config The configuration for the feed-forward term. + * @return The feed-forward parameters for a motor. + */ static float feed_forward(const Twist& speed, const Twist& acceleration, const Config::FeedForward& config); + /** + * @brief The maximum linear acceleration of the robot. + */ float max_linear_acceleration; + + /** + * @brief The maximum angular acceleration of the robot. + */ float max_angular_acceleration; + /** + * @brief The last linear speed of the robot. + */ float last_linear_speed{}; + + /** + * @brief The last angular speed of the robot. + */ float last_angular_speed{}; /** @@ -76,8 +98,14 @@ class SpeedController { */ core::PidController angular_pid; + /** + * @brief Feed-forward parameters for the left motor. + */ Config::FeedForward left_feed_forward; + /** + * @brief Feed-forward parameters for the right motor. + */ Config::FeedForward right_feed_forward; }; } // namespace micras::nav diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 71bac86..04850c1 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -9,12 +9,11 @@ namespace micras::nav { ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, - start_offset{config.start_offset}, exploring_params{config.exploring}, // solving_params{config.solving}, start{std::make_shared( - cell_size - start_offset, 0.001F * exploring_params.max_linear_acceleration, exploring_params.max_linear_speed, - exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, + cell_size - config.start_offset, 0.001F * exploring_params.max_linear_acceleration, + exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration )}, move_forward{std::make_shared( diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index a3616ea..e255a57 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -29,20 +29,14 @@ TMaze::TMaze(Config config) : start{config.start}, goal{config.go // Hardcoded walls at the end region if (width == 16 and height == 16) { - this->cells.at(7).at(7).walls[Side::DOWN] = true; - this->cells.at(7).at(7).walls[Side::LEFT] = true; - this->cells.at(6).at(7).walls[Side::UP] = true; - this->cells.at(7).at(6).walls[Side::RIGHT] = true; - this->cells.at(7).at(8).walls[Side::DOWN] = true; - this->cells.at(6).at(8).walls[Side::UP] = true; - this->cells.at(8).at(7).walls[Side::LEFT] = true; - this->cells.at(8).at(7).walls[Side::UP] = true; - this->cells.at(8).at(6).walls[Side::RIGHT] = true; - this->cells.at(9).at(7).walls[Side::DOWN] = true; - this->cells.at(8).at(8).walls[Side::UP] = true; - this->cells.at(8).at(8).walls[Side::RIGHT] = true; - this->cells.at(9).at(8).walls[Side::DOWN] = true; - this->cells.at(8).at(9).walls[Side::LEFT] = true; + this->update_wall({{7, 7}, Side::DOWN}, true); + this->update_wall({{8, 7}, Side::DOWN}, true); + // this->update_wall({{8, 7}, Side::RIGHT}, true); + this->update_wall({{8, 8}, Side::RIGHT}, true); + this->update_wall({{8, 8}, Side::UP}, true); + this->update_wall({{7, 8}, Side::UP}, true); + this->update_wall({{7, 8}, Side::LEFT}, true); + this->update_wall({{7, 7}, Side::LEFT}, true); } for (const auto& position : this->goal) { diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 37afcf9..47f4659 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -149,10 +149,13 @@ class Imu { core::ButterworthFilter calibration_filter{5.0F}; /** - * @brief Flag to check if the IMU is calibrated. + * @brief Flag to check if the IMU was calibrated. */ bool calibrated{}; + /** + * @brief Flag to check if the IMU was initialized. + */ bool initialized{}; }; } // namespace micras::proxy From e4a14e67069be9667485e413fb08ba1ca270093b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Apr 2025 17:15:31 -0300 Subject: [PATCH 325/561] =?UTF-8?q?=E2=9C=8F=EF=B8=8F=20Fix=20typo=20on=20?= =?UTF-8?q?follow=20wall=20variable?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 6 +++--- micras_nav/src/follow_wall.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 8fbfcfe..f336185 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -57,9 +57,9 @@ class FollowWall { /** * @brief Reset the displacement of the robot. * - * @param reseted_by_post Whether the reset was triggered by a post. + * @param reset_by_post Whether the reset was triggered by a post. */ - void reset_displacement(bool reseted_by_post = false); + void reset_displacement(bool reset_by_post = false); /** * @brief Check if the robot is seeing a post. @@ -134,7 +134,7 @@ class FollowWall { /** * @brief Flag to indicate if relative pose was reset by a post. */ - bool reseted_by_post{}; + bool reset_by_post{}; }; } // namespace micras::nav diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index face27f..f14cc64 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -22,7 +22,7 @@ float FollowWall::action(float elapsed_time, float linear_speed) { } if ((not this->left_wall or not this->right_wall) and - (this->last_blind_distance >= (this->cell_size + (this->reseted_by_post ? this->post_margin : 0.0F)))) { + (this->last_blind_distance >= (this->cell_size + (this->reset_by_post ? this->post_margin : 0.0F)))) { this->left_wall = this->wall_sensors->get_wall(0); this->right_wall = this->wall_sensors->get_wall(3); this->reset_displacement(); @@ -84,10 +84,10 @@ bool FollowWall::check_posts() { return found_posts; } -void FollowWall::reset_displacement(bool reseted_by_post) { +void FollowWall::reset_displacement(bool reset_by_post) { this->blind_pose.reset_reference(); this->last_blind_distance = 0.0F; - this->reseted_by_post = reseted_by_post; + this->reset_by_post = reset_by_post; } void FollowWall::reset() { From 7e47a3733bc1bf63c49adafb3244507b5711ce07 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Apr 2025 02:05:14 -0300 Subject: [PATCH 326/561] =?UTF-8?q?=F0=9F=8E=A8=20Improve=20variable=20nam?= =?UTF-8?q?es=20and=20comments?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: dardengo --- micras_core/include/micras/core/fsm.hpp | 2 +- .../include/micras/core/pid_controller.hpp | 4 +- micras_core/src/pid_controller.cpp | 6 +-- .../include/micras/nav/action_queuer.hpp | 4 +- .../include/micras/nav/actions/turn.hpp | 10 +++-- micras_nav/include/micras/nav/follow_wall.hpp | 22 +++++----- micras_nav/include/micras/nav/maze.hpp | 12 +++--- micras_nav/include/micras/nav/odometry.hpp | 2 +- .../include/micras/nav/speed_controller.hpp | 7 ++-- micras_nav/src/action_queuer.cpp | 2 +- micras_nav/src/follow_wall.cpp | 40 +++++++++---------- micras_nav/src/maze.cpp | 8 ++-- micras_nav/src/speed_controller.cpp | 6 +-- .../include/micras/proxy/wall_sensors.hpp | 7 ++-- 14 files changed, 68 insertions(+), 64 deletions(-) diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index a850cc6..1a187d3 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -74,7 +74,7 @@ class FSM { void add_state(std::unique_ptr state); /** - * @brief Run the FSM current state to calculate the next state. + * @brief Run the FSM current state to compute the next state. */ void run(); diff --git a/micras_core/include/micras/core/pid_controller.hpp b/micras_core/include/micras/core/pid_controller.hpp index 5028a38..5a62612 100644 --- a/micras_core/include/micras/core/pid_controller.hpp +++ b/micras_core/include/micras/core/pid_controller.hpp @@ -50,7 +50,7 @@ class PidController { * @param save Whether to save the variables for calibration. * @return Response of the controller. */ - float update(float state, float elapsed_time, bool save = false); + float compute_response(float state, float elapsed_time, bool save = false); /** * @brief Update PID with new state and return response. @@ -61,7 +61,7 @@ class PidController { * @param save Whether to save the variables for calibration. * @return Response of the controller. */ - float update(float state, float elapsed_time, float state_change, bool save = false); + float compute_response(float state, float elapsed_time, float state_change, bool save = false); private: /** diff --git a/micras_core/src/pid_controller.cpp b/micras_core/src/pid_controller.cpp index b2464a7..baa44c6 100644 --- a/micras_core/src/pid_controller.cpp +++ b/micras_core/src/pid_controller.cpp @@ -35,12 +35,12 @@ void PidController::reset() { this->last_response = 0; } -float PidController::update(float state, float elapsed_time, bool save) { +float PidController::compute_response(float state, float elapsed_time, bool save) { const float state_change = (state - this->prev_state) / elapsed_time; - return this->update(state, elapsed_time, state_change, save); + return this->compute_response(state, elapsed_time, state_change, save); } -float PidController::update(float state, float elapsed_time, float state_change, bool save) { +float PidController::compute_response(float state, float elapsed_time, float state_change, bool save) { const float error = this->setpoint - state; this->prev_state = state; diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index d4fb889..752aa2f 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -67,9 +67,9 @@ class ActionQueuer { bool empty() const; /** - * @brief Fill the action action queue with a sequence of actions to the end. + * @brief Fill the action queue with a sequence of actions to the end. */ - void recalculate(const std::map>& best_route); + void recompute(const std::map>& best_route); private: /** diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 572caef..4823dae 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -26,14 +26,15 @@ class TurnAction : public Action { * @param linear_speed Linear speed in m/s. * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. * - * @details The angular max speed is calculated to generate a curve equivalent in displacement to one that + * @details Maximum angular velocity is computed to generate a curve equivalent in displacement to one that * maintains the desired radius of curvature throughout the entire turn. + * If linear speed is zero, a minimal angular speed is assigned. */ TurnAction(float angle, float curve_radius, float linear_speed, float max_angular_acceleration) : angle{angle}, linear_speed{linear_speed}, acceleration{max_angular_acceleration}, - max_speed{ + max_angular_speed{ (linear_speed == 0.0F) ? (max_angular_acceleration * 0.01F) : (correction_factor * max_angular_acceleration * @@ -65,7 +66,8 @@ class TurnAction : public Action { }; } - twist.angular = std::copysign(std::clamp(twist.angular, -this->max_speed, this->max_speed), this->angle); + twist.angular = + std::copysign(std::clamp(twist.angular, -this->max_angular_speed, this->max_angular_speed), this->angle); return twist; } @@ -104,7 +106,7 @@ class TurnAction : public Action { /** * @brief Maximum angular speed in rad/s while turning. */ - float max_speed; + float max_angular_speed; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index f336185..b679048 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -25,7 +25,7 @@ class FollowWall { float max_linear_speed{}; float post_threshold{}; float cell_size{}; - float post_margin{}; + float post_clearance{}; }; /** @@ -40,13 +40,13 @@ class FollowWall { ); /** - * @brief Update the PID controller and return the response. + * @brief Calculate the desired angular speed to follow wall. * * @param elapsed_time The time elapsed since the last update. * @param linear_speed Current linear speed of the robot. - * @return The response of the PID controller. + * @return The desired angular speed to follow wall. */ - float action(float elapsed_time, float linear_speed); + float compute_angular_correction(float elapsed_time, float linear_speed); /** * @brief Reset the PID controller and the relative pose. @@ -62,12 +62,12 @@ class FollowWall { void reset_displacement(bool reset_by_post = false); /** - * @brief Check if the robot is seeing a post. + * @brief Check if the robot saw a post. * - * @return True if the robot is seeing a post, false otherwise. + * @return True if the robot saw a post, false otherwise. * - * @details This function uses the derivative of the distance sensors reading in relation to the distance - * traveled by the robot, comparing this value to the threshold to detect posts. + * @details This function uses the derivative of the distance sensors readings with respect + * to the robot's traveled distance, comparing it to a threshold to detect posts. */ bool check_posts(); @@ -109,17 +109,17 @@ class FollowWall { /** * @brief Margin in front of a post to stop seeing it. */ - float post_margin; + float post_clearance; /** * @brief Flag to indicate if the robot is currently following the left wall. */ - bool left_wall{true}; + bool following_left{true}; /** * @brief Flag to indicate if the robot is currently following the right wall. */ - bool right_wall{true}; + bool following_right{true}; /** * @brief Last error measured by the left wall sensor, used to compute the derivative of the distance sensors. diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 5a18070..38b12ae 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -51,10 +51,10 @@ class TMaze : public core::ISerializable { GridPose get_next_goal(const GridPoint& position, bool returning) const; /** - * @brief Check the type of wall following the robot can do. + * @brief Detects walls around the robot at a given grid pose. * - * @param pose The pose of the robot. - * @return The type of wall following. + * @param pose The robotโ€™s current grid pose. + * @return The walls that are present around the robot. */ core::Observation get_observation(const GridPose& pose) const; @@ -70,7 +70,7 @@ class TMaze : public core::ISerializable { /** * @brief Calculate the best route to the goal using the current costmap. */ - void calculate_best_route(); + void compute_best_route(); /** * @brief Return the best route to the goal. @@ -106,7 +106,7 @@ class TMaze : public core::ISerializable { /** * @brief Calculate the costmap for the flood fill algorithm. */ - void calculate_costmap(); + void compute_costmap(); /** * @brief Return the cell at the given position. @@ -125,7 +125,7 @@ class TMaze : public core::ISerializable { Cell& get_cell(const GridPoint& position); /** - * @brief Update the probability of a wall in the maze. + * @brief Update the existence of a wall in the maze. * * @param pose The pose of the robot. * @param wall Whether there is a wall in front of the robot. diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 7346705..996c571 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -15,7 +15,7 @@ namespace micras::nav { /** - * @brief Class for calculating the robot odometry. + * @brief Class for computing the robot odometry. */ class Odometry { public: diff --git a/micras_nav/include/micras/nav/speed_controller.hpp b/micras_nav/include/micras/nav/speed_controller.hpp index 952ce93..845e0f4 100644 --- a/micras_nav/include/micras/nav/speed_controller.hpp +++ b/micras_nav/include/micras/nav/speed_controller.hpp @@ -12,7 +12,7 @@ namespace micras::nav { /** - * @brief Class to calculate a command to follow a pair of linear and angular speeds. + * @brief Class to compute a command to follow a pair of linear and angular speeds. */ class SpeedController { public: @@ -48,9 +48,10 @@ class SpeedController { * @param current_twist The current speeds of the robot. * @param desired_twist The desired speeds of the robot. * @param elapsed_time The time since the last update. - * @return A pair of floats representing the left and right wheel speeds. + * @return A pair of floats representing the left and right commands. */ - std::pair action(const Twist& current_twist, const Twist& desired_twist, float elapsed_time); + std::pair + compute_commands(const Twist& current_twist, const Twist& desired_twist, float elapsed_time); /** * @brief Reset the PID controllers. diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 04850c1..8d8e8b8 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -77,7 +77,7 @@ bool ActionQueuer::empty() const { return this->action_queue.empty(); } -void ActionQueuer::recalculate(const std::map>& best_route) { +void ActionQueuer::recompute(const std::map>& best_route) { this->action_queue = {}; this->action_queue.emplace(start); diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index f14cc64..f4535d2 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -14,33 +14,33 @@ FollowWall::FollowWall( post_threshold{config.post_threshold}, blind_pose{absolute_pose}, cell_size{config.cell_size}, - post_margin{config.post_margin} { } + post_clearance{config.post_clearance} { } -float FollowWall::action(float elapsed_time, float linear_speed) { +float FollowWall::compute_angular_correction(float elapsed_time, float linear_speed) { if (this->check_posts()) { return 0.0F; } - if ((not this->left_wall or not this->right_wall) and - (this->last_blind_distance >= (this->cell_size + (this->reset_by_post ? this->post_margin : 0.0F)))) { - this->left_wall = this->wall_sensors->get_wall(0); - this->right_wall = this->wall_sensors->get_wall(3); + if ((not this->following_left or not this->following_right) and + (this->last_blind_distance >= (this->cell_size + (this->reset_by_post ? this->post_clearance : 0.0F)))) { + this->following_left = this->wall_sensors->get_wall(0); + this->following_right = this->wall_sensors->get_wall(3); this->reset_displacement(); } float error{}; - if (this->left_wall and this->right_wall) { + if (this->following_left and this->following_right) { error = this->wall_sensors->get_sensor_error(1) - this->wall_sensors->get_sensor_error(2); - } else if (this->left_wall) { + } else if (this->following_left) { error = 2.0F * this->wall_sensors->get_sensor_error(1); - } else if (this->right_wall) { + } else if (this->following_right) { error = -2.0F * this->wall_sensors->get_sensor_error(2); } else { return 0.0F; } - const float response = this->pid.update(error, elapsed_time); + const float response = this->pid.compute_response(error, elapsed_time); return response * linear_speed / this->max_linear_speed; } @@ -56,22 +56,22 @@ bool FollowWall::check_posts() { this->last_blind_distance = current_distance; bool found_posts = false; - if (this->left_wall and - (this->wall_sensors->get_sensor_error(1) - this->last_left_error) / delta_distance <= this->post_threshold) { - this->left_wall = false; + if (this->following_left and + -(this->wall_sensors->get_sensor_error(1) - this->last_left_error) / delta_distance >= this->post_threshold) { + this->following_left = false; - if (this->right_wall) { + if (this->following_right) { this->reset_displacement(true); } found_posts = true; } - if (this->right_wall and - (this->wall_sensors->get_sensor_error(2) - this->last_right_error) / delta_distance <= this->post_threshold) { - this->right_wall = false; + if (this->following_right and + -(this->wall_sensors->get_sensor_error(2) - this->last_right_error) / delta_distance >= this->post_threshold) { + this->following_right = false; - if (this->left_wall) { + if (this->following_left) { this->reset_displacement(true); } @@ -93,7 +93,7 @@ void FollowWall::reset_displacement(bool reset_by_post) { void FollowWall::reset() { this->pid.reset(); this->reset_displacement(); - this->left_wall = this->wall_sensors->get_wall(1); - this->right_wall = this->wall_sensors->get_wall(2); + this->following_left = this->wall_sensors->get_wall(1); + this->following_right = this->wall_sensors->get_wall(2); } } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index e255a57..6441e3d 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -43,7 +43,7 @@ TMaze::TMaze(Config config) : start{config.start}, goal{config.go this->get_cell(position).cost = 0; } - this->calculate_costmap(); + this->compute_costmap(); } template @@ -52,7 +52,7 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ this->update_wall(pose, observation.front); this->update_wall(pose.turned_right(), observation.right); - this->calculate_costmap(); + this->compute_costmap(); } template @@ -128,7 +128,7 @@ bool TMaze::finished(const GridPoint& position, bool returning) c } template -void TMaze::calculate_best_route() { +void TMaze::compute_best_route() { GridPose current_pose = this->start; this->best_route.clear(); this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start); @@ -145,7 +145,7 @@ const std::map>& TMaze::get_be } template -void TMaze::calculate_costmap() { +void TMaze::compute_costmap() { std::array, height> visited{}; std::array, height> origin{}; std::queue queue; diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp index e68d57d..303986c 100644 --- a/micras_nav/src/speed_controller.cpp +++ b/micras_nav/src/speed_controller.cpp @@ -18,12 +18,12 @@ SpeedController::SpeedController(const Config& config) : right_feed_forward{config.right_feed_forward} { } std::pair - SpeedController::action(const Twist& current_twist, const Twist& desired_twist, float elapsed_time) { + SpeedController::compute_commands(const Twist& current_twist, const Twist& desired_twist, float elapsed_time) { this->linear_pid.set_setpoint(desired_twist.linear); this->angular_pid.set_setpoint(desired_twist.angular); - const float linear_command = this->linear_pid.update(current_twist.linear, elapsed_time); - const float angular_command = this->angular_pid.update(current_twist.angular, elapsed_time); + const float linear_command = this->linear_pid.compute_response(current_twist.linear, elapsed_time); + const float angular_command = this->angular_pid.compute_response(current_twist.angular, elapsed_time); const float linear_acceleration = (desired_twist.linear - this->last_linear_speed) / elapsed_time; const float angular_acceleration = (desired_twist.angular - this->last_angular_speed) / elapsed_time; diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 48fdbf4..72c6eb6 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -58,6 +58,7 @@ class TWallSensors { * @brief Get the observation from a sensor. * * @param sensor_index Index of the sensor. + * @param disturbed Whether or not there is another wall perpendicular to the one being measured. * @return True if the sensor detects a wall, false otherwise. */ bool get_wall(uint8_t sensor_index, bool disturbed = false) const; @@ -86,10 +87,10 @@ class TWallSensors { float get_adc_reading(uint8_t sensor_index) const; /** - * @brief Get the error of a sensor. + * @brief Get the deviation of a wall sensor reading from its calibrated baseline. * * @param sensor_index Index of the sensor. - * @return Error of the sensor. + * @return The reading error relative to the baseline; positive if above baseline. */ float get_sensor_error(uint8_t sensor_index) const; @@ -140,7 +141,7 @@ class TWallSensors { std::array base_readings; /** - * @brief Uncertainty of the wall sensors. + * @brief Ratio of the base reading to still consider as seeing a wall. */ float uncertainty; }; From f69afe98a033d02a870291c91845bef5be96ce4b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Apr 2025 09:50:53 -0300 Subject: [PATCH 327/561] =?UTF-8?q?=F0=9F=8E=A8=20Improve=20function=20nam?= =?UTF-8?q?es=20from=20states=20and=20actions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/fsm.hpp | 4 ++-- micras_core/src/fsm.cpp | 4 ++-- micras_nav/include/micras/nav/actions/base.hpp | 2 +- micras_nav/include/micras/nav/actions/move.hpp | 6 +++--- micras_nav/include/micras/nav/actions/turn.hpp | 2 +- micras_nav/include/micras/nav/state.hpp | 15 +++++++++++++++ micras_nav/src/follow_wall.cpp | 2 +- micras_nav/src/state.cpp | 8 ++++++++ 8 files changed, 33 insertions(+), 10 deletions(-) diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 1a187d3..88fcc2a 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -26,7 +26,7 @@ class FSM { * * @return The id of the next state. */ - virtual uint8_t run(uint8_t previous_state_id) = 0; + virtual uint8_t execute(uint8_t previous_state_id) = 0; /** * @brief Get the id object of the state. @@ -76,7 +76,7 @@ class FSM { /** * @brief Run the FSM current state to compute the next state. */ - void run(); + void update(); private: /** diff --git a/micras_core/src/fsm.cpp b/micras_core/src/fsm.cpp index 8664459..8cf7ea3 100644 --- a/micras_core/src/fsm.cpp +++ b/micras_core/src/fsm.cpp @@ -17,8 +17,8 @@ void FSM::add_state(std::unique_ptr state) { this->states.emplace(state->get_id(), std::move(state)); } -void FSM::run() { - const uint8_t next_state = this->states.at(this->current_state_id)->run(this->previous_state_id); +void FSM::update() { + const uint8_t next_state = this->states.at(this->current_state_id)->execute(this->previous_state_id); this->previous_state_id = this->current_state_id; this->current_state_id = next_state; } diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index ca6794d..9696f86 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -24,7 +24,7 @@ class Action { * @param pose The current pose of the robot. * @return The desired speeds for the robot. */ - virtual Twist get_twist(const Pose& pose) const = 0; + virtual Twist get_speeds(const Pose& pose) const = 0; /** * @brief Check if the action is finished. diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 67bd41e..8673dff 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -50,8 +50,8 @@ class MoveAction : public Action { * * @details The desired velocity is calculated from the linear displacement based on the Torricelli equation. */ - Twist get_twist(const Pose& pose) const override { - const float current_distance = pose.position.distance({0.0F, 0.0F}); + Twist get_speeds(const Pose& pose) const override { + const float current_distance = pose.position.magnitude(); Twist twist{}; if (current_distance < this->decelerate_distance) { @@ -74,7 +74,7 @@ class MoveAction : public Action { * @param pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - bool finished(const Pose& pose) const override { return pose.position.distance({0.0F, 0.0F}) >= this->distance; } + bool finished(const Pose& pose) const override { return pose.position.magnitude() >= this->distance; } /** * @brief Check if the action allows the robot to follow walls. diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 4823dae..a22aa5d 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -52,7 +52,7 @@ class TurnAction : public Action { * * @details The desired velocity is calculated from the angular displacement based on the Torricelli equation. */ - Twist get_twist(const Pose& pose) const override { + Twist get_speeds(const Pose& pose) const override { Twist twist{}; const float current_orientation = std::abs(pose.orientation); diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index 7c5113e..ad7d6f3 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -34,6 +34,13 @@ struct Point { */ float distance(const Point& other) const; + /** + * @brief Calculate the distance of the point to the origin. + * + * @return The distance of the point to the origin. + */ + float magnitude() const; + /** * @brief Calculate the angle between two points. * @@ -67,6 +74,14 @@ struct Point { */ Point move_towards(const Point& other, float distance) const; + /** + * @brief Add a point to another. + * + * @param other The point to add. + * @return The result of the addition. + */ + Point operator+(const Point& other) const; + /** * @brief Subtract a point from another. * diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index f4535d2..90d200d 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -46,7 +46,7 @@ float FollowWall::compute_angular_correction(float elapsed_time, float linear_sp } bool FollowWall::check_posts() { - const float current_distance = this->blind_pose.get().position.distance({0.0F, 0.0F}); + const float current_distance = this->blind_pose.get().position.magnitude(); const float delta_distance = current_distance - this->last_blind_distance; if (delta_distance <= 0.0F) { diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 8694877..a273ba5 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -12,6 +12,10 @@ float Point::distance(const Point& other) const { return std::hypot(other.x - this->x, other.y - this->y); } +float Point::magnitude() const { + return std::hypot(this->x, this->y); +} + float Point::angle_between(const Point& other) const { return std::atan2(other.y - this->y, other.x - this->x); } @@ -50,6 +54,10 @@ Point Point::move_towards(const Point& other, float distance) const { return {this->x + distance * std::cos(angle), this->y + distance * std::sin(angle)}; } +Point Point::operator+(const Point& other) const { + return {this->x + other.x, this->y + other.y}; +} + Point Point::operator-(const Point& other) const { return {this->x - other.x, this->y - other.y}; } From 8faa187f417282a9769d81c78e9a69fe0cf69e09 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Apr 2025 21:53:18 -0300 Subject: [PATCH 328/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20run=20function=20?= =?UTF-8?q?to=20Micras?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/speed_controller.hpp | 11 ++++++++++- micras_nav/src/speed_controller.cpp | 13 ++++++++----- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/micras_nav/include/micras/nav/speed_controller.hpp b/micras_nav/include/micras/nav/speed_controller.hpp index 845e0f4..9bbacda 100644 --- a/micras_nav/include/micras/nav/speed_controller.hpp +++ b/micras_nav/include/micras/nav/speed_controller.hpp @@ -51,7 +51,16 @@ class SpeedController { * @return A pair of floats representing the left and right commands. */ std::pair - compute_commands(const Twist& current_twist, const Twist& desired_twist, float elapsed_time); + compute_control_commands(const Twist& current_twist, const Twist& desired_twist, float elapsed_time); + + /** + * @brief Calculate the feed-forward command to achieve the desired speeds and accelerations. + * + * @param desired_twist The desired speeds of the robot. + * @param elapsed_time The time since the last update. + * @return A pair of floats representing the left and right feed-forward commands. + */ + std::pair compute_feed_forward_commands(const Twist& desired_twist, float elapsed_time); /** * @brief Reset the PID controllers. diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp index 303986c..678de32 100644 --- a/micras_nav/src/speed_controller.cpp +++ b/micras_nav/src/speed_controller.cpp @@ -17,14 +17,19 @@ SpeedController::SpeedController(const Config& config) : left_feed_forward{config.left_feed_forward}, right_feed_forward{config.right_feed_forward} { } -std::pair - SpeedController::compute_commands(const Twist& current_twist, const Twist& desired_twist, float elapsed_time) { +std::pair SpeedController::compute_control_commands( + const Twist& current_twist, const Twist& desired_twist, float elapsed_time +) { this->linear_pid.set_setpoint(desired_twist.linear); this->angular_pid.set_setpoint(desired_twist.angular); const float linear_command = this->linear_pid.compute_response(current_twist.linear, elapsed_time); const float angular_command = this->angular_pid.compute_response(current_twist.angular, elapsed_time); + return {linear_command - angular_command, linear_command + angular_command}; +} + +std::pair SpeedController::compute_feed_forward_commands(const Twist& desired_twist, float elapsed_time) { const float linear_acceleration = (desired_twist.linear - this->last_linear_speed) / elapsed_time; const float angular_acceleration = (desired_twist.angular - this->last_angular_speed) / elapsed_time; const Twist acceleration_twist{ @@ -38,9 +43,7 @@ std::pair const float left_feed_forward = feed_forward(desired_twist, acceleration_twist, this->left_feed_forward); const float right_feed_forward = feed_forward(desired_twist, acceleration_twist, this->right_feed_forward); - return { - left_feed_forward + linear_command - angular_command, right_feed_forward + linear_command + angular_command - }; + return {left_feed_forward, right_feed_forward}; } float SpeedController::feed_forward(const Twist& speed, const Twist& acceleration, const Config::FeedForward& config) { From e4fb633022f52bc5cd9e70b3ad0c78dccfbeb76b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Apr 2025 22:05:17 -0300 Subject: [PATCH 329/561] =?UTF-8?q?=F0=9F=9A=B8=20Add=20start=5Forientatio?= =?UTF-8?q?n=20to=20turn=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/turn.hpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index a22aa5d..6fdd35c 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -31,6 +31,7 @@ class TurnAction : public Action { * If linear speed is zero, a minimal angular speed is assigned. */ TurnAction(float angle, float curve_radius, float linear_speed, float max_angular_acceleration) : + start_orientation{max_angular_acceleration * 0.001F * 0.001F / 2.0F}, angle{angle}, linear_speed{linear_speed}, acceleration{max_angular_acceleration}, @@ -54,12 +55,10 @@ class TurnAction : public Action { */ Twist get_speeds(const Pose& pose) const override { Twist twist{}; - const float current_orientation = std::abs(pose.orientation); + const float current_orientation = std::max(std::abs(pose.orientation), this->start_orientation); if (current_orientation < std::abs(this->angle)) { - twist = { - linear_speed, std::sqrt(2.0F * this->acceleration * (current_orientation + this->acceleration / 2e6F)) - }; + twist = {linear_speed, std::sqrt(2.0F * this->acceleration * current_orientation)}; } else { twist = { linear_speed, std::sqrt(2.0F * this->acceleration * (std::abs(this->angle) - current_orientation)) @@ -88,6 +87,11 @@ class TurnAction : public Action { bool allow_follow_wall() const override { return false; } private: + /** + * @brief Start orientation in radians. Being zero causes the robot to not move. + */ + float start_orientation; + /** * @brief Angle to turn in radians. */ From 0025630058c8cef4ff07f85be3d7cf0cc9c1cc7a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 29 Apr 2025 12:10:48 -0300 Subject: [PATCH 330/561] =?UTF-8?q?=E2=9C=A8=20Add=20interface=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/state.hpp | 22 ----------------- micras_nav/src/state.cpp | 25 -------------------- micras_proxy/include/micras/proxy/button.hpp | 12 +++++----- micras_proxy/include/micras/proxy/fan.hpp | 4 ++-- micras_proxy/include/micras/proxy/imu.hpp | 6 ++--- 5 files changed, 11 insertions(+), 58 deletions(-) diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index ad7d6f3..8e8a24e 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -8,20 +8,6 @@ #include "micras/nav/grid_pose.hpp" namespace micras::nav { -/** - * @brief Possible directions in the maze. - */ -enum Direction : uint8_t { - EAST = 0, - NORTHEAST = 1, - NORTH = 2, - NORTHWEST = 3, - WEST = 4, - SOUTHWEST = 5, - SOUTH = 6, - SOUTHEAST = 7, -}; - /** * @brief Type to store a point in 2D space. */ @@ -57,14 +43,6 @@ struct Point { */ GridPoint to_grid(float cell_size) const; - /** - * @brief Rotate the point by a given angle. - * - * @param angle The angle to rotate the point by. - * @return The rotated point. - */ - Point rotate(Direction angle); - /** * @brief Move the point towards another point by a given distance. * diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index a273ba5..87c1888 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -24,31 +24,6 @@ GridPoint Point::to_grid(float cell_size) const { return {static_cast(this->x / cell_size), static_cast(this->y / cell_size)}; } -Point Point::rotate(Direction angle) { - constexpr float sqrt2_2 = std::numbers::sqrt2_v / 2.0F; - - switch (angle) { - case Direction::EAST: - return {-this->y, this->x}; - case Direction::NORTHEAST: - return {sqrt2_2 * (this->x - this->y), sqrt2_2 * (this->x + this->y)}; - case Direction::NORTH: - return *this; - case Direction::NORTHWEST: - return {sqrt2_2 * (this->x + this->y), sqrt2_2 * (-this->x + this->y)}; - case Direction::WEST: - return {this->y, -this->x}; - case Direction::SOUTHWEST: - return {sqrt2_2 * (-this->x + this->y), sqrt2_2 * (-this->x - this->y)}; - case Direction::SOUTH: - return {-this->x, -this->y}; - case Direction::SOUTHEAST: - return {sqrt2_2 * (-this->x - this->y), sqrt2_2 * (this->x - this->y)}; - } - - return *this; -} - Point Point::move_towards(const Point& other, float distance) const { const float angle = this->angle_between(other); return {this->x + distance * std::cos(angle), this->y + distance * std::sin(angle)}; diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index 8f68af1..b2120d8 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -20,18 +20,18 @@ class Button { * @brief Enum for button status. */ enum Status : uint8_t { - NO_PRESS, - SHORT_PRESS, - LONG_PRESS, - EXTRA_LONG_PRESS + NO_PRESS = 0, + SHORT_PRESS = 1, + LONG_PRESS = 2, + EXTRA_LONG_PRESS = 3 }; /** * @brief Enum for button pull resistor. */ enum PullResistor : uint8_t { - PULL_UP, - PULL_DOWN, + PULL_UP = 0, + PULL_DOWN = 1, }; /** diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index 8879831..01dccc8 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -66,8 +66,8 @@ class Fan { * @brief Enum for rotation direction. */ enum RotationDirection : uint8_t { - FORWARD, - BACKWARDS + FORWARD = 0, + BACKWARDS = 1 }; /** diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 47f4659..20891a2 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -37,9 +37,9 @@ class Imu { * @brief Enum to select the axis of the IMU. */ enum Axis : uint8_t { - X, - Y, - Z + X = 0, + Y = 1, + Z = 2 }; /** From 902a2767dcfcc081250356c09e843cd6d1f8edba Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 29 Apr 2025 13:58:00 -0300 Subject: [PATCH 331/561] =?UTF-8?q?=E2=9C=A8=20Add=20on=5Fentry=20function?= =?UTF-8?q?=20to=20states?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/fsm.hpp | 9 ++++++--- micras_core/src/fsm.cpp | 7 ++++++- micras_nav/src/action_queuer.cpp | 4 ++++ 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 88fcc2a..95023e0 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -19,14 +19,17 @@ class FSM { */ virtual ~State() = default; + /** + * @brief Execute the entry function of this state. + */ + virtual void on_entry() = 0; + /** * @brief Execute this state. * - * @param previous_state_id The id of the last executed state. - * * @return The id of the next state. */ - virtual uint8_t execute(uint8_t previous_state_id) = 0; + virtual uint8_t execute() = 0; /** * @brief Get the id object of the state. diff --git a/micras_core/src/fsm.cpp b/micras_core/src/fsm.cpp index 8cf7ea3..f0d1b4c 100644 --- a/micras_core/src/fsm.cpp +++ b/micras_core/src/fsm.cpp @@ -18,7 +18,12 @@ void FSM::add_state(std::unique_ptr state) { } void FSM::update() { - const uint8_t next_state = this->states.at(this->current_state_id)->execute(this->previous_state_id); + if (this->current_state_id != this->previous_state_id) { + this->states.at(this->current_state_id)->on_entry(); + } + + const uint8_t next_state = this->states.at(this->current_state_id)->execute(); + this->previous_state_id = this->current_state_id; this->current_state_id = next_state; } diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 8d8e8b8..11fc486 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -81,6 +81,10 @@ void ActionQueuer::recompute(const std::map>& this->action_queue = {}; this->action_queue.emplace(start); + if (best_route.empty()) { + return; + } + for (auto it = std::next(best_route.begin()); std::next(it) != best_route.end(); it++) { this->push(it->second, std::next(it)->second.position); } From cc9f2e76ef67d3e7c537adc124cad4ac40e4bedd Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 29 Apr 2025 14:09:21 -0300 Subject: [PATCH 332/561] =?UTF-8?q?=F0=9F=8E=A8=20Add=20sensor=20name=20en?= =?UTF-8?q?um=20to=20follow=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 10 ++++++++ micras_nav/src/follow_wall.cpp | 25 ++++++++++--------- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index b679048..ced5c9d 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -17,6 +17,16 @@ namespace micras::nav { */ class FollowWall { public: + /** + * @brief Name of each wall sensor. + */ + enum SensorName { + LEFT_FRONT = 0, + LEFT = 1, + RIGHT = 2, + RIGHT_FRONT = 3, + }; + /** * @brief Configuration struct for the FollowWall class. */ diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 90d200d..94f3562 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -23,19 +23,18 @@ float FollowWall::compute_angular_correction(float elapsed_time, float linear_sp if ((not this->following_left or not this->following_right) and (this->last_blind_distance >= (this->cell_size + (this->reset_by_post ? this->post_clearance : 0.0F)))) { - this->following_left = this->wall_sensors->get_wall(0); - this->following_right = this->wall_sensors->get_wall(3); - this->reset_displacement(); + this->reset(); } float error{}; if (this->following_left and this->following_right) { - error = this->wall_sensors->get_sensor_error(1) - this->wall_sensors->get_sensor_error(2); + error = this->wall_sensors->get_sensor_error(SensorName::LEFT) - + this->wall_sensors->get_sensor_error(SensorName::RIGHT); } else if (this->following_left) { - error = 2.0F * this->wall_sensors->get_sensor_error(1); + error = 2.0F * this->wall_sensors->get_sensor_error(SensorName::LEFT); } else if (this->following_right) { - error = -2.0F * this->wall_sensors->get_sensor_error(2); + error = -2.0F * this->wall_sensors->get_sensor_error(SensorName::RIGHT); } else { return 0.0F; } @@ -57,7 +56,8 @@ bool FollowWall::check_posts() { bool found_posts = false; if (this->following_left and - -(this->wall_sensors->get_sensor_error(1) - this->last_left_error) / delta_distance >= this->post_threshold) { + -(this->wall_sensors->get_sensor_error(SensorName::LEFT) - this->last_left_error) / delta_distance >= + this->post_threshold) { this->following_left = false; if (this->following_right) { @@ -68,7 +68,8 @@ bool FollowWall::check_posts() { } if (this->following_right and - -(this->wall_sensors->get_sensor_error(2) - this->last_right_error) / delta_distance >= this->post_threshold) { + -(this->wall_sensors->get_sensor_error(SensorName::RIGHT) - this->last_right_error) / delta_distance >= + this->post_threshold) { this->following_right = false; if (this->following_left) { @@ -78,8 +79,8 @@ bool FollowWall::check_posts() { found_posts = true; } - this->last_left_error = this->wall_sensors->get_sensor_error(1); - this->last_right_error = this->wall_sensors->get_sensor_error(2); + this->last_left_error = this->wall_sensors->get_sensor_error(SensorName::LEFT); + this->last_right_error = this->wall_sensors->get_sensor_error(SensorName::RIGHT); return found_posts; } @@ -93,7 +94,7 @@ void FollowWall::reset_displacement(bool reset_by_post) { void FollowWall::reset() { this->pid.reset(); this->reset_displacement(); - this->following_left = this->wall_sensors->get_wall(1); - this->following_right = this->wall_sensors->get_wall(2); + this->following_left = this->wall_sensors->get_wall(SensorName::LEFT); + this->following_right = this->wall_sensors->get_wall(SensorName::RIGHT); } } // namespace micras::nav From ff966bf2132da74735bd4c19b520f54e4bc3e262 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 29 Apr 2025 14:19:59 -0300 Subject: [PATCH 333/561] =?UTF-8?q?=F0=9F=9A=B8=20Add=20enum=20for=20the?= =?UTF-8?q?=20action=20types?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 15 ++++++++++- .../include/micras/nav/actions/base.hpp | 21 ++++++++++++++- .../include/micras/nav/actions/move.hpp | 3 ++- .../include/micras/nav/actions/turn.hpp | 3 ++- micras_nav/src/action_queuer.cpp | 26 +++++++++---------- 5 files changed, 51 insertions(+), 17 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 752aa2f..3828ceb 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -18,6 +18,19 @@ namespace micras::nav { */ class ActionQueuer { public: + /** + * @brief Enum for the exploration action types. + */ + enum ActionType { + STOP = 0, + START = 1, + MOVE_FORWARD = 2, + MOVE_HALF = 3, + TURN_LEFT = 4, + TURN_RIGHT = 5, + TURN_BACK = 6, + }; + /** * @brief Configuration struct for the ActionQueuer class. */ @@ -91,9 +104,9 @@ class ActionQueuer { * @brief Pre-built actions to use in the exploration. */ ///@{ + std::shared_ptr stop; std::shared_ptr start; std::shared_ptr move_forward; - std::shared_ptr stop; std::shared_ptr move_half; std::shared_ptr turn_left; std::shared_ptr turn_right; diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 9696f86..93f24ea 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -13,6 +13,13 @@ namespace micras::nav { */ class Action { public: + /** + * @brief Constructor for the Action class. + * + * @param action_id The ID of the action. + */ + explicit Action(uint8_t action_id) : id(action_id) { } + /** * @brief Virtual destructor for the Action class. */ @@ -41,17 +48,29 @@ class Action { */ virtual bool allow_follow_wall() const = 0; + /** + * @brief Get the ID of the action. + * + * @return The ID of the action. + */ + uint8_t get_id() const { return id; } + protected: /** * @brief Special member functions declared as default. */ ///@{ - Action() = default; Action(const Action&) = default; Action(Action&&) = default; Action& operator=(const Action&) = default; Action& operator=(Action&&) = default; ///@} + +private: + /** + * @brief The ID of the action. + */ + uint8_t id; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 8673dff..0270688 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -27,9 +27,10 @@ class MoveAction : public Action { * @param follow_wall Whether the robot can follow wall while executing this action. */ MoveAction( - float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, + uint8_t action_id, float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, float max_deceleration, bool follow_wall = true ) : + Action{action_id}, distance(distance), start_speed_2(start_speed * start_speed), end_speed_2(end_speed * end_speed), diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 6fdd35c..2999706 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -30,7 +30,8 @@ class TurnAction : public Action { * maintains the desired radius of curvature throughout the entire turn. * If linear speed is zero, a minimal angular speed is assigned. */ - TurnAction(float angle, float curve_radius, float linear_speed, float max_angular_acceleration) : + TurnAction(uint8_t action_id, float angle, float curve_radius, float linear_speed, float max_angular_acceleration) : + Action{action_id}, start_orientation{max_angular_acceleration * 0.001F * 0.001F / 2.0F}, angle{angle}, linear_speed{linear_speed}, diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 11fc486..5846db4 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -11,36 +11,36 @@ ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, exploring_params{config.exploring}, // solving_params{config.solving}, + stop{std::make_shared( + ActionType::STOP, cell_size / 2.0F, exploring_params.max_linear_speed, 0.0F, exploring_params.max_linear_speed, + exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false + )}, start{std::make_shared( - cell_size - config.start_offset, 0.001F * exploring_params.max_linear_acceleration, + ActionType::START, cell_size - config.start_offset, 0.001F * exploring_params.max_linear_acceleration, exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration )}, move_forward{std::make_shared( - cell_size, exploring_params.max_linear_speed, exploring_params.max_linear_speed, + ActionType::MOVE_FORWARD, cell_size, exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration )}, - stop{std::make_shared( - cell_size / 2.0F, exploring_params.max_linear_speed, 0.0F, exploring_params.max_linear_speed, - exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false - )}, move_half{std::make_shared( - cell_size / 2.0F, 0.001F * exploring_params.max_linear_acceleration, exploring_params.max_linear_speed, - exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, + ActionType::MOVE_HALF, cell_size / 2.0F, 0.001F * exploring_params.max_linear_acceleration, + exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false )}, turn_left{std::make_shared( - std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, + ActionType::TURN_LEFT, std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, exploring_params.max_angular_acceleration )}, turn_right{std::make_shared( - -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, + ActionType::TURN_RIGHT, -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, exploring_params.max_angular_acceleration )}, - turn_back{ - std::make_shared(std::numbers::pi_v, 0.0F, 0.0F, exploring_params.max_angular_acceleration) - }, + turn_back{std::make_shared( + ActionType::TURN_BACK, std::numbers::pi_v, 0.0F, 0.0F, exploring_params.max_angular_acceleration + )}, action_queue{{start}} { } void ActionQueuer::push(const GridPose& current_pose, const GridPoint& target_position) { From 800400cf8cdf2500f462194b54fa76970848e84b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 29 Apr 2025 14:22:56 -0300 Subject: [PATCH 334/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/action_queuer.hpp | 2 +- micras_nav/include/micras/nav/follow_wall.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 3828ceb..925e383 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -21,7 +21,7 @@ class ActionQueuer { /** * @brief Enum for the exploration action types. */ - enum ActionType { + enum ActionType : uint8_t { STOP = 0, START = 1, MOVE_FORWARD = 2, diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index ced5c9d..29a7903 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -20,7 +20,7 @@ class FollowWall { /** * @brief Name of each wall sensor. */ - enum SensorName { + enum SensorName : uint8_t { LEFT_FRONT = 0, LEFT = 1, RIGHT = 2, From 77aa3ff1537ab142600862ec2573b6cb68ece210 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 29 Apr 2025 17:21:12 -0300 Subject: [PATCH 335/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20observation=20to?= =?UTF-8?q?=20follow=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 7 ++++++ micras_nav/src/follow_wall.cpp | 11 +++++++++ .../include/micras/proxy/wall_sensors.hpp | 21 ++-------------- micras_proxy/src/wall_sensors.cpp | 24 ++----------------- 4 files changed, 22 insertions(+), 41 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 29a7903..2a46d1c 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -58,6 +58,13 @@ class FollowWall { */ float compute_angular_correction(float elapsed_time, float linear_speed); + /** + * @brief Get the observation of the walls around the robot. + * + * @return Observations from all sensors. + */ + core::Observation get_observation() const; + /** * @brief Reset the PID controller and the relative pose. */ diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 94f3562..ef6a782 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -85,6 +85,17 @@ bool FollowWall::check_posts() { return found_posts; } +core::Observation FollowWall::get_observation() const { + if (this->wall_sensors->get_wall(SensorName::LEFT_FRONT) && this->wall_sensors->get_wall(SensorName::RIGHT_FRONT)) { + return { + this->wall_sensors->get_wall(SensorName::LEFT, true), true, + this->wall_sensors->get_wall(SensorName::RIGHT, true) + }; + } + + return {this->wall_sensors->get_wall(SensorName::LEFT), false, this->wall_sensors->get_wall(SensorName::RIGHT)}; +} + void FollowWall::reset_displacement(bool reset_by_post) { this->blind_pose.reset_reference(); this->last_blind_distance = 0.0F; diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 72c6eb6..0670fee 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -63,13 +63,6 @@ class TWallSensors { */ bool get_wall(uint8_t sensor_index, bool disturbed = false) const; - /** - * @brief Get the observations from all sensors. - * - * @return Observations from all sensors. - */ - core::Observation get_observation() const; - /** * @brief Get the reading from a sensor. * @@ -95,19 +88,9 @@ class TWallSensors { float get_sensor_error(uint8_t sensor_index) const; /** - * @brief Calibrate the wall sensors for a wall at the front. - */ - void calibrate_front_wall(); - - /** - * @brief Calibrate the wall sensors for a wall at the left. - */ - void calibrate_left_wall(); - - /** - * @brief Calibrate the wall sensors for a wall at the right. + * @brief Calibrate a wall sensor base reading. */ - void calibrate_right_wall(); + void calibrate_sensor(uint8_t sensor_index); private: /** diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index b4f2949..ba70139 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -46,15 +46,6 @@ bool TWallSensors::get_wall(uint8_t sensor_index, bool disturbed this->base_readings.at(sensor_index) * this->uncertainty * (disturbed ? 1.2F : 1.0F); } -template -core::Observation TWallSensors::get_observation() const { - if (this->get_wall(0) && this->get_wall(3)) { - return {this->get_wall(1, true), true, this->get_wall(2, true)}; - } - - return {this->get_wall(1), false, this->get_wall(2)}; -} - template float TWallSensors::get_reading(uint8_t sensor_index) const { return this->filters.at(sensor_index).get_last(); @@ -73,19 +64,8 @@ float TWallSensors::get_sensor_error(uint8_t sensor_index) const } template -void TWallSensors::calibrate_front_wall() { - this->base_readings[0] = this->get_reading(0); - this->base_readings[3] = this->get_reading(3); -} - -template -void TWallSensors::calibrate_left_wall() { - this->base_readings[1] = this->get_reading(1); -} - -template -void TWallSensors::calibrate_right_wall() { - this->base_readings[2] = this->get_reading(2); +void TWallSensors::calibrate_sensor(uint8_t sensor_index) { + this->base_readings[sensor_index] = this->get_reading(sensor_index); } } // namespace micras::proxy From 20ef6b37449b1851842c6500883a973272588312 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 30 Apr 2025 01:20:21 -0300 Subject: [PATCH 336/561] =?UTF-8?q?=F0=9F=8E=A8=20Apply=20suggestions=20fr?= =?UTF-8?q?om=20code=20review?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi <62271285+PedroDeSanti@users.noreply.github.com> --- micras_nav/include/micras/nav/actions/move.hpp | 12 ++++++++---- micras_nav/include/micras/nav/actions/turn.hpp | 8 ++++++-- micras_nav/include/micras/nav/maze.hpp | 2 +- micras_nav/src/maze.cpp | 6 +++--- micras_nav/src/speed_controller.cpp | 4 ++-- micras_proxy/include/micras/proxy/imu.hpp | 5 +++++ micras_proxy/src/button.cpp | 1 + micras_proxy/src/wall_sensors.cpp | 13 ++++++++----- 8 files changed, 34 insertions(+), 17 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 8673dff..b7ecb81 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -43,7 +43,7 @@ class MoveAction : public Action { } { } /** - * @brief Get the desired speeds for the robot to compete the action. + * @brief Get the desired speeds for the robot to complete the action. * * @param pose The current pose of the robot. * @return The desired speeds for the robot to complete the action. @@ -55,11 +55,15 @@ class MoveAction : public Action { Twist twist{}; if (current_distance < this->decelerate_distance) { - twist = {std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance), 0.0F}; + twist = { + .linear = std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance), + .angular = 0.0F, + }; } else { twist = { - std::sqrt(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), - 0.0F + .linear = + std::sqrt(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), + .angular = 0.0F, }; } diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 6fdd35c..f77d79c 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -58,10 +58,14 @@ class TurnAction : public Action { const float current_orientation = std::max(std::abs(pose.orientation), this->start_orientation); if (current_orientation < std::abs(this->angle)) { - twist = {linear_speed, std::sqrt(2.0F * this->acceleration * current_orientation)}; + twist = { + .linear = linear_speed, + .angular = std::sqrt(2.0F * this->acceleration * current_orientation), + }; } else { twist = { - linear_speed, std::sqrt(2.0F * this->acceleration * (std::abs(this->angle) - current_orientation)) + .linear = linear_speed, + .angular = std::sqrt(2.0F * this->acceleration * (std::abs(this->angle) - current_orientation)), }; } diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 38b12ae..2d09f95 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -128,7 +128,7 @@ class TMaze : public core::ISerializable { * @brief Update the existence of a wall in the maze. * * @param pose The pose of the robot. - * @param wall Whether there is a wall in front of the robot. + * @param wall Whether there is a wall at the front of a given pose. */ void update_wall(const GridPose& pose, bool wall); diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 6441e3d..54908f1 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -13,19 +13,19 @@ namespace micras::nav { template TMaze::TMaze(Config config) : start{config.start}, goal{config.goal} { + // Add left and right boundary walls for (uint8_t row = 0; row < height; row++) { this->cells[row][0].walls[Side::LEFT] = true; this->cells[row][width - 1].walls[Side::RIGHT] = true; } + // Add up and down boundary walls for (uint8_t col = 0; col < width; col++) { this->cells[0][col].walls[Side::DOWN] = true; this->cells[height - 1][col].walls[Side::UP] = true; } - this->cells[start.position.y][start.position.x].walls[start.turned_right().orientation] = true; - GridPose right_pose = start.turned_right().front(); - this->get_cell(right_pose.position).walls[right_pose.turned_back().orientation] = true; + this->update_wall(start.turned_right(), true); // Hardcoded walls at the end region if (width == 16 and height == 16) { diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp index 678de32..fe4b31a 100644 --- a/micras_nav/src/speed_controller.cpp +++ b/micras_nav/src/speed_controller.cpp @@ -33,8 +33,8 @@ std::pair SpeedController::compute_feed_forward_commands(const Twi const float linear_acceleration = (desired_twist.linear - this->last_linear_speed) / elapsed_time; const float angular_acceleration = (desired_twist.angular - this->last_angular_speed) / elapsed_time; const Twist acceleration_twist{ - std::clamp(linear_acceleration, -this->max_linear_acceleration, this->max_linear_acceleration), - std::clamp(angular_acceleration, -this->max_angular_acceleration, this->max_angular_acceleration) + .linear = std::clamp(linear_acceleration, -this->max_linear_acceleration, this->max_linear_acceleration), + .angular = std::clamp(angular_acceleration, -this->max_angular_acceleration, this->max_angular_acceleration), }; this->last_linear_speed = desired_twist.linear; diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 47f4659..76b42e0 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -75,6 +75,11 @@ class Imu { */ void calibrate(); + /** + * @brief Check if IMU was initialized. + * + * @return True if the device was successfully initialized, false otherwise. + */ bool was_initialized() const; private: diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp index ae0dab2..86b9e45 100644 --- a/micras_proxy/src/button.cpp +++ b/micras_proxy/src/button.cpp @@ -59,6 +59,7 @@ void Button::update_state() { this->is_debouncing = false; } } else { + this->is_debouncing = false; this->current_state = raw_reading; } } diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index b4f2949..91fc32d 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -48,11 +48,14 @@ bool TWallSensors::get_wall(uint8_t sensor_index, bool disturbed template core::Observation TWallSensors::get_observation() const { - if (this->get_wall(0) && this->get_wall(3)) { - return {this->get_wall(1, true), true, this->get_wall(2, true)}; - } - - return {this->get_wall(1), false, this->get_wall(2)}; + bool front_wall = this->get_wall(0) and this->get_wall(3); + bool disturbed = front_wall; + + return { + .left = this->get_wall(1, disturbed), + .front = front_wall, + .right = this->get_wall(2, disturbed), + }; } template From 4f8119d61e0a6d926da2000478cc4159fe801f95 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 30 Apr 2025 01:32:48 -0300 Subject: [PATCH 337/561] =?UTF-8?q?=F0=9F=8E=A8=20Add=20max=5Fangular=5Fsp?= =?UTF-8?q?eed=20calculation=20function=20to=20turn=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi <62271285+PedroDeSanti@users.noreply.github.com> --- .../include/micras/nav/actions/turn.hpp | 46 +++++++++++++------ 1 file changed, 33 insertions(+), 13 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index f77d79c..69d7546 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -11,6 +11,11 @@ #include "micras/nav/actions/base.hpp" namespace micras::nav { +/** + * @brief Correction factor to adjust the maximum angular speed. + * + * @details This factor is determined by analyzing the graph of the maximum angular speed. + */ static constexpr float correction_factor{0.95F}; /** @@ -25,25 +30,13 @@ class TurnAction : public Action { * @param curve_radius Radius of the curve in meters. * @param linear_speed Linear speed in m/s. * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. - * - * @details Maximum angular velocity is computed to generate a curve equivalent in displacement to one that - * maintains the desired radius of curvature throughout the entire turn. - * If linear speed is zero, a minimal angular speed is assigned. */ TurnAction(float angle, float curve_radius, float linear_speed, float max_angular_acceleration) : start_orientation{max_angular_acceleration * 0.001F * 0.001F / 2.0F}, angle{angle}, linear_speed{linear_speed}, acceleration{max_angular_acceleration}, - max_angular_speed{ - (linear_speed == 0.0F) ? - (max_angular_acceleration * 0.01F) : - (correction_factor * max_angular_acceleration * - (curve_radius / linear_speed - std::sqrt( - std::pow(curve_radius / linear_speed, 2.0F) - - 2.0F / (correction_factor * max_angular_acceleration) - ))) - } { } + max_angular_speed{calculate_max_angular_speed(angle, curve_radius, linear_speed, max_angular_acceleration)} { } /** * @brief Get the desired speeds for the robot to complete the action. @@ -91,6 +84,33 @@ class TurnAction : public Action { bool allow_follow_wall() const override { return false; } private: + /** + * @brief Calculate the maximum angular speed for a given curve radius and linear speed. + * + * @param angle Angle to turn in radians. + * @param curve_radius Radius of the curve in meters. + * @param linear_speed Linear speed in m/s. + * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * @return Maximum angular speed in rad/s. + * + * @details Maximum angular velocity is computed to generate a curve equivalent in displacement to one that + * maintains the desired radius of curvature throughout the entire turn. + * If linear speed is zero, a minimal angular speed is assigned. + */ + static constexpr float calculate_max_angular_speed( + float angle, float curve_radius, float linear_speed, float max_angular_acceleration + ) { + if (linear_speed == 0.0F) { + return max_angular_acceleration * 0.01F; + } + + float radius_speed_ratio = curve_radius / linear_speed; + float discriminant = std::pow(radius_speed_ratio, 2.0F) - + (2.0F * (1 - std::cos(angle))) / (correction_factor * max_angular_acceleration); + + return correction_factor * max_angular_acceleration * (radius_speed_ratio - std::sqrt(discriminant)); + } + /** * @brief Start orientation in radians. Being zero causes the robot to not move. */ From 21cde1f8226eb89cbf5cbb9472cafe20acd7c7da Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 30 Apr 2025 01:51:51 -0300 Subject: [PATCH 338/561] =?UTF-8?q?=F0=9F=8E=A8=20Add=20enum=20to=20wall?= =?UTF-8?q?=20sensors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/actions/turn.hpp | 6 +-- micras_nav/include/micras/nav/follow_wall.hpp | 17 +++++++ micras_nav/src/follow_wall.cpp | 45 ++++++++++++------- .../include/micras/proxy/wall_sensors.hpp | 22 +-------- micras_proxy/src/wall_sensors.cpp | 27 +---------- 5 files changed, 54 insertions(+), 63 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 69d7546..254670a 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -104,9 +104,9 @@ class TurnAction : public Action { return max_angular_acceleration * 0.01F; } - float radius_speed_ratio = curve_radius / linear_speed; - float discriminant = std::pow(radius_speed_ratio, 2.0F) - - (2.0F * (1 - std::cos(angle))) / (correction_factor * max_angular_acceleration); + const float radius_speed_ratio = curve_radius / linear_speed; + const float discriminant = std::pow(radius_speed_ratio, 2.0F) - + (2.0F * (1 - std::cos(angle))) / (correction_factor * max_angular_acceleration); return correction_factor * max_angular_acceleration * (radius_speed_ratio - std::sqrt(discriminant)); } diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index b679048..3b1596f 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -17,6 +17,16 @@ namespace micras::nav { */ class FollowWall { public: + /** + * @brief Name of each wall sensor. + */ + enum SensorName { + LEFT_FRONT = 0, + LEFT = 1, + RIGHT = 2, + RIGHT_FRONT = 3, + }; + /** * @brief Configuration struct for the FollowWall class. */ @@ -48,6 +58,13 @@ class FollowWall { */ float compute_angular_correction(float elapsed_time, float linear_speed); + /** + * @brief Get the observation of the walls around the robot. + * + * @return Observations from all sensors. + */ + core::Observation get_observation() const; + /** * @brief Reset the PID controller and the relative pose. */ diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 90d200d..9285c71 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -23,19 +23,20 @@ float FollowWall::compute_angular_correction(float elapsed_time, float linear_sp if ((not this->following_left or not this->following_right) and (this->last_blind_distance >= (this->cell_size + (this->reset_by_post ? this->post_clearance : 0.0F)))) { - this->following_left = this->wall_sensors->get_wall(0); - this->following_right = this->wall_sensors->get_wall(3); + this->following_left = this->wall_sensors->get_wall(SensorName::LEFT); + this->following_right = this->wall_sensors->get_wall(SensorName::RIGHT); this->reset_displacement(); } float error{}; if (this->following_left and this->following_right) { - error = this->wall_sensors->get_sensor_error(1) - this->wall_sensors->get_sensor_error(2); + error = this->wall_sensors->get_sensor_error(SensorName::LEFT) - + this->wall_sensors->get_sensor_error(SensorName::RIGHT); } else if (this->following_left) { - error = 2.0F * this->wall_sensors->get_sensor_error(1); + error = 2.0F * this->wall_sensors->get_sensor_error(SensorName::LEFT); } else if (this->following_right) { - error = -2.0F * this->wall_sensors->get_sensor_error(2); + error = -2.0F * this->wall_sensors->get_sensor_error(SensorName::RIGHT); } else { return 0.0F; } @@ -57,7 +58,8 @@ bool FollowWall::check_posts() { bool found_posts = false; if (this->following_left and - -(this->wall_sensors->get_sensor_error(1) - this->last_left_error) / delta_distance >= this->post_threshold) { + -(this->wall_sensors->get_sensor_error(SensorName::LEFT) - this->last_left_error) / delta_distance >= + this->post_threshold) { this->following_left = false; if (this->following_right) { @@ -68,7 +70,8 @@ bool FollowWall::check_posts() { } if (this->following_right and - -(this->wall_sensors->get_sensor_error(2) - this->last_right_error) / delta_distance >= this->post_threshold) { + -(this->wall_sensors->get_sensor_error(SensorName::RIGHT) - this->last_right_error) / delta_distance >= + this->post_threshold) { this->following_right = false; if (this->following_left) { @@ -78,22 +81,34 @@ bool FollowWall::check_posts() { found_posts = true; } - this->last_left_error = this->wall_sensors->get_sensor_error(1); - this->last_right_error = this->wall_sensors->get_sensor_error(2); + this->last_left_error = this->wall_sensors->get_sensor_error(SensorName::LEFT); + this->last_right_error = this->wall_sensors->get_sensor_error(SensorName::RIGHT); return found_posts; } -void FollowWall::reset_displacement(bool reset_by_post) { - this->blind_pose.reset_reference(); - this->last_blind_distance = 0.0F; - this->reset_by_post = reset_by_post; +core::Observation FollowWall::get_observation() const { + bool front_wall = + this->wall_sensors->get_wall(SensorName::LEFT_FRONT) and this->wall_sensors->get_wall(SensorName::RIGHT_FRONT); + bool disturbed = front_wall; + + return { + .left = this->wall_sensors->get_wall(SensorName::LEFT, disturbed), + .front = front_wall, + .right = this->wall_sensors->get_wall(SensorName::RIGHT, disturbed), + }; } void FollowWall::reset() { this->pid.reset(); this->reset_displacement(); - this->following_left = this->wall_sensors->get_wall(1); - this->following_right = this->wall_sensors->get_wall(2); + this->following_left = this->wall_sensors->get_wall(SensorName::LEFT); + this->following_right = this->wall_sensors->get_wall(SensorName::RIGHT); +} + +void FollowWall::reset_displacement(bool reset_by_post) { + this->blind_pose.reset_reference(); + this->last_blind_distance = 0.0F; + this->reset_by_post = reset_by_post; } } // namespace micras::nav diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 72c6eb6..3ce9053 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -9,7 +9,6 @@ #include #include "micras/core/butterworth_filter.hpp" -#include "micras/core/types.hpp" #include "micras/hal/adc_dma.hpp" #include "micras/hal/pwm.hpp" @@ -63,13 +62,6 @@ class TWallSensors { */ bool get_wall(uint8_t sensor_index, bool disturbed = false) const; - /** - * @brief Get the observations from all sensors. - * - * @return Observations from all sensors. - */ - core::Observation get_observation() const; - /** * @brief Get the reading from a sensor. * @@ -95,19 +87,9 @@ class TWallSensors { float get_sensor_error(uint8_t sensor_index) const; /** - * @brief Calibrate the wall sensors for a wall at the front. - */ - void calibrate_front_wall(); - - /** - * @brief Calibrate the wall sensors for a wall at the left. - */ - void calibrate_left_wall(); - - /** - * @brief Calibrate the wall sensors for a wall at the right. + * @brief Calibrate a wall sensor base reading. */ - void calibrate_right_wall(); + void calibrate_sensor(uint8_t sensor_index); private: /** diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index 91fc32d..c14b568 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -46,18 +46,6 @@ bool TWallSensors::get_wall(uint8_t sensor_index, bool disturbed this->base_readings.at(sensor_index) * this->uncertainty * (disturbed ? 1.2F : 1.0F); } -template -core::Observation TWallSensors::get_observation() const { - bool front_wall = this->get_wall(0) and this->get_wall(3); - bool disturbed = front_wall; - - return { - .left = this->get_wall(1, disturbed), - .front = front_wall, - .right = this->get_wall(2, disturbed), - }; -} - template float TWallSensors::get_reading(uint8_t sensor_index) const { return this->filters.at(sensor_index).get_last(); @@ -76,19 +64,8 @@ float TWallSensors::get_sensor_error(uint8_t sensor_index) const } template -void TWallSensors::calibrate_front_wall() { - this->base_readings[0] = this->get_reading(0); - this->base_readings[3] = this->get_reading(3); -} - -template -void TWallSensors::calibrate_left_wall() { - this->base_readings[1] = this->get_reading(1); -} - -template -void TWallSensors::calibrate_right_wall() { - this->base_readings[2] = this->get_reading(2); +void TWallSensors::calibrate_sensor(uint8_t sensor_index) { + this->base_readings.at(sensor_index) = this->get_reading(sensor_index); } } // namespace micras::proxy From d9e7774a0a451dbf2b37aef812ab5689801d8f6d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 30 Apr 2025 02:03:25 -0300 Subject: [PATCH 339/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= =?UTF-8?q?=20on=20FollowWall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 2 +- micras_nav/src/follow_wall.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 3b1596f..2a46d1c 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -20,7 +20,7 @@ class FollowWall { /** * @brief Name of each wall sensor. */ - enum SensorName { + enum SensorName : uint8_t { LEFT_FRONT = 0, LEFT = 1, RIGHT = 2, diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 9285c71..e26ee8d 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -88,9 +88,9 @@ bool FollowWall::check_posts() { } core::Observation FollowWall::get_observation() const { - bool front_wall = + const bool front_wall = this->wall_sensors->get_wall(SensorName::LEFT_FRONT) and this->wall_sensors->get_wall(SensorName::RIGHT_FRONT); - bool disturbed = front_wall; + const bool disturbed = front_wall; return { .left = this->wall_sensors->get_wall(SensorName::LEFT, disturbed), From 308e4b47fe7b47ab3bbeed45092905b446b99c05 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 30 Apr 2025 11:35:25 -0300 Subject: [PATCH 340/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20comments=20from?= =?UTF-8?q?=20Maze=20constructor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 54908f1..f92fa10 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -13,13 +13,11 @@ namespace micras::nav { template TMaze::TMaze(Config config) : start{config.start}, goal{config.goal} { - // Add left and right boundary walls for (uint8_t row = 0; row < height; row++) { this->cells[row][0].walls[Side::LEFT] = true; this->cells[row][width - 1].walls[Side::RIGHT] = true; } - // Add up and down boundary walls for (uint8_t col = 0; col < width; col++) { this->cells[0][col].walls[Side::DOWN] = true; this->cells[height - 1][col].walls[Side::UP] = true; From cb7f1fcd7b8b2829418d9efd57631196be07730e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 30 Apr 2025 11:50:39 -0300 Subject: [PATCH 341/561] =?UTF-8?q?=F0=9F=9A=B8=20Add=20WallSensorIndex=20?= =?UTF-8?q?struct=20to=20core?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/types.hpp | 10 ++++++ micras_nav/include/micras/nav/follow_wall.hpp | 17 ++++------ micras_nav/src/follow_wall.cpp | 33 ++++++++++--------- 3 files changed, 34 insertions(+), 26 deletions(-) diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index 326c76a..5a29bba 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -25,6 +25,16 @@ enum Objective : uint8_t { RETURN = 1, SOLVE = 2 }; + +/** + * @brief Struct to store the index of each of the wall sensors. + */ +struct WallSensorsIndex { + uint8_t left_front{}; + uint8_t left{}; + uint8_t right{}; + uint8_t right_front{}; +}; } // namespace micras::core #endif // MICRAS_CORE_TYPES_HPP diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 2a46d1c..09ceff9 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -8,6 +8,7 @@ #include #include "micras/core/pid_controller.hpp" +#include "micras/core/types.hpp" #include "micras/nav/state.hpp" #include "micras/proxy/wall_sensors.hpp" @@ -17,21 +18,12 @@ namespace micras::nav { */ class FollowWall { public: - /** - * @brief Name of each wall sensor. - */ - enum SensorName : uint8_t { - LEFT_FRONT = 0, - LEFT = 1, - RIGHT = 2, - RIGHT_FRONT = 3, - }; - /** * @brief Configuration struct for the FollowWall class. */ struct Config { core::PidController::Config pid; + core::WallSensorsIndex wall_sensor_index{}; float max_linear_speed{}; float post_threshold{}; float cell_size{}; @@ -98,6 +90,11 @@ class FollowWall { */ core::PidController pid; + /** + * @brief Index of each of the wall sensors used for wall following. + */ + core::WallSensorsIndex sensor_index; + /** * @brief Maximum linear speed of the robot. */ diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index e26ee8d..5ab620e 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -10,6 +10,7 @@ FollowWall::FollowWall( ) : wall_sensors{wall_sensors}, pid{config.pid}, + sensor_index{config.wall_sensor_index}, max_linear_speed{config.max_linear_speed}, post_threshold{config.post_threshold}, blind_pose{absolute_pose}, @@ -23,20 +24,20 @@ float FollowWall::compute_angular_correction(float elapsed_time, float linear_sp if ((not this->following_left or not this->following_right) and (this->last_blind_distance >= (this->cell_size + (this->reset_by_post ? this->post_clearance : 0.0F)))) { - this->following_left = this->wall_sensors->get_wall(SensorName::LEFT); - this->following_right = this->wall_sensors->get_wall(SensorName::RIGHT); + this->following_left = this->wall_sensors->get_wall(this->sensor_index.left); + this->following_right = this->wall_sensors->get_wall(this->sensor_index.right); this->reset_displacement(); } float error{}; if (this->following_left and this->following_right) { - error = this->wall_sensors->get_sensor_error(SensorName::LEFT) - - this->wall_sensors->get_sensor_error(SensorName::RIGHT); + error = this->wall_sensors->get_sensor_error(this->sensor_index.left) - + this->wall_sensors->get_sensor_error(this->sensor_index.right); } else if (this->following_left) { - error = 2.0F * this->wall_sensors->get_sensor_error(SensorName::LEFT); + error = 2.0F * this->wall_sensors->get_sensor_error(this->sensor_index.left); } else if (this->following_right) { - error = -2.0F * this->wall_sensors->get_sensor_error(SensorName::RIGHT); + error = -2.0F * this->wall_sensors->get_sensor_error(this->sensor_index.right); } else { return 0.0F; } @@ -58,7 +59,7 @@ bool FollowWall::check_posts() { bool found_posts = false; if (this->following_left and - -(this->wall_sensors->get_sensor_error(SensorName::LEFT) - this->last_left_error) / delta_distance >= + -(this->wall_sensors->get_sensor_error(this->sensor_index.left) - this->last_left_error) / delta_distance >= this->post_threshold) { this->following_left = false; @@ -70,7 +71,7 @@ bool FollowWall::check_posts() { } if (this->following_right and - -(this->wall_sensors->get_sensor_error(SensorName::RIGHT) - this->last_right_error) / delta_distance >= + -(this->wall_sensors->get_sensor_error(this->sensor_index.right) - this->last_right_error) / delta_distance >= this->post_threshold) { this->following_right = false; @@ -81,29 +82,29 @@ bool FollowWall::check_posts() { found_posts = true; } - this->last_left_error = this->wall_sensors->get_sensor_error(SensorName::LEFT); - this->last_right_error = this->wall_sensors->get_sensor_error(SensorName::RIGHT); + this->last_left_error = this->wall_sensors->get_sensor_error(this->sensor_index.left); + this->last_right_error = this->wall_sensors->get_sensor_error(this->sensor_index.right); return found_posts; } core::Observation FollowWall::get_observation() const { - const bool front_wall = - this->wall_sensors->get_wall(SensorName::LEFT_FRONT) and this->wall_sensors->get_wall(SensorName::RIGHT_FRONT); + const bool front_wall = this->wall_sensors->get_wall(this->sensor_index.left_front) and + this->wall_sensors->get_wall(this->sensor_index.right_front); const bool disturbed = front_wall; return { - .left = this->wall_sensors->get_wall(SensorName::LEFT, disturbed), + .left = this->wall_sensors->get_wall(this->sensor_index.left, disturbed), .front = front_wall, - .right = this->wall_sensors->get_wall(SensorName::RIGHT, disturbed), + .right = this->wall_sensors->get_wall(this->sensor_index.right, disturbed), }; } void FollowWall::reset() { this->pid.reset(); this->reset_displacement(); - this->following_left = this->wall_sensors->get_wall(SensorName::LEFT); - this->following_right = this->wall_sensors->get_wall(SensorName::RIGHT); + this->following_left = this->wall_sensors->get_wall(this->sensor_index.left); + this->following_right = this->wall_sensors->get_wall(this->sensor_index.right); } void FollowWall::reset_displacement(bool reset_by_post) { From 7530d31fc3546e5fe9d0c757ac14eef3344413a3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 1 May 2025 18:50:44 -0300 Subject: [PATCH 342/561] =?UTF-8?q?=E2=9C=A8=20Add=20lint=20target?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cmake/run_clang_tidy.sh | 94 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 94 insertions(+) create mode 100755 cmake/run_clang_tidy.sh diff --git a/cmake/run_clang_tidy.sh b/cmake/run_clang_tidy.sh new file mode 100755 index 0000000..5fe3036 --- /dev/null +++ b/cmake/run_clang_tidy.sh @@ -0,0 +1,94 @@ +#!/bin/bash + +# Usage: ./run_clang_tidy.sh [-j NUM_JOBS] [--fix] file1.cpp file2.cpp ... + +# Default to using all available logical cores +NUM_JOBS=$(nproc) +FIX_FLAG="" +EXIT_CODE=0 + +# Parse the arguments +while [[ $# -gt 0 ]]; do + case "$1" in + -j) + if [[ -n "$2" && "$2" =~ ^[0-9]+$ ]]; then + NUM_JOBS="$2" + shift + else + echo "Error: Invalid number of jobs specified with -j." + exit 1 + fi + ;; + --fix) + FIX_FLAG="--fix" + ;; + -*) + echo "Error: Unknown option $1" + exit 1 + ;; + *) + FILES+=("$1") + ;; + esac + shift +done + +# Ensure at least one file is provided +if [[ "${#FILES[@]}" -lt 1 ]]; then + echo "Usage: $0 [-j NUM_JOBS] [--fix] file1.cpp file2.cpp ..." + exit 1 +fi + +# Initialize a semaphore to limit the number of parallel jobs +SEMAPHORE_FILE="/tmp/run_clang_tidy_semaphore_$$" +mkfifo "$SEMAPHORE_FILE" +exec 3<>"$SEMAPHORE_FILE" +rm "$SEMAPHORE_FILE" + +# Add tokens to the semaphore for job limiting +for ((i = 0; i < NUM_JOBS; i++)); do + echo "token" >&3 +done + +# Function to run clang-tidy on a single file +run_clang_tidy() { + local file="$1" + + # Wait for a token (block if none are available) + read -r -u 3 + + # Run clang-tidy + echo "Running clang-tidy on $file..." + clang-tidy $FIX_FLAG --quiet -p build "$file" + if [[ $? -ne 0 ]]; then + echo "clang-tidy failed on $file" >&2 + EXIT_CODE=1 + fi + + # Return the token to the semaphore + echo "token" >&3 +} + +# Export variables and functions for subshells +export FIX_FLAG EXIT_CODE +export -f run_clang_tidy + +# Track background jobs +declare -a PIDS + +# Process each file +for file in "${FILES[@]}"; do + run_clang_tidy "$file" & + PIDS+=("$!") +done + +# Wait for all background jobs to finish +for pid in "${PIDS[@]}"; do + wait "$pid" || EXIT_CODE=1 +done + +# Close the semaphore +exec 3>&- + +# Return the aggregated exit code +exit $EXIT_CODE From ef50a072d6f69404e10725551241911624be8dbe Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 1 May 2025 20:00:37 -0300 Subject: [PATCH 343/561] =?UTF-8?q?=F0=9F=9A=B8=20Simplify=20linter=20scri?= =?UTF-8?q?pt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cmake/run_clang_tidy.sh | 72 ++++------------------------------------- 1 file changed, 7 insertions(+), 65 deletions(-) diff --git a/cmake/run_clang_tidy.sh b/cmake/run_clang_tidy.sh index 5fe3036..bdc146e 100755 --- a/cmake/run_clang_tidy.sh +++ b/cmake/run_clang_tidy.sh @@ -1,94 +1,36 @@ #!/bin/bash -# Usage: ./run_clang_tidy.sh [-j NUM_JOBS] [--fix] file1.cpp file2.cpp ... - -# Default to using all available logical cores -NUM_JOBS=$(nproc) FIX_FLAG="" EXIT_CODE=0 -# Parse the arguments -while [[ $# -gt 0 ]]; do - case "$1" in - -j) - if [[ -n "$2" && "$2" =~ ^[0-9]+$ ]]; then - NUM_JOBS="$2" - shift - else - echo "Error: Invalid number of jobs specified with -j." - exit 1 - fi - ;; - --fix) - FIX_FLAG="--fix" - ;; - -*) - echo "Error: Unknown option $1" - exit 1 - ;; - *) - FILES+=("$1") - ;; - esac +if [[ "$1" == "--fix" ]]; then + FIX_FLAG="--fix" shift -done - -# Ensure at least one file is provided -if [[ "${#FILES[@]}" -lt 1 ]]; then - echo "Usage: $0 [-j NUM_JOBS] [--fix] file1.cpp file2.cpp ..." - exit 1 fi -# Initialize a semaphore to limit the number of parallel jobs -SEMAPHORE_FILE="/tmp/run_clang_tidy_semaphore_$$" -mkfifo "$SEMAPHORE_FILE" -exec 3<>"$SEMAPHORE_FILE" -rm "$SEMAPHORE_FILE" - -# Add tokens to the semaphore for job limiting -for ((i = 0; i < NUM_JOBS; i++)); do - echo "token" >&3 -done - -# Function to run clang-tidy on a single file run_clang_tidy() { - local file="$1" - - # Wait for a token (block if none are available) - read -r -u 3 + local output=$(clang-tidy $FIX_FLAG --quiet -p build $1 2>&1) - # Run clang-tidy - echo "Running clang-tidy on $file..." - clang-tidy $FIX_FLAG --quiet -p build "$file" if [[ $? -ne 0 ]]; then - echo "clang-tidy failed on $file" >&2 + echo "clang-tidy failed on $1" >&2 + echo "$output" >&2 EXIT_CODE=1 fi - - # Return the token to the semaphore - echo "token" >&3 } -# Export variables and functions for subshells export FIX_FLAG EXIT_CODE export -f run_clang_tidy -# Track background jobs declare -a PIDS +echo "Running clang-tidy on all source files..." -# Process each file -for file in "${FILES[@]}"; do +for file in "$@"; do run_clang_tidy "$file" & PIDS+=("$!") done -# Wait for all background jobs to finish for pid in "${PIDS[@]}"; do wait "$pid" || EXIT_CODE=1 done -# Close the semaphore -exec 3>&- - -# Return the aggregated exit code exit $EXIT_CODE From b1f8febefce34d16da8f52c8972e004e6865ec27 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 1 May 2025 21:35:40 -0300 Subject: [PATCH 344/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20run=5Fclang=5Ftid?= =?UTF-8?q?y=20to=20scripts=20folder?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cmake/{ => scripts}/run_clang_tidy.sh | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename cmake/{ => scripts}/run_clang_tidy.sh (100%) diff --git a/cmake/run_clang_tidy.sh b/cmake/scripts/run_clang_tidy.sh similarity index 100% rename from cmake/run_clang_tidy.sh rename to cmake/scripts/run_clang_tidy.sh From f4c85bbed12631c2179a9711cc1e59cbada5c8d5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 1 May 2025 22:08:28 -0300 Subject: [PATCH 345/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20run=5Fclang=5Ftidy?= =?UTF-8?q?=20script?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cmake/scripts/run_clang_tidy.sh | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/cmake/scripts/run_clang_tidy.sh b/cmake/scripts/run_clang_tidy.sh index bdc146e..1bc5c26 100755 --- a/cmake/scripts/run_clang_tidy.sh +++ b/cmake/scripts/run_clang_tidy.sh @@ -9,18 +9,15 @@ if [[ "$1" == "--fix" ]]; then fi run_clang_tidy() { - local output=$(clang-tidy $FIX_FLAG --quiet -p build $1 2>&1) + local output + output=$(clang-tidy $FIX_FLAG --quiet -p build $1 2>&1) if [[ $? -ne 0 ]]; then - echo "clang-tidy failed on $1" >&2 - echo "$output" >&2 - EXIT_CODE=1 + echo "$output" + return 1 fi } -export FIX_FLAG EXIT_CODE -export -f run_clang_tidy - declare -a PIDS echo "Running clang-tidy on all source files..." From 9ed01c0ed9a3ae1b0a5c2988eb005a9254ddae68 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 1 May 2025 22:58:51 -0300 Subject: [PATCH 346/561] =?UTF-8?q?=F0=9F=9A=B8=20Make=20run=5Fclang=5Ftid?= =?UTF-8?q?y=20a=20script=20template?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../run_clang_tidy.sh => templates/run_clang_tidy.sh.in} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename cmake/{scripts/run_clang_tidy.sh => templates/run_clang_tidy.sh.in} (82%) diff --git a/cmake/scripts/run_clang_tidy.sh b/cmake/templates/run_clang_tidy.sh.in similarity index 82% rename from cmake/scripts/run_clang_tidy.sh rename to cmake/templates/run_clang_tidy.sh.in index 1bc5c26..39b9ac6 100755 --- a/cmake/scripts/run_clang_tidy.sh +++ b/cmake/templates/run_clang_tidy.sh.in @@ -10,7 +10,7 @@ fi run_clang_tidy() { local output - output=$(clang-tidy $FIX_FLAG --quiet -p build $1 2>&1) + output=$(clang-tidy $FIX_FLAG --use-color --quiet -p @CMAKE_CURRENT_BINARY_DIR@ $1 2>&1) if [[ $? -ne 0 ]]; then echo "$output" From 6419344502f17ce0f870ec2c114e61705b19ca0b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 1 May 2025 23:43:26 -0300 Subject: [PATCH 347/561] =?UTF-8?q?=F0=9F=92=84=20Improve=20run=5Fclang=5F?= =?UTF-8?q?tidy=20script?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cmake/templates/run_clang_tidy.sh.in | 42 +++++++++++++--------------- 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/cmake/templates/run_clang_tidy.sh.in b/cmake/templates/run_clang_tidy.sh.in index 39b9ac6..1b183ee 100755 --- a/cmake/templates/run_clang_tidy.sh.in +++ b/cmake/templates/run_clang_tidy.sh.in @@ -1,33 +1,29 @@ #!/bin/bash FIX_FLAG="" -EXIT_CODE=0 if [[ "$1" == "--fix" ]]; then - FIX_FLAG="--fix" - shift + FIX_FLAG="--fix" + shift fi -run_clang_tidy() { - local output - output=$(clang-tidy $FIX_FLAG --use-color --quiet -p @CMAKE_CURRENT_BINARY_DIR@ $1 2>&1) - - if [[ $? -ne 0 ]]; then - echo "$output" - return 1 - fi -} - -declare -a PIDS echo "Running clang-tidy on all source files..." -for file in "$@"; do - run_clang_tidy "$file" & - PIDS+=("$!") -done - -for pid in "${PIDS[@]}"; do - wait "$pid" || EXIT_CODE=1 -done +ERRORS=$( + { + for file in "$@"; do + { + clang-tidy $FIX_FLAG --use-color --quiet -p @CMAKE_CURRENT_BINARY_DIR@ "$file" 2>&1 + } & + done + + wait + } | grep -v ' warnings generated\.$' | awk '!seen[$0]++' +) + +if [[ -n "$ERRORS" ]]; then + echo -e "\n$ERRORS\033[0m" + exit 1 +fi -exit $EXIT_CODE +exit 0 From 5cfe90b9432fd0cea1b5a2cdc9c079d774d0b220 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 1 May 2025 23:52:40 -0300 Subject: [PATCH 348/561] =?UTF-8?q?=F0=9F=8E=A8=20Format=20shell=20script?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cmake/templates/run_clang_tidy.sh.in | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/cmake/templates/run_clang_tidy.sh.in b/cmake/templates/run_clang_tidy.sh.in index 1b183ee..119d374 100755 --- a/cmake/templates/run_clang_tidy.sh.in +++ b/cmake/templates/run_clang_tidy.sh.in @@ -3,27 +3,25 @@ FIX_FLAG="" if [[ "$1" == "--fix" ]]; then - FIX_FLAG="--fix" - shift + FIX_FLAG="--fix" + shift fi echo "Running clang-tidy on all source files..." ERRORS=$( - { - for file in "$@"; do - { - clang-tidy $FIX_FLAG --use-color --quiet -p @CMAKE_CURRENT_BINARY_DIR@ "$file" 2>&1 - } & - done + { + for file in "$@"; do + { + clang-tidy $FIX_FLAG --use-color --quiet -p @CMAKE_CURRENT_BINARY_DIR@ "$file" 2>&1 + } & + done - wait - } | grep -v ' warnings generated\.$' | awk '!seen[$0]++' + wait + } | grep -v ' warnings generated\.$' | awk '!seen[$0]++' ) if [[ -n "$ERRORS" ]]; then - echo -e "\n$ERRORS\033[0m" - exit 1 + echo -e "\n$ERRORS\033[0m" + exit 1 fi - -exit 0 From ee3e0da907b40e94363f3496412b869580107caa Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 2 May 2025 00:20:54 -0300 Subject: [PATCH 349/561] =?UTF-8?q?=F0=9F=93=9D=20Add=20missing=20action?= =?UTF-8?q?=5Fis=20param=20to=20actions=20documentation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/move.hpp | 1 + micras_nav/include/micras/nav/actions/turn.hpp | 1 + 2 files changed, 2 insertions(+) diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index acfffb9..fa9a7e3 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -18,6 +18,7 @@ class MoveAction : public Action { /** * @brief Construct a new Move Action object. * + * @param action_id The ID of the action. * @param distance Distance to move in meters. * @param start_speed Initial speed in m/s. * @param end_speed Final speed in m/s. diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 6e43612..c7a2a18 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -26,6 +26,7 @@ class TurnAction : public Action { /** * @brief Construct a new Turn Action object. * + * @param action_id The ID of the action. * @param angle Angle to turn in radians. * @param curve_radius Radius of the curve in meters. * @param linear_speed Linear speed in m/s. From db109713fdcd6d5b4657c835ee3d706712105ad1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 2 May 2025 00:46:19 -0300 Subject: [PATCH 350/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20recompute=20funct?= =?UTF-8?q?ion=20to=20prepare?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/action_queuer.hpp | 2 +- micras_nav/src/action_queuer.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 925e383..2c37781 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -116,7 +116,7 @@ class ActionQueuer { /** * @brief Queue of actions to be performed. */ - std::queue> action_queue; + std::queue> action_queue{}; }; } // namespace micras::nav diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 5846db4..0aeff27 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -40,8 +40,7 @@ ActionQueuer::ActionQueuer(Config config) : )}, turn_back{std::make_shared( ActionType::TURN_BACK, std::numbers::pi_v, 0.0F, 0.0F, exploring_params.max_angular_acceleration - )}, - action_queue{{start}} { } + )} { } void ActionQueuer::push(const GridPose& current_pose, const GridPoint& target_position) { if (current_pose.front().position == target_position) { From 60882da9683dc25e95dccc4815f48607fda85ef7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 2 May 2025 00:59:13 -0300 Subject: [PATCH 351/561] =?UTF-8?q?=F0=9F=9A=A8=20Remove=20initializer=20f?= =?UTF-8?q?rom=20action=5Fqueuer=20queue?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/action_queuer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 2c37781..925e383 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -116,7 +116,7 @@ class ActionQueuer { /** * @brief Queue of actions to be performed. */ - std::queue> action_queue{}; + std::queue> action_queue; }; } // namespace micras::nav From a367c06af7a0485a125f26b101fe747140f66376 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 3 May 2025 12:55:36 -0300 Subject: [PATCH 352/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20Point=20to=20core?= =?UTF-8?q?=20as=20Vector?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/vector.hpp | 89 +++++++++++++++++ micras_core/src/vector.cpp | 43 +++++++++ micras_nav/include/micras/nav/grid_pose.hpp | 18 ++++ micras_nav/include/micras/nav/odometry.hpp | 8 +- micras_nav/include/micras/nav/state.hpp | 101 +------------------- micras_nav/src/follow_wall.cpp | 4 + micras_nav/src/grid_pose.cpp | 8 ++ micras_nav/src/odometry.cpp | 8 +- micras_nav/src/state.cpp | 47 +-------- 9 files changed, 178 insertions(+), 148 deletions(-) create mode 100644 micras_core/include/micras/core/vector.hpp create mode 100644 micras_core/src/vector.cpp diff --git a/micras_core/include/micras/core/vector.hpp b/micras_core/include/micras/core/vector.hpp new file mode 100644 index 0000000..3f8d706 --- /dev/null +++ b/micras_core/include/micras/core/vector.hpp @@ -0,0 +1,89 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_VECTOR_HPP +#define MICRAS_CORE_VECTOR_HPP + +namespace micras::core { +/** + * @brief Type to store a point in 2D space. + */ +struct Vector { + /** + * @brief Calculate the distance to another point. + * + * @param other The other point. + * @return The distance to the other point. + */ + float distance(const Vector& other) const; + + /** + * @brief Calculate the distance of the point to the origin. + * + * @return The distance of the point to the origin. + */ + float magnitude() const; + + /** + * @brief Calculate the angle between two points. + * + * @param other The other point. + * @return The angle between the two points. + */ + float angle_between(const Vector& other) const; + + /** + * @brief Move the point towards another point by a given distance. + * + * @param other The point to move towards. + * @param distance The distance to move. + * @return The point after moving towards the other point. + */ + Vector move_towards(const Vector& other, float distance) const; + + /** + * @brief Add a point to another. + * + * @param other The point to add. + * @return The result of the addition. + */ + Vector operator+(const Vector& other) const; + + /** + * @brief Subtract a point from another. + * + * @param other The point to subtract. + * @return The result of the subtraction. + */ + Vector operator-(const Vector& other) const; + + /** + * @brief Compare two points for equality. + * + * @param other The other point to compare. + * @return True if the points are equal, false otherwise. + */ + bool operator==(const Vector& other) const; + + /** + * @brief Calculate the remainder of the division of the point by a value. + * + * @param value The value to divide the point by. + * @return The remainder of the division. + */ + Vector operator%(float value) const; + + /** + * @brief The x coordinate of the point in 2D space. + */ + float x; + + /** + * @brief The y coordinate of the point in 2D space. + */ + float y; +}; +} // namespace micras::core + +#endif // MICRAS_CORE_VECTOR_HPP diff --git a/micras_core/src/vector.cpp b/micras_core/src/vector.cpp new file mode 100644 index 0000000..e11f419 --- /dev/null +++ b/micras_core/src/vector.cpp @@ -0,0 +1,43 @@ +/** + * @file + */ + +#include +#include + +#include "micras/core/vector.hpp" + +namespace micras::core { +float Vector::distance(const Vector& other) const { + return std::hypot(other.x - this->x, other.y - this->y); +} + +float Vector::magnitude() const { + return std::hypot(this->x, this->y); +} + +float Vector::angle_between(const Vector& other) const { + return std::atan2(other.y - this->y, other.x - this->x); +} + +Vector Vector::move_towards(const Vector& other, float distance) const { + const float angle = this->angle_between(other); + return {this->x + distance * std::cos(angle), this->y + distance * std::sin(angle)}; +} + +Vector Vector::operator+(const Vector& other) const { + return {this->x + other.x, this->y + other.y}; +} + +Vector Vector::operator-(const Vector& other) const { + return {this->x - other.x, this->y - other.y}; +} + +bool Vector::operator==(const Vector& other) const { + return this->x == other.x and this->y == other.y; +} + +Vector Vector::operator%(float value) const { + return {std::fmod(this->x, value), std::fmod(this->y, value)}; +} +} // namespace micras::core diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index 54c630e..99390f4 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -9,6 +9,7 @@ #include #include "micras/core/types.hpp" +#include "micras/core/vector.hpp" namespace micras::nav { /** @@ -55,6 +56,23 @@ struct GridPoint { */ Side direction(const GridPoint& next) const; + /** + * @brief Convert the point to a grid point. + * + * @param cell_size The size of the grid cells. + * @return The grid point corresponding to the point. + */ + static GridPoint from_vector(const core::Vector& point, float cell_size); + + /** + * @brief Convert a grid point to a point. + * + * @param grid_point The grid point to convert. + * @param cell_size The size of the grid cells. + * @return The point corresponding to the grid point. + */ + core::Vector to_vector(float cell_size) const; + /** * @brief Move in the grid in the direction of the side. * diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 996c571..e958c82 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -37,8 +37,8 @@ class Odometry { * @param config Configuration for the odometry. */ Odometry( - const std::shared_ptr& left_rotary_sensor, - const std::shared_ptr& right_rotary_sensor, const std::shared_ptr& imu, + const std::shared_ptr& left_rotary_sensor, + const std::shared_ptr& right_rotary_sensor, const std::shared_ptr& imu, Config config ); @@ -72,12 +72,12 @@ class Odometry { /** * @brief Left rotary sensor. */ - std::shared_ptr left_rotary_sensor; + std::shared_ptr left_rotary_sensor; /** * @brief Right rotary sensor. */ - std::shared_ptr right_rotary_sensor; + std::shared_ptr right_rotary_sensor; /** * @brief IMU sensor. diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index 8e8a24e..1a8fa21 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -5,105 +5,10 @@ #ifndef MICRAS_NAV_POSE_HPP #define MICRAS_NAV_POSE_HPP +#include "micras/core/vector.hpp" #include "micras/nav/grid_pose.hpp" namespace micras::nav { -/** - * @brief Type to store a point in 2D space. - */ -struct Point { - /** - * @brief Calculate the distance to another point. - * - * @param other The other point. - * @return The distance to the other point. - */ - float distance(const Point& other) const; - - /** - * @brief Calculate the distance of the point to the origin. - * - * @return The distance of the point to the origin. - */ - float magnitude() const; - - /** - * @brief Calculate the angle between two points. - * - * @param other The other point. - * @return The angle between the two points. - */ - float angle_between(const Point& other) const; - - /** - * @brief Convert the point to a grid point. - * - * @param cell_size The size of the grid cells. - * @return The grid point corresponding to the point. - */ - GridPoint to_grid(float cell_size) const; - - /** - * @brief Move the point towards another point by a given distance. - * - * @param other The point to move towards. - * @param distance The distance to move. - * @return The point after moving towards the other point. - */ - Point move_towards(const Point& other, float distance) const; - - /** - * @brief Add a point to another. - * - * @param other The point to add. - * @return The result of the addition. - */ - Point operator+(const Point& other) const; - - /** - * @brief Subtract a point from another. - * - * @param other The point to subtract. - * @return The result of the subtraction. - */ - Point operator-(const Point& other) const; - - /** - * @brief Compare two points for equality. - * - * @param other The other point to compare. - * @return True if the points are equal, false otherwise. - */ - bool operator==(const Point& other) const; - - /** - * @brief Calculate the remainder of the division of the point by a value. - * - * @param value The value to divide the point by. - * @return The remainder of the division. - */ - Point operator%(float value) const; - - /** - * @brief Convert a grid point to a point. - * - * @param grid_point The grid point to convert. - * @param cell_size The size of the grid cells. - * @return The point corresponding to the grid point. - */ - static Point from_grid(const GridPoint& grid_point, float cell_size); - - /** - * @brief The x coordinate of the point in 2D space. - */ - float x; - - /** - * @brief The y coordinate of the point in 2D space. - */ - float y; -}; - /** * @brief Type to store a pose in 2D space. */ @@ -122,12 +27,12 @@ struct Pose { * @param cell_size The size of the grid cells. * @return The point inside the cell reference. */ - Point to_cell(float cell_size) const; + core::Vector to_cell(float cell_size) const; /** * @brief The position of the pose. */ - Point position; + core::Vector position; /** * @brief The orientation of the pose. diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 2024ac2..1b5f177 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -18,6 +18,10 @@ FollowWall::FollowWall( post_clearance{config.post_clearance} { } float FollowWall::compute_angular_correction(float elapsed_time, float linear_speed) { + if (this->wall_sensors.use_count() == 1) { + this->wall_sensors->update(); + } + if (this->check_posts()) { return 0.0F; } diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index 88b9262..ecb21b1 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -32,6 +32,14 @@ Side GridPoint::direction(const GridPoint& next) const { return Side::UP; } +GridPoint GridPoint::from_vector(const core::Vector& point, float cell_size) { + return {static_cast(point.x / cell_size), static_cast(point.y / cell_size)}; +} + +core::Vector GridPoint::to_vector(float cell_size) const { + return {cell_size * (this->x + 0.5F), cell_size * (this->y + 0.5F)}; +} + GridPoint GridPoint::operator+(const Side& side) const { switch (side) { case Side::RIGHT: diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index a97c8c6..e1f7505 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -10,8 +10,8 @@ namespace micras::nav { Odometry::Odometry( - const std::shared_ptr& left_rotary_sensor, - const std::shared_ptr& right_rotary_sensor, const std::shared_ptr& imu, + const std::shared_ptr& left_rotary_sensor, + const std::shared_ptr& right_rotary_sensor, const std::shared_ptr& imu, Config config ) : left_rotary_sensor{left_rotary_sensor}, @@ -24,6 +24,10 @@ Odometry::Odometry( state{config.initial_pose, {0.0F, 0.0F}} { } void Odometry::update(float elapsed_time) { + if (this->imu.use_count() == 1) { + this->imu->update(); + } + const float left_position = this->left_rotary_sensor->get_position(); const float right_position = this->right_rotary_sensor->get_position(); diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 87c1888..15eba52 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -8,53 +8,12 @@ #include "micras/nav/state.hpp" namespace micras::nav { -float Point::distance(const Point& other) const { - return std::hypot(other.x - this->x, other.y - this->y); -} - -float Point::magnitude() const { - return std::hypot(this->x, this->y); -} - -float Point::angle_between(const Point& other) const { - return std::atan2(other.y - this->y, other.x - this->x); -} - -GridPoint Point::to_grid(float cell_size) const { - return {static_cast(this->x / cell_size), static_cast(this->y / cell_size)}; -} - -Point Point::move_towards(const Point& other, float distance) const { - const float angle = this->angle_between(other); - return {this->x + distance * std::cos(angle), this->y + distance * std::sin(angle)}; -} - -Point Point::operator+(const Point& other) const { - return {this->x + other.x, this->y + other.y}; -} - -Point Point::operator-(const Point& other) const { - return {this->x - other.x, this->y - other.y}; -} - -bool Point::operator==(const Point& other) const { - return this->x == other.x and this->y == other.y; -} - -Point Point::operator%(float value) const { - return {std::fmod(this->x, value), std::fmod(this->y, value)}; -} - -Point Point::from_grid(const GridPoint& grid_point, float cell_size) { - return {cell_size * (grid_point.x + 0.5F), cell_size * (grid_point.y + 0.5F)}; -} - GridPose Pose::to_grid(float cell_size) const { - return {this->position.to_grid(cell_size), angle_to_grid(this->orientation)}; + return {GridPoint::from_vector(this->position, cell_size), angle_to_grid(this->orientation)}; }; -Point Pose::to_cell(float cell_size) const { - const Point cell_position = this->position % cell_size; +core::Vector Pose::to_cell(float cell_size) const { + const core::Vector cell_position = this->position % cell_size; float cell_advance = 0.0F; float cell_alignment = 0.0F; From 4a2f727846b12938f8cb062adf02bcb92311093a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 3 May 2025 16:20:27 -0300 Subject: [PATCH 353/561] =?UTF-8?q?=F0=9F=94=A5=20Delete=20Information=20s?= =?UTF-8?q?truct?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/grid_pose.hpp | 14 -------------- micras_nav/src/maze.cpp | 8 ++++++-- 2 files changed, 6 insertions(+), 16 deletions(-) diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index 99390f4..b483609 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -30,20 +30,6 @@ enum Side : uint8_t { */ Side angle_to_grid(float angle); -/** - * @brief Type to store information originating from the wall sensors. - */ -struct Information { - /** - * @brief Possible walls in the grid to be checked. - */ - core::Observation left; - core::Observation front_left; - core::Observation front; - core::Observation front_right; - core::Observation right; -}; - /** * @brief Type to store a point in the grid. */ diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index f92fa10..02ef0d2 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -26,7 +26,7 @@ TMaze::TMaze(Config config) : start{config.start}, goal{config.go this->update_wall(start.turned_right(), true); // Hardcoded walls at the end region - if (width == 16 and height == 16) { + if constexpr (width == 16 and height == 16) { this->update_wall({{7, 7}, Side::DOWN}, true); this->update_wall({{8, 7}, Side::DOWN}, true); // this->update_wall({{8, 7}, Side::RIGHT}, true); @@ -117,7 +117,11 @@ bool TMaze::has_wall(const GridPose& pose) const { template core::Observation TMaze::get_observation(const GridPose& pose) const { - return {this->has_wall(pose.turned_left()), this->has_wall(pose), this->has_wall(pose.turned_right())}; + return { + .left = this->has_wall(pose.turned_left()), + .front = this->has_wall(pose), + .right = this->has_wall(pose.turned_right()) + }; } template From 235ee72b9726975f1ed758cd062b3b660e12c827 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 3 May 2025 22:19:06 -0300 Subject: [PATCH 354/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Improve=20flood=20?= =?UTF-8?q?fill=20function?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 13 ++- micras_nav/src/maze.cpp | 124 ++++++++++++++++--------- 2 files changed, 90 insertions(+), 47 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 2d09f95..a1bf45e 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -48,7 +48,7 @@ class TMaze : public core::ISerializable { * @param returning Whether the robot is returning to the start position. * @return The next point the robot should go to when exploring. */ - GridPose get_next_goal(const GridPoint& position, bool returning) const; + GridPose get_next_goal(const GridPoint& position) const; /** * @brief Detects walls around the robot at a given grid pose. @@ -67,6 +67,8 @@ class TMaze : public core::ISerializable { */ bool finished(const GridPoint& position, bool returning) const; + void compute_return_costmap(); + /** * @brief Calculate the best route to the goal using the current costmap. */ @@ -100,13 +102,16 @@ class TMaze : public core::ISerializable { */ struct Cell { std::array walls{}; - uint16_t cost{0xFFFF}; + int16_t cost{0x7FFF}; + bool visited{}; }; /** * @brief Calculate the costmap for the flood fill algorithm. */ - void compute_costmap(); + void compute_costmap(const GridPoint& reference); + + void update_cost(const GridPoint& reference); /** * @brief Return the cell at the given position. @@ -130,7 +135,7 @@ class TMaze : public core::ISerializable { * @param pose The pose of the robot. * @param wall Whether there is a wall at the front of a given pose. */ - void update_wall(const GridPose& pose, bool wall); + bool update_wall(const GridPose& pose, bool wall); /** * @brief Check whether there is a wall at the front of a given pose. diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 02ef0d2..563dd65 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -41,33 +41,39 @@ TMaze::TMaze(Config config) : start{config.start}, goal{config.go this->get_cell(position).cost = 0; } - this->compute_costmap(); + this->get_cell(this->start.position).visited = true; + this->compute_costmap(*(this->goal.begin())); } template void TMaze::update_walls(const GridPose& pose, const core::Observation& observation) { - this->update_wall(pose.turned_left(), observation.left); - this->update_wall(pose, observation.front); - this->update_wall(pose.turned_right(), observation.right); + this->get_cell(pose.position).visited = true; - this->compute_costmap(); -} + bool updated_left = this->update_wall(pose.turned_left(), observation.left); + bool updated_front = this->update_wall(pose, observation.front); + bool updated_right = this->update_wall(pose.turned_right(), observation.right); -template -GridPose TMaze::get_next_goal(const GridPoint& position, bool returning) const { - uint16_t current_cost = this->get_cell(position).cost; + if (updated_left) { + this->update_cost(pose.turned_left().front().position); + } - if (returning) { - if (this->best_route.contains(current_cost) and this->best_route.at(current_cost).position == position) { - auto current = this->best_route.find(current_cost); - return {std::prev(current)->second.position, current->second.turned_back().orientation}; - } + if (updated_front) { + this->update_cost(pose.front().position); + } - return get_next_goal(position, false); + if (updated_right) { + this->update_cost(pose.turned_right().front().position); } - GridPoint next_position = position; - Side next_side{}; + if (updated_left or updated_front or updated_right) { + this->update_cost(pose.position); + } +} + +template +GridPose TMaze::get_next_goal(const GridPoint& position) const { + uint16_t current_cost = this->get_cell(position).cost; + GridPose next_pose = {position, {}}; for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { Side side = static_cast(i); @@ -75,12 +81,12 @@ GridPose TMaze::get_next_goal(const GridPoint& position, bool ret if (not this->has_wall({position, side}) and this->get_cell(front_position).cost < current_cost) { current_cost = this->get_cell(front_position).cost; - next_position = front_position; - next_side = side; + next_pose.position = front_position; + next_pose.orientation = side; } } - return {next_position, next_side}; + return next_pose; } template @@ -94,20 +100,23 @@ TMaze::Cell& TMaze::get_cell(const GridPoint& posi } template -void TMaze::update_wall(const GridPose& pose, bool wall) { +bool TMaze::update_wall(const GridPose& pose, bool wall) { if (pose.position.x >= width or pose.position.y >= height) { - return; + return false; } + bool updated = wall and not this->get_cell(pose.position).walls[pose.orientation]; this->get_cell(pose.position).walls[pose.orientation] |= wall; GridPose front_pose = pose.front(); if (front_pose.position.x >= width or front_pose.position.y >= height) { - return; + return updated; } this->get_cell(front_pose.position).walls[pose.turned_back().orientation] |= wall; + + return updated; } template @@ -129,6 +138,18 @@ bool TMaze::finished(const GridPoint& position, bool returning) c return returning ? this->start.position == position : this->goal.contains(position); } +template +void TMaze::compute_return_costmap() { + for (auto& row : this->cells) { + for (auto& cell : row) { + cell.cost = 0x7FFF; + } + } + + this->get_cell(this->start.position).cost = 0; + this->compute_costmap(this->start.position); +} + template void TMaze::compute_best_route() { GridPose current_pose = this->start; @@ -147,15 +168,9 @@ const std::map>& TMaze::get_be } template -void TMaze::compute_costmap() { - std::array, height> visited{}; - std::array, height> origin{}; - std::queue queue; - - for (const auto& position : this->goal) { - queue.push(position); - visited.at(position.y).at(position.x) = true; - } +void TMaze::compute_costmap(const GridPoint& reference) { + std::queue queue; + queue.push(reference); while (not queue.empty()) { GridPoint current_position = queue.front(); @@ -167,22 +182,45 @@ void TMaze::compute_costmap() { Side side = static_cast(i); GridPoint front_position = current_position + side; - if (not this->has_wall({current_position, side}) and - not visited.at(front_position.y).at(front_position.x)) { - visited.at(front_position.y).at(front_position.x) = true; - origin.at(front_position.y).at(front_position.x) = side; - - this->get_cell(front_position).cost = - (current_cell.cost + (this->goal.contains(current_position) or - (side == origin.at(current_position.y).at(current_position.x)) ? - 1 : - 2)); - queue.push(front_position); + if (not this->has_wall({current_position, side})) { + uint16_t new_cost = current_cell.cost + 1 - + (current_cell.visited and not this->get_cell(front_position).visited ? 10 : 0); + + if (this->get_cell(front_position).cost > new_cost) { + this->get_cell(front_position).cost = new_cost; + queue.push(front_position); + } } } } } +template +void TMaze::update_cost(const GridPoint& reference) { + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { + Side side = static_cast(i); + GridPoint front_position = reference + side; + + if (not this->has_wall({reference, side}) and + this->get_cell(reference).cost == this->get_cell(front_position).cost + 1) { + this->compute_costmap(reference); + return; + } + } + + uint16_t old_cost = this->get_cell(reference).cost; + this->get_cell(reference).cost = 0x7FFF; + + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { + Side side = static_cast(i); + GridPoint front_position = reference + side; + + if (not this->has_wall({reference, side}) and this->get_cell(front_position).cost == old_cost + 1) { + this->update_cost(front_position); + } + } +} + template std::vector TMaze::serialize() const { std::vector buffer; From 0da45e1e2cfcfa200037b22310b3bfb1afe687ff Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 3 May 2025 22:22:53 -0300 Subject: [PATCH 355/561] =?UTF-8?q?=E2=9C=A8=20Add=20new=20return=20algori?= =?UTF-8?q?thm?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 4 ++- micras_nav/src/maze.cpp | 39 ++++++++++++++------------ 2 files changed, 24 insertions(+), 19 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index a1bf45e..ab5b55c 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -65,7 +65,7 @@ class TMaze : public core::ISerializable { * @param returning Whether the robot is returning to the start position. * @return True if the robot has finished the maze, false otherwise. */ - bool finished(const GridPoint& position, bool returning) const; + bool finished(const GridPoint& position) const; void compute_return_costmap(); @@ -164,6 +164,8 @@ class TMaze : public core::ISerializable { * @brief Current best found route to the goal. */ std::map> best_route; + + bool returning{}; }; } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 563dd65..591ead4 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -49,25 +49,25 @@ template void TMaze::update_walls(const GridPose& pose, const core::Observation& observation) { this->get_cell(pose.position).visited = true; - bool updated_left = this->update_wall(pose.turned_left(), observation.left); - bool updated_front = this->update_wall(pose, observation.front); - bool updated_right = this->update_wall(pose.turned_right(), observation.right); + this->update_wall(pose.turned_left(), observation.left); + this->update_wall(pose, observation.front); + this->update_wall(pose.turned_right(), observation.right); - if (updated_left) { - this->update_cost(pose.turned_left().front().position); - } - - if (updated_front) { - this->update_cost(pose.front().position); + for (auto& row : this->cells) { + for (auto& cell : row) { + cell.cost = 0x7FFF; + } } - if (updated_right) { - this->update_cost(pose.turned_right().front().position); + if (this->returning) { + this->get_cell(this->start.position).cost = 0; + } else { + for (const auto& position : this->goal) { + this->get_cell(position).cost = 0; + } } - if (updated_left or updated_front or updated_right) { - this->update_cost(pose.position); - } + this->compute_costmap(this->returning ? this->start.position : *(this->goal.begin())); } template @@ -134,8 +134,8 @@ core::Observation TMaze::get_observation(const GridPose& pose) co } template -bool TMaze::finished(const GridPoint& position, bool returning) const { - return returning ? this->start.position == position : this->goal.contains(position); +bool TMaze::finished(const GridPoint& position) const { + return this->returning ? this->start.position == position : this->goal.contains(position); } template @@ -146,6 +146,8 @@ void TMaze::compute_return_costmap() { } } + this->returning = true; + this->get_cell(this->start.position).cost = 0; this->compute_costmap(this->start.position); } @@ -183,8 +185,9 @@ void TMaze::compute_costmap(const GridPoint& reference) { GridPoint front_position = current_position + side; if (not this->has_wall({current_position, side})) { - uint16_t new_cost = current_cell.cost + 1 - - (current_cell.visited and not this->get_cell(front_position).visited ? 10 : 0); + uint16_t new_cost = + current_cell.cost + 1 - + (this->returning and current_cell.visited and not this->get_cell(front_position).visited ? 10 : 0); if (this->get_cell(front_position).cost > new_cost) { this->get_cell(front_position).cost = new_cost; From 4ebd47b54d80c6cb4b2eaa95e5d0234000ede341 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 4 May 2025 00:11:30 -0300 Subject: [PATCH 356/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20compute=20costmap?= =?UTF-8?q?=20calls=20on=20exploration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 591ead4..4de2137 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -42,7 +42,10 @@ TMaze::TMaze(Config config) : start{config.start}, goal{config.go } this->get_cell(this->start.position).visited = true; - this->compute_costmap(*(this->goal.begin())); + + for (const auto& position : this->goal) { + this->compute_costmap(position); + } } template @@ -61,13 +64,16 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ if (this->returning) { this->get_cell(this->start.position).cost = 0; + this->compute_costmap(this->start.position); } else { for (const auto& position : this->goal) { this->get_cell(position).cost = 0; } - } - this->compute_costmap(this->returning ? this->start.position : *(this->goal.begin())); + for (const auto& position : this->goal) { + this->compute_costmap(position); + } + } } template From 7309047b025e0efc216ace895431dbe84afab6f0 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 4 May 2025 00:30:49 -0300 Subject: [PATCH 357/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Improve=20visited?= =?UTF-8?q?=20cell=20logic?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 16 ++++++++++++--- micras_nav/src/maze.cpp | 27 +++++++++++++------------- 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index ab5b55c..d7b3e08 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -97,13 +97,23 @@ class TMaze : public core::ISerializable { void deserialize(const uint8_t* buffer, uint16_t size) override; private: + enum WallState : uint8_t { + UNKNOWN = 0, + NO_WALL = 1, + WALL = 2, + }; + /** * @brief Type to store the information of a cell in the maze. */ struct Cell { - std::array walls{}; - int16_t cost{0x7FFF}; - bool visited{}; + std::array walls{}; + int16_t cost{0x7FFF}; + + bool visited() const { + return this->walls[Side::UP] != WallState::UNKNOWN and this->walls[Side::RIGHT] != WallState::UNKNOWN and + this->walls[Side::LEFT] != WallState::UNKNOWN and this->walls[Side::DOWN] != WallState::UNKNOWN; + } }; /** diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 4de2137..7d4f770 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -14,15 +14,16 @@ namespace micras::nav { template TMaze::TMaze(Config config) : start{config.start}, goal{config.goal} { for (uint8_t row = 0; row < height; row++) { - this->cells[row][0].walls[Side::LEFT] = true; - this->cells[row][width - 1].walls[Side::RIGHT] = true; + this->cells[row][0].walls[Side::LEFT] = WallState::WALL; + this->cells[row][width - 1].walls[Side::RIGHT] = WallState::WALL; } for (uint8_t col = 0; col < width; col++) { - this->cells[0][col].walls[Side::DOWN] = true; - this->cells[height - 1][col].walls[Side::UP] = true; + this->cells[0][col].walls[Side::DOWN] = WallState::WALL; + this->cells[height - 1][col].walls[Side::UP] = WallState::WALL; } + this->update_wall(this->start, false); this->update_wall(start.turned_right(), true); // Hardcoded walls at the end region @@ -41,8 +42,6 @@ TMaze::TMaze(Config config) : start{config.start}, goal{config.go this->get_cell(position).cost = 0; } - this->get_cell(this->start.position).visited = true; - for (const auto& position : this->goal) { this->compute_costmap(position); } @@ -50,8 +49,6 @@ TMaze::TMaze(Config config) : start{config.start}, goal{config.go template void TMaze::update_walls(const GridPose& pose, const core::Observation& observation) { - this->get_cell(pose.position).visited = true; - this->update_wall(pose.turned_left(), observation.left); this->update_wall(pose, observation.front); this->update_wall(pose.turned_right(), observation.right); @@ -107,12 +104,12 @@ TMaze::Cell& TMaze::get_cell(const GridPoint& posi template bool TMaze::update_wall(const GridPose& pose, bool wall) { - if (pose.position.x >= width or pose.position.y >= height) { + if (pose.position.x >= width or pose.position.y >= height or this->has_wall(pose)) { return false; } - bool updated = wall and not this->get_cell(pose.position).walls[pose.orientation]; - this->get_cell(pose.position).walls[pose.orientation] |= wall; + bool updated = wall; + this->get_cell(pose.position).walls[pose.orientation] = wall ? WallState::WALL : WallState::NO_WALL; GridPose front_pose = pose.front(); @@ -120,14 +117,15 @@ bool TMaze::update_wall(const GridPose& pose, bool wall) { return updated; } - this->get_cell(front_pose.position).walls[pose.turned_back().orientation] |= wall; + this->get_cell(front_pose.position).walls[pose.turned_back().orientation] = + wall ? WallState::WALL : WallState::NO_WALL; return updated; } template bool TMaze::has_wall(const GridPose& pose) const { - return this->get_cell(pose.position).walls[pose.orientation]; + return this->get_cell(pose.position).walls[pose.orientation] == WallState::WALL; } template @@ -193,7 +191,8 @@ void TMaze::compute_costmap(const GridPoint& reference) { if (not this->has_wall({current_position, side})) { uint16_t new_cost = current_cell.cost + 1 - - (this->returning and current_cell.visited and not this->get_cell(front_position).visited ? 10 : 0); + (this->returning and current_cell.visited() and not this->get_cell(front_position).visited() ? 10 : + 0); if (this->get_cell(front_position).cost > new_cost) { this->get_cell(front_position).cost = new_cost; From bf8d9321c9a408bb833f419a7c3b437dfb702d4c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 4 May 2025 00:54:34 -0300 Subject: [PATCH 358/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Add=20flip=20cost?= =?UTF-8?q?=20to=20maze=20get=20next=20goal?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 2 +- micras_nav/src/maze.cpp | 21 ++++++++++++--------- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index d7b3e08..584e989 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -48,7 +48,7 @@ class TMaze : public core::ISerializable { * @param returning Whether the robot is returning to the start position. * @return The next point the robot should go to when exploring. */ - GridPose get_next_goal(const GridPoint& position) const; + GridPose get_next_goal(const GridPose& pose) const; /** * @brief Detects walls around the robot at a given grid pose. diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 7d4f770..0e5f6a3 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -74,16 +74,18 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ } template -GridPose TMaze::get_next_goal(const GridPoint& position) const { - uint16_t current_cost = this->get_cell(position).cost; - GridPose next_pose = {position, {}}; +GridPose TMaze::get_next_goal(const GridPose& pose) const { + uint16_t current_cost = 0x7FFF; + GridPose next_pose = {}; for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { Side side = static_cast(i); - GridPoint front_position = position + side; + GridPoint front_position = pose.position + side; + int16_t flip_cost = pose.turned_back().orientation == side ? 2 : 0; - if (not this->has_wall({position, side}) and this->get_cell(front_position).cost < current_cost) { - current_cost = this->get_cell(front_position).cost; + if (not this->has_wall({pose.position, side}) and + this->get_cell(front_position).cost + flip_cost < current_cost) { + current_cost = this->get_cell(front_position).cost + flip_cost; next_pose.position = front_position; next_pose.orientation = side; } @@ -163,7 +165,7 @@ void TMaze::compute_best_route() { this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start); while (not this->goal.contains(current_pose.position)) { - current_pose = this->get_next_goal(current_pose.position, false); + current_pose = this->get_next_goal(current_pose, false); this->best_route.try_emplace(this->get_cell(current_pose.position).cost, current_pose); } } @@ -191,8 +193,9 @@ void TMaze::compute_costmap(const GridPoint& reference) { if (not this->has_wall({current_position, side})) { uint16_t new_cost = current_cell.cost + 1 - - (this->returning and current_cell.visited() and not this->get_cell(front_position).visited() ? 10 : - 0); + (this->returning and current_cell.visited() and not this->get_cell(front_position).visited() ? + width + height : + 0); if (this->get_cell(front_position).cost > new_cost) { this->get_cell(front_position).cost = new_cost; From e191944b746f94c55366447382961368d6dbb0fb Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 4 May 2025 01:39:40 -0300 Subject: [PATCH 359/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20new=5Fcost=20type?= =?UTF-8?q?=20on=20compute=5Fbest=5Froute?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 0e5f6a3..232a073 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -191,7 +191,7 @@ void TMaze::compute_costmap(const GridPoint& reference) { GridPoint front_position = current_position + side; if (not this->has_wall({current_position, side})) { - uint16_t new_cost = + int16_t new_cost = current_cell.cost + 1 - (this->returning and current_cell.visited() and not this->get_cell(front_position).visited() ? width + height : From cffd70fd20dd17260ab0810df8e3aecfa8a98719 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 4 May 2025 02:11:45 -0300 Subject: [PATCH 360/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20current=5Fcost=20t?= =?UTF-8?q?ype=20on=20compute=5Fbest=5Froute?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 232a073..0f7e345 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -75,7 +75,7 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ template GridPose TMaze::get_next_goal(const GridPose& pose) const { - uint16_t current_cost = 0x7FFF; + int16_t current_cost = 0x7FFF; GridPose next_pose = {}; for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { @@ -110,7 +110,7 @@ bool TMaze::update_wall(const GridPose& pose, bool wall) { return false; } - bool updated = wall; + const bool updated = wall; this->get_cell(pose.position).walls[pose.orientation] = wall ? WallState::WALL : WallState::NO_WALL; GridPose front_pose = pose.front(); From 962cab6546be795cc1d71ad4750df205ee5fa8c9 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 4 May 2025 02:39:21 -0300 Subject: [PATCH 361/561] =?UTF-8?q?=F0=9F=9A=B8=20Make=20get=5Fnext=5Fgoal?= =?UTF-8?q?=20deterministic?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 0f7e345..81fe5f4 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -28,6 +28,11 @@ TMaze::TMaze(Config config) : start{config.start}, goal{config.go // Hardcoded walls at the end region if constexpr (width == 16 and height == 16) { + this->update_wall({{7, 7}, Side::UP}, false); + this->update_wall({{8, 7}, Side::UP}, false); + this->update_wall({{8, 7}, Side::LEFT}, false); + this->update_wall({{8, 8}, Side::LEFT}, false); + this->update_wall({{7, 7}, Side::DOWN}, true); this->update_wall({{8, 7}, Side::DOWN}, true); // this->update_wall({{8, 7}, Side::RIGHT}, true); @@ -78,10 +83,11 @@ GridPose TMaze::get_next_goal(const GridPose& pose) const { int16_t current_cost = 0x7FFF; GridPose next_pose = {}; - for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); + for (Side side : + {pose.orientation.turned_right(), pose.orientation.turned_left(), pose.orientation, + pose.orientation.turned_back()}) { GridPoint front_position = pose.position + side; - int16_t flip_cost = pose.turned_back().orientation == side ? 2 : 0; + int16_t flip_cost = pose.turned_back().orientation == side ? 1 : 0; if (not this->has_wall({pose.position, side}) and this->get_cell(front_position).cost + flip_cost < current_cost) { @@ -194,7 +200,7 @@ void TMaze::compute_costmap(const GridPoint& reference) { int16_t new_cost = current_cell.cost + 1 - (this->returning and current_cell.visited() and not this->get_cell(front_position).visited() ? - width + height : + std::lround(std::hypot(width, height)) : 0); if (this->get_cell(front_position).cost > new_cost) { From b9b2cda58ed72d8edb06f7f8e8071f691f490c6b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 4 May 2025 12:38:04 -0300 Subject: [PATCH 362/561] =?UTF-8?q?=F0=9F=A5=85=20Fix=20compilation=20erro?= =?UTF-8?q?rs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 81fe5f4..845e7e5 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -5,6 +5,7 @@ #ifndef MICRAS_NAV_MAZE_CPP #define MICRAS_NAV_MAZE_CPP +#include #include #include @@ -84,8 +85,8 @@ GridPose TMaze::get_next_goal(const GridPose& pose) const { GridPose next_pose = {}; for (Side side : - {pose.orientation.turned_right(), pose.orientation.turned_left(), pose.orientation, - pose.orientation.turned_back()}) { + {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, + pose.turned_back().orientation}) { GridPoint front_position = pose.position + side; int16_t flip_cost = pose.turned_back().orientation == side ? 1 : 0; From 191148c096d6853fa84f6af08d2ed3579bb10e41 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 4 May 2025 14:38:54 -0300 Subject: [PATCH 363/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Fix=20unexplored?= =?UTF-8?q?=20cells=20values?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 845e7e5..e202908 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -199,10 +199,9 @@ void TMaze::compute_costmap(const GridPoint& reference) { if (not this->has_wall({current_position, side})) { int16_t new_cost = - current_cell.cost + 1 - (this->returning and current_cell.visited() and not this->get_cell(front_position).visited() ? - std::lround(std::hypot(width, height)) : - 0); + -(width + height) / 2 : + current_cell.cost + 1); if (this->get_cell(front_position).cost > new_cost) { this->get_cell(front_position).cost = new_cost; From 4436f59c2ce048181a5a15d79fe05fe061cfb22f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 5 May 2025 22:00:04 -0300 Subject: [PATCH 364/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20unused=20includ?= =?UTF-8?q?es?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/src/vector.cpp | 1 - micras_nav/include/micras/nav/grid_pose.hpp | 1 - micras_nav/include/micras/nav/maze.hpp | 1 + micras_nav/include/micras/nav/odometry.hpp | 1 - micras_nav/src/maze.cpp | 1 - micras_nav/src/odometry.cpp | 2 -- micras_nav/src/speed_controller.cpp | 1 - micras_nav/src/state.cpp | 1 - micras_proxy/src/battery.cpp | 1 - micras_proxy/src/dip_switch.cpp | 1 - micras_proxy/src/imu.cpp | 1 - 11 files changed, 1 insertion(+), 11 deletions(-) diff --git a/micras_core/src/vector.cpp b/micras_core/src/vector.cpp index e11f419..e85f9ce 100644 --- a/micras_core/src/vector.cpp +++ b/micras_core/src/vector.cpp @@ -3,7 +3,6 @@ */ #include -#include #include "micras/core/vector.hpp" diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index b483609..d1c6b22 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -8,7 +8,6 @@ #include #include -#include "micras/core/types.hpp" #include "micras/core/vector.hpp" namespace micras::nav { diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 584e989..e69247f 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -11,6 +11,7 @@ #include #include "micras/core/serializable.hpp" +#include "micras/core/types.hpp" #include "micras/nav/grid_pose.hpp" namespace micras::nav { diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index e958c82..6b3352a 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -5,7 +5,6 @@ #ifndef MICRAS_NAV_ODOMETRY_HPP #define MICRAS_NAV_ODOMETRY_HPP -#include #include #include "micras/core/butterworth_filter.hpp" diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index e202908..8dc81a0 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -6,7 +6,6 @@ #define MICRAS_NAV_MAZE_CPP #include -#include #include #include "micras/nav/maze.hpp" diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index e1f7505..55f60ea 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -3,9 +3,7 @@ */ #include -#include -#include "micras/core/utils.hpp" #include "micras/nav/odometry.hpp" namespace micras::nav { diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp index fe4b31a..c27cd02 100644 --- a/micras_nav/src/speed_controller.cpp +++ b/micras_nav/src/speed_controller.cpp @@ -5,7 +5,6 @@ #include #include -#include "micras/core/utils.hpp" #include "micras/nav/speed_controller.hpp" namespace micras::nav { diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 15eba52..ad206e4 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -3,7 +3,6 @@ */ #include -#include #include "micras/nav/state.hpp" diff --git a/micras_proxy/src/battery.cpp b/micras_proxy/src/battery.cpp index 0e42075..2b10b2b 100644 --- a/micras_proxy/src/battery.cpp +++ b/micras_proxy/src/battery.cpp @@ -2,7 +2,6 @@ * @file */ -#include "micras/core/utils.hpp" #include "micras/proxy/battery.hpp" namespace micras::proxy { diff --git a/micras_proxy/src/dip_switch.cpp b/micras_proxy/src/dip_switch.cpp index 977c336..44f6f30 100644 --- a/micras_proxy/src/dip_switch.cpp +++ b/micras_proxy/src/dip_switch.cpp @@ -6,7 +6,6 @@ #define MICRAS_PROXY_DIP_SWITCH_CPP #include "micras/core/utils.hpp" -#include "micras/hal/mcu.hpp" #include "micras/proxy/dip_switch.hpp" namespace micras::proxy { diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 305336e..2161080 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -3,7 +3,6 @@ */ #include -#include #include "micras/proxy/imu.hpp" #include "micras/proxy/stopwatch.hpp" From 6c9962089f5326f073426f1fdcb8a558e6d1db9e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 6 May 2025 14:27:54 -0300 Subject: [PATCH 365/561] =?UTF-8?q?=F0=9F=9A=B8=20Move=20sysroot=20search?= =?UTF-8?q?=20from=20.clang-tidy=20to=20run=20script?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-tidy | 8 ++------ cmake/templates/run_clang_tidy.sh.in | 6 +++++- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 38e56e5..37382f4 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -4,6 +4,7 @@ Checks: bugprone-*, -bugprone-easily-swappable-parameters, -bugprone-dynamic-static-initializers, + -bugprone-bitwise-pointer-cast, clang-*, cppcoreguidelines-*, -cppcoreguidelines-avoid-magic-numbers, @@ -23,7 +24,7 @@ Checks: readability-*, -readability-duplicate-include, -readability-magic-numbers, -WarningsAsErrors: '*' + -readability-math-missing-parentheses, HeaderFileExtensions: - '' - h @@ -45,9 +46,4 @@ CheckOptions: misc-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic: true bugprone-narrowing-conversions.WarnOnIntegerToFloatingPointNarrowingConversion: false SystemHeaders: false -ExtraArgsBefore: - - '--target=arm-none-eabi' - - '--sysroot=/usr/lib/arm-none-eabi/' - - '-I/usr/lib/arm-none-eabi/include/c++/13.2.1/' - - '-I/usr/lib/arm-none-eabi/include/c++/13.2.1/arm-none-eabi/' ... diff --git a/cmake/templates/run_clang_tidy.sh.in b/cmake/templates/run_clang_tidy.sh.in index 119d374..6f64bfd 100755 --- a/cmake/templates/run_clang_tidy.sh.in +++ b/cmake/templates/run_clang_tidy.sh.in @@ -13,7 +13,11 @@ ERRORS=$( { for file in "$@"; do { - clang-tidy $FIX_FLAG --use-color --quiet -p @CMAKE_CURRENT_BINARY_DIR@ "$file" 2>&1 + clang-tidy $FIX_FLAG --use-color --quiet --warnings-as-errors="*" \ + --extra-arg-before="--sysroot=@SYSROOT@/" \ + --extra-arg-before="-I@CXX_INCLUDE_DIR@/" \ + --extra-arg-before="-I@CXX_TRIPLE_INCLUDE_DIR@/" \ + -p @CMAKE_CURRENT_BINARY_DIR@ "$file" 2>&1 } & done From 82af9d4333fc37ec9877e95bc0c470b29172da69 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 9 May 2025 03:40:52 -0300 Subject: [PATCH 366/561] =?UTF-8?q?=F0=9F=9A=A7=20Add=20new=20return=20alg?= =?UTF-8?q?orithm=20to=20maze=20using=20costmap=20layers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/costmap.hpp | 91 +++++++ micras_nav/include/micras/nav/maze.hpp | 84 +++---- micras_nav/src/costmap.cpp | 152 ++++++++++++ micras_nav/src/maze.cpp | 275 ++++++++++------------ 4 files changed, 396 insertions(+), 206 deletions(-) create mode 100644 micras_nav/include/micras/nav/costmap.hpp create mode 100644 micras_nav/src/costmap.cpp diff --git a/micras_nav/include/micras/nav/costmap.hpp b/micras_nav/include/micras/nav/costmap.hpp new file mode 100644 index 0000000..f4223d0 --- /dev/null +++ b/micras_nav/include/micras/nav/costmap.hpp @@ -0,0 +1,91 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_COSTMAP_HPP +#define MICRAS_NAV_COSTMAP_HPP + +#include +#include + +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +constexpr int16_t max_cost{0x7FFF}; + +/** + * @brief Class for storing the robot information about the maze. + * + * @tparam width The width of the maze. + * @tparam height The height of the maze. + */ +template +class Costmap { +public: + enum WallState : uint8_t { + UNKNOWN = 0, + NO_WALL = 1, + WALL = 2, + VIRTUAL = 3, + }; + + /** + * @brief Type to store the information of a cell in the maze. + */ + struct Cell { + std::array walls{WallState::UNKNOWN}; + std::array costs{max_cost}; + }; + + Costmap(); + + /** + * @brief Calculate the costmap for the flood fill algorithm. + */ + void compute(const GridPoint& reference, uint8_t layer); + + /** + * @brief Return the cell at the given position. + * + * @param position The position of the cell. + * @return The cell at the given position. + */ + const Cell& get_cell(const GridPoint& position) const; + + int16_t get_cost(const GridPoint& position, uint8_t layer) const; + + void update_cost(const GridPoint& position, uint8_t layer, int16_t cost); + + /** + * @brief Update the existence of a wall in the maze. + * + * @param pose The pose of the robot. + * @param wall Whether there is a wall at the front of a given pose. + */ + bool update_wall(const GridPose& pose, bool wall); + + /** + * @brief Check whether there is a wall at the front of a given pose. + * + * @param pose The pose to check. + * @return True if there is a wall, false otherwise. + */ + bool has_wall(const GridPose& pose) const; + + void recompute(const GridPoint& reference, uint8_t layer); + + void add_virtual_wall(const GridPose& pose); + +private: + Cell& get_cell(const GridPoint& position); + + /** + * @brief Cells matrix representing the maze. + */ + std::array, height> cells{}; +}; +} // namespace micras::nav + +#include "../src/costmap.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) + +#endif // MICRAS_NAV_COSTMAP_HPP diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index e69247f..8beea48 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -5,13 +5,13 @@ #ifndef MICRAS_NAV_MAZE_HPP #define MICRAS_NAV_MAZE_HPP -#include #include #include #include #include "micras/core/serializable.hpp" #include "micras/core/types.hpp" +#include "micras/nav/costmap.hpp" #include "micras/nav/grid_pose.hpp" namespace micras::nav { @@ -27,6 +27,7 @@ class TMaze : public core::ISerializable { struct Config { GridPose start{}; std::unordered_set goal; + uint16_t cost_margin{}; }; /** @@ -49,7 +50,7 @@ class TMaze : public core::ISerializable { * @param returning Whether the robot is returning to the start position. * @return The next point the robot should go to when exploring. */ - GridPose get_next_goal(const GridPose& pose) const; + GridPose get_next_goal(const GridPose& pose, bool returning, bool discover = true) const; /** * @brief Detects walls around the robot at a given grid pose. @@ -66,15 +67,15 @@ class TMaze : public core::ISerializable { * @param returning Whether the robot is returning to the start position. * @return True if the robot has finished the maze, false otherwise. */ - bool finished(const GridPoint& position) const; - - void compute_return_costmap(); + bool finished(const GridPoint& position, bool returning) const; /** * @brief Calculate the best route to the goal using the current costmap. */ void compute_best_route(); + void enable_discovery_costmap(); + /** * @brief Return the best route to the goal. * @@ -98,23 +99,11 @@ class TMaze : public core::ISerializable { void deserialize(const uint8_t* buffer, uint16_t size) override; private: - enum WallState : uint8_t { - UNKNOWN = 0, - NO_WALL = 1, - WALL = 2, - }; - - /** - * @brief Type to store the information of a cell in the maze. - */ - struct Cell { - std::array walls{}; - int16_t cost{0x7FFF}; - - bool visited() const { - return this->walls[Side::UP] != WallState::UNKNOWN and this->walls[Side::RIGHT] != WallState::UNKNOWN and - this->walls[Side::LEFT] != WallState::UNKNOWN and this->walls[Side::DOWN] != WallState::UNKNOWN; - } + enum Layer : uint8_t { + EXPLORE = 0, + RETURN = 1, + DISCOVER = 2, + NUM_OF_LAYERS = 3, }; /** @@ -122,61 +111,42 @@ class TMaze : public core::ISerializable { */ void compute_costmap(const GridPoint& reference); - void update_cost(const GridPoint& reference); + static bool is_dead_end(const Costmap::Cell& cell); - /** - * @brief Return the cell at the given position. - * - * @param position The position of the cell. - * @return The cell at the given position. - */ - const Cell& get_cell(const GridPoint& position) const; + static bool was_visited(const Costmap::Cell& cell); - /** - * @brief Return the cell at the given position. - * - * @param position The position of the cell. - * @return The cell at the given position. - */ - Cell& get_cell(const GridPoint& position); + static bool must_visit(const Costmap::Cell& cell, int16_t cost_threshold); - /** - * @brief Update the existence of a wall in the maze. - * - * @param pose The pose of the robot. - * @param wall Whether there is a wall at the front of a given pose. - */ - bool update_wall(const GridPose& pose, bool wall); + void update_cell(const GridPoint& position); + + Costmap costmap; /** - * @brief Check whether there is a wall at the front of a given pose. - * - * @param pose The pose to check. - * @return True if there is a wall, false otherwise. + * @brief Start pose of the robot in the maze. */ - bool has_wall(const GridPose& pose) const; + GridPose start; /** - * @brief Cells matrix representing the maze. + * @brief Goal points in the maze. */ - std::array, height> cells{}; + std::unordered_set goal; /** - * @brief Start pose of the robot in the maze. + * @brief Cost margin for the robot to explore. */ - GridPose start; + uint16_t cost_margin; /** - * @brief Goal points in the maze. + * @brief Minimum cost of path to the goal containing only visited cells. */ - std::unordered_set goal; + int16_t minimum_cost{}; + + bool discovery_enabled{false}; /** * @brief Current best found route to the goal. */ std::map> best_route; - - bool returning{}; }; } // namespace micras::nav diff --git a/micras_nav/src/costmap.cpp b/micras_nav/src/costmap.cpp new file mode 100644 index 0000000..1f7f95c --- /dev/null +++ b/micras_nav/src/costmap.cpp @@ -0,0 +1,152 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_COSTMAP_CPP +#define MICRAS_NAV_COSTMAP_CPP + +#include +#include +#include + +#include "micras/nav/costmap.hpp" + +namespace micras::nav { +template +Costmap::Costmap() { + for (uint8_t row = 0; row < height; row++) { + this->cells[row][0].walls[Side::LEFT] = WallState::WALL; + this->cells[row][width - 1].walls[Side::RIGHT] = WallState::WALL; + } + + for (uint8_t col = 0; col < width; col++) { + this->cells[0][col].walls[Side::DOWN] = WallState::WALL; + this->cells[height - 1][col].walls[Side::UP] = WallState::WALL; + } +} + +template +const Costmap::Cell& Costmap::get_cell(const GridPoint& position) const { + return this->cells.at(position.y).at(position.x); +} + +template +Costmap::Cell& Costmap::get_cell(const GridPoint& position) { + return this->cells.at(position.y).at(position.x); +} + +template +void Costmap::update_cost(const GridPoint& position, uint8_t layer, int16_t cost) { + this->get_cell(position).costs.at(layer) = cost; +} + +template +bool Costmap::update_wall(const GridPose& pose, bool wall) { + if (pose.position.x >= width or pose.position.y >= height or this->has_wall(pose)) { + return false; + } + + const bool updated = wall; + this->get_cell(pose.position).walls[pose.orientation] = wall ? WallState::WALL : WallState::NO_WALL; + + GridPose front_pose = pose.front(); + + if (front_pose.position.x >= width or front_pose.position.y >= height) { + return updated; + } + + this->get_cell(front_pose.position).walls[pose.turned_back().orientation] = + wall ? WallState::WALL : WallState::NO_WALL; + + return updated; +} + +template +void Costmap::compute(const GridPoint& reference, uint8_t layer) { + std::queue queue; + queue.push(reference); + + while (not queue.empty()) { + GridPoint current_position = queue.front(); + queue.pop(); + + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { + Side side = static_cast(i); + GridPoint front_position = current_position + side; + + if (not this->has_wall({current_position, side})) { + int16_t new_cost = this->get_cost(current_position, layer) + 1; + + if (this->get_cost(front_position, layer) > new_cost) { + this->update_cost(front_position, layer, new_cost); + queue.push(front_position); + } + } + } + } +} + +template +void Costmap::recompute(const GridPoint& reference, uint8_t layer) { + std::queue queue; + std::list compute_list; + queue.push(reference); + + while (not queue.empty()) { + GridPoint current_position = queue.front(); + queue.pop(); + bool reset_cost = true; + + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { + Side side = static_cast(i); + GridPoint front_position = current_position + side; + + if (not this->has_wall({current_position, side}) and + this->get_cost(current_position, layer) == this->get_cost(front_position, layer) + 1) { + compute_list.emplace_back(current_position); + reset_cost = false; + break; + } + } + + if (not reset_cost) { + continue; + } + + uint16_t old_cost = this->get_cost(current_position, layer); + this->update_cost(current_position, layer, max_cost); + + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { + Side side = static_cast(i); + GridPoint front_position = current_position + side; + + if (not this->has_wall({current_position, side}) and + this->get_cost(front_position, layer) == old_cost + 1) { + queue.push(front_position); + } + } + } + + for (const auto& position : compute_list) { + this->compute(position, layer); + } +} + +template +bool Costmap::has_wall(const GridPose& pose) const { + return this->get_cell(pose.position).walls[pose.orientation] == WallState::WALL; +} + +template +int16_t Costmap::get_cost(const GridPoint& position, uint8_t layer) const { + return this->get_cell(position).costs[layer]; +} + +template +void Costmap::add_virtual_wall(const GridPose& pose) { + this->get_cell(pose.position).walls[pose.orientation] = WallState::VIRTUAL; + this->get_cell(pose.position).walls[pose.turned_back().orientation] = WallState::VIRTUAL; +} +} // namespace micras::nav + +#endif // MICRAS_NAV_COSTMAP_CPP diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 8dc81a0..18cb996 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -6,173 +6,175 @@ #define MICRAS_NAV_MAZE_CPP #include -#include +#include #include "micras/nav/maze.hpp" namespace micras::nav { template -TMaze::TMaze(Config config) : start{config.start}, goal{config.goal} { - for (uint8_t row = 0; row < height; row++) { - this->cells[row][0].walls[Side::LEFT] = WallState::WALL; - this->cells[row][width - 1].walls[Side::RIGHT] = WallState::WALL; - } - - for (uint8_t col = 0; col < width; col++) { - this->cells[0][col].walls[Side::DOWN] = WallState::WALL; - this->cells[height - 1][col].walls[Side::UP] = WallState::WALL; - } - - this->update_wall(this->start, false); - this->update_wall(start.turned_right(), true); +TMaze::TMaze(Config config) : start{config.start}, goal{config.goal}, cost_margin(config.cost_margin) { + this->costmap.update_wall(this->start, false); + this->costmap.update_wall(start.turned_right(), true); // Hardcoded walls at the end region if constexpr (width == 16 and height == 16) { - this->update_wall({{7, 7}, Side::UP}, false); - this->update_wall({{8, 7}, Side::UP}, false); - this->update_wall({{8, 7}, Side::LEFT}, false); - this->update_wall({{8, 8}, Side::LEFT}, false); + this->costmap.update_wall({{7, 7}, Side::UP}, false); + this->costmap.update_wall({{8, 7}, Side::UP}, false); + this->costmap.update_wall({{8, 7}, Side::LEFT}, false); + this->costmap.update_wall({{8, 8}, Side::LEFT}, false); - this->update_wall({{7, 7}, Side::DOWN}, true); - this->update_wall({{8, 7}, Side::DOWN}, true); + // this->update_wall({{7, 7}, Side::DOWN}, true); + // this->update_wall({{8, 7}, Side::DOWN}, true); // this->update_wall({{8, 7}, Side::RIGHT}, true); - this->update_wall({{8, 8}, Side::RIGHT}, true); - this->update_wall({{8, 8}, Side::UP}, true); - this->update_wall({{7, 8}, Side::UP}, true); - this->update_wall({{7, 8}, Side::LEFT}, true); - this->update_wall({{7, 7}, Side::LEFT}, true); + // this->update_wall({{8, 8}, Side::RIGHT}, true); + // this->update_wall({{8, 8}, Side::UP}, true); + // this->update_wall({{7, 8}, Side::UP}, true); + // this->update_wall({{7, 8}, Side::LEFT}, true); + // this->update_wall({{7, 7}, Side::LEFT}, true); } for (const auto& position : this->goal) { - this->get_cell(position).cost = 0; + this->costmap.update_cost(position, Layer::EXPLORE, 0); } for (const auto& position : this->goal) { - this->compute_costmap(position); + this->costmap.compute(position, Layer::EXPLORE); } + + this->costmap.update_cost(this->start.position, Layer::RETURN, 0); + this->costmap.compute(this->start.position, Layer::RETURN); } template void TMaze::update_walls(const GridPose& pose, const core::Observation& observation) { - this->update_wall(pose.turned_left(), observation.left); - this->update_wall(pose, observation.front); - this->update_wall(pose.turned_right(), observation.right); + const bool new_left_wall = this->costmap.update_wall(pose.turned_left(), observation.left); + const bool new_front_wall = this->costmap.update_wall(pose, observation.front); + const bool new_right_wall = this->costmap.update_wall(pose.turned_right(), observation.right); - for (auto& row : this->cells) { - for (auto& cell : row) { - cell.cost = 0x7FFF; - } + if (new_left_wall) { + this->update_cell(pose.turned_left().front().position); } - if (this->returning) { - this->get_cell(this->start.position).cost = 0; - this->compute_costmap(this->start.position); - } else { - for (const auto& position : this->goal) { - this->get_cell(position).cost = 0; - } + if (new_front_wall) { + this->update_cell(pose.front().position); + } - for (const auto& position : this->goal) { - this->compute_costmap(position); - } + if (new_right_wall) { + this->update_cell(pose.turned_right().front().position); + } + + if (new_left_wall or new_front_wall or new_right_wall) { + this->update_cell(pose.position); } } template -GridPose TMaze::get_next_goal(const GridPose& pose) const { - int16_t current_cost = 0x7FFF; - GridPose next_pose = {}; +void TMaze::update_cell(const GridPoint& position) { + this->costmap.recompute(position, Layer::EXPLORE); + this->costmap.recompute(position, Layer::RETURN); - for (Side side : - {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, - pose.turned_back().orientation}) { - GridPoint front_position = pose.position + side; - int16_t flip_cost = pose.turned_back().orientation == side ? 1 : 0; + GridPoint current_position = position; - if (not this->has_wall({pose.position, side}) and - this->get_cell(front_position).cost + flip_cost < current_cost) { - current_cost = this->get_cell(front_position).cost + flip_cost; - next_pose.position = front_position; - next_pose.orientation = side; + while (this->is_dead_end(std::as_const(this->costmap).get_cell(current_position))) { + for (Side side : {Side::UP, Side::DOWN, Side::LEFT, Side::RIGHT}) { + if (this->costmap.has_wall({current_position, side})) { + continue; + } + + this->costmap.add_virtual_wall({current_position, side}); + this->costmap.update_cost(current_position, Layer::DISCOVER, max_cost); + current_position = current_position + side; + break; } } - return next_pose; -} + if (not this->discovery_enabled) { + return; + } -template -const TMaze::Cell& TMaze::get_cell(const GridPoint& position) const { - return this->cells.at(position.y).at(position.x); -} + if (not this->must_visit( + std::as_const(this->costmap).get_cell(current_position), this->minimum_cost + this->cost_margin + )) { + this->costmap.update_cost(current_position, Layer::DISCOVER, max_cost); + } -template -TMaze::Cell& TMaze::get_cell(const GridPoint& position) { - return this->cells.at(position.y).at(position.x); + this->costmap.recompute(current_position, Layer::DISCOVER); } template -bool TMaze::update_wall(const GridPose& pose, bool wall) { - if (pose.position.x >= width or pose.position.y >= height or this->has_wall(pose)) { - return false; - } - - const bool updated = wall; - this->get_cell(pose.position).walls[pose.orientation] = wall ? WallState::WALL : WallState::NO_WALL; +GridPose TMaze::get_next_goal(const GridPose& pose, bool returning, bool discover) const { + int16_t current_cost = max_cost; + GridPose next_pose = {}; + const uint8_t discovery_factor = this->discovery_enabled ? 2 : 0; - GridPose front_pose = pose.front(); - - if (front_pose.position.x >= width or front_pose.position.y >= height) { - return updated; + for (Side side : + {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, + pose.turned_back().orientation}) { + GridPoint front_position = pose.position + side; + const int16_t flip_cost = pose.turned_back().orientation == side ? 1 : 0; + const int16_t front_cost = + (returning ? this->costmap.get_cost(front_position, Layer::RETURN) + + discovery_factor * this->costmap.get_cost(front_position, Layer::DISCOVER) : + this->costmap.get_cost(front_position, Layer::EXPLORE)) + + flip_cost; + + const bool can_go = discover ? not this->costmap.has_wall({pose.position, side}) : + was_visited(this->costmap.get_cell(front_position)); + + if (can_go and front_cost < current_cost) { + current_cost = front_cost; + next_pose.position = front_position; + next_pose.orientation = side; + } } - this->get_cell(front_pose.position).walls[pose.turned_back().orientation] = - wall ? WallState::WALL : WallState::NO_WALL; - - return updated; -} - -template -bool TMaze::has_wall(const GridPose& pose) const { - return this->get_cell(pose.position).walls[pose.orientation] == WallState::WALL; + return next_pose; } template core::Observation TMaze::get_observation(const GridPose& pose) const { return { - .left = this->has_wall(pose.turned_left()), - .front = this->has_wall(pose), - .right = this->has_wall(pose.turned_right()) + .left = this->costmap.has_wall(pose.turned_left()), + .front = this->costmap.has_wall(pose), + .right = this->costmap.has_wall(pose.turned_right()) }; } template -bool TMaze::finished(const GridPoint& position) const { - return this->returning ? this->start.position == position : this->goal.contains(position); +bool TMaze::finished(const GridPoint& position, bool returning) const { + return returning ? this->start.position == position : this->goal.contains(position); } template -void TMaze::compute_return_costmap() { - for (auto& row : this->cells) { - for (auto& cell : row) { - cell.cost = 0x7FFF; +void TMaze::enable_discovery_costmap() { + this->compute_best_route(); + this->minimum_cost = this->best_route.size(); + + for (uint8_t y = 0; y < height; y++) { + for (uint8_t x = 0; x < width; x++) { + GridPoint position{x, y}; + + if (this->must_visit( + std::as_const(this->costmap).get_cell(position), this->minimum_cost + this->cost_margin + )) { + this->costmap.update_cost(position, Layer::DISCOVER, 0); + this->costmap.compute(position, Layer::DISCOVER); + } } } - this->returning = true; - - this->get_cell(this->start.position).cost = 0; - this->compute_costmap(this->start.position); + this->discovery_enabled = true; } template void TMaze::compute_best_route() { GridPose current_pose = this->start; this->best_route.clear(); - this->best_route.try_emplace(this->get_cell(this->start.position).cost, this->start); + this->best_route.try_emplace(this->costmap.get_cost(this->start.position, Layer::EXPLORE), this->start); while (not this->goal.contains(current_pose.position)) { - current_pose = this->get_next_goal(current_pose, false); - this->best_route.try_emplace(this->get_cell(current_pose.position).cost, current_pose); + current_pose = this->get_next_goal(current_pose, false, false); + this->best_route.try_emplace(this->costmap.get_cost(current_pose.position, Layer::EXPLORE), current_pose); } } @@ -182,59 +184,34 @@ const std::map>& TMaze::get_be } template -void TMaze::compute_costmap(const GridPoint& reference) { - std::queue queue; - queue.push(reference); - - while (not queue.empty()) { - GridPoint current_position = queue.front(); - queue.pop(); - - const Cell& current_cell = this->get_cell(current_position); - - for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); - GridPoint front_position = current_position + side; - - if (not this->has_wall({current_position, side})) { - int16_t new_cost = - (this->returning and current_cell.visited() and not this->get_cell(front_position).visited() ? - -(width + height) / 2 : - current_cell.cost + 1); - - if (this->get_cell(front_position).cost > new_cost) { - this->get_cell(front_position).cost = new_cost; - queue.push(front_position); - } - } - } - } -} +bool TMaze::is_dead_end(const Costmap::Cell& cell) { + uint8_t wall_count = 0; -template -void TMaze::update_cost(const GridPoint& reference) { - for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); - GridPoint front_position = reference + side; - - if (not this->has_wall({reference, side}) and - this->get_cell(reference).cost == this->get_cell(front_position).cost + 1) { - this->compute_costmap(reference); - return; + for (const auto& wall : cell.walls) { + if (wall == Costmap::WallState::WALL or + wall == Costmap::WallState::VIRTUAL) { + wall_count++; } } - uint16_t old_cost = this->get_cell(reference).cost; - this->get_cell(reference).cost = 0x7FFF; + return (wall_count == 3); +} - for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); - GridPoint front_position = reference + side; +template +bool TMaze::was_visited(const Costmap::Cell& cell) { + return not( + cell.walls[Side::UP] == Costmap::WallState::UNKNOWN or + cell.walls[Side::DOWN] == Costmap::WallState::UNKNOWN or + cell.walls[Side::LEFT] == Costmap::WallState::UNKNOWN or + cell.walls[Side::RIGHT] == Costmap::WallState::UNKNOWN + ); +} - if (not this->has_wall({reference, side}) and this->get_cell(front_position).cost == old_cost + 1) { - this->update_cost(front_position); - } - } +template +bool TMaze::must_visit( + const Costmap::Cell& cell, int16_t cost_threshold +) { + return not was_visited(cell) and (cell.costs[Layer::EXPLORE] + cell.costs[Layer::RETURN] <= cost_threshold); } template From c898ee0f186524afb0464083d68009544813582e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 9 May 2025 20:53:34 -0300 Subject: [PATCH 367/561] =?UTF-8?q?=F0=9F=93=9D=20Add=20costmap=20and=20ma?= =?UTF-8?q?ze=20documentation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/mcu.hpp | 2 - micras_nav/include/micras/nav/costmap.hpp | 61 +++++-- micras_nav/include/micras/nav/maze.hpp | 66 +++++--- micras_nav/src/costmap.cpp | 87 +++++----- micras_nav/src/maze.cpp | 194 +++++++++++----------- 5 files changed, 245 insertions(+), 165 deletions(-) diff --git a/micras_hal/include/micras/hal/mcu.hpp b/micras_hal/include/micras/hal/mcu.hpp index 2ad6c59..9414fad 100644 --- a/micras_hal/include/micras/hal/mcu.hpp +++ b/micras_hal/include/micras/hal/mcu.hpp @@ -5,8 +5,6 @@ #ifndef MICRAS_HAL_MCU_HPP #define MICRAS_HAL_MCU_HPP -#include - namespace micras::hal { /** * @brief Microcontroller unit class. diff --git a/micras_nav/include/micras/nav/costmap.hpp b/micras_nav/include/micras/nav/costmap.hpp index f4223d0..127aee0 100644 --- a/micras_nav/include/micras/nav/costmap.hpp +++ b/micras_nav/include/micras/nav/costmap.hpp @@ -14,14 +14,18 @@ namespace micras::nav { constexpr int16_t max_cost{0x7FFF}; /** - * @brief Class for storing the robot information about the maze. + * @brief Class for managing a layered costmap. * * @tparam width The width of the maze. * @tparam height The height of the maze. + * @tparam layers The number of layers in the costmap. */ template class Costmap { public: + /** + * @brief Type to store the state of a wall. + */ enum WallState : uint8_t { UNKNOWN = 0, NO_WALL = 1, @@ -37,13 +41,27 @@ class Costmap { std::array costs{max_cost}; }; + /** + * @brief Construct a new Costmap object. + */ Costmap(); /** - * @brief Calculate the costmap for the flood fill algorithm. + * @brief Compute the costmap of a given layer from a reference point using the flood fill algorithm. + * + * @param reference The reference point to start the flood fill algorithm. + * @param layer The layer to compute the costmap for. */ void compute(const GridPoint& reference, uint8_t layer); + /** + * @brief Reset the costs and recompute the new values locally for a given layer. + * + * @param reference The reference point to start resetting the stored cost. + * @param layer The layer to recompute. + */ + void recompute(const GridPoint& reference, uint8_t layer); + /** * @brief Return the cell at the given position. * @@ -52,17 +70,23 @@ class Costmap { */ const Cell& get_cell(const GridPoint& position) const; + /** + * @brief Get the cost of a cell at a given position and layer. + * + * @param position The position of the cell. + * @param layer The layer to get the cost for. + * @return The cost of the cell at the given position and layer. + */ int16_t get_cost(const GridPoint& position, uint8_t layer) const; - void update_cost(const GridPoint& position, uint8_t layer, int16_t cost); - /** - * @brief Update the existence of a wall in the maze. + * @brief Update the cost of a cell at a given position and layer. * - * @param pose The pose of the robot. - * @param wall Whether there is a wall at the front of a given pose. + * @param position The position of the cell. + * @param layer The layer to update the cost for. + * @param cost The new cost to set. */ - bool update_wall(const GridPose& pose, bool wall); + void update_cost(const GridPoint& position, uint8_t layer, int16_t cost); /** * @brief Check whether there is a wall at the front of a given pose. @@ -72,12 +96,29 @@ class Costmap { */ bool has_wall(const GridPose& pose) const; - void recompute(const GridPoint& reference, uint8_t layer); + /** + * @brief Update the existence of a wall in the maze. + * + * @param pose The pose of the robot. + * @param wall Whether there is a wall at the front of a given pose. + */ + bool update_wall(const GridPose& pose, bool wall); + /** + * @brief Add a virtual wall to the costmap at a given position and side. + * + * @param pose The pose to add the virtual wall at. + */ void add_virtual_wall(const GridPose& pose); private: - Cell& get_cell(const GridPoint& position); + /** + * @brief Get the cell at the given position. + * + * @param position The position of the cell. + * @return The cell at the given position. + */ + Cell& cell_on_position(const GridPoint& position); /** * @brief Cells matrix representing the maze. diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 8beea48..8c7679f 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -44,21 +44,14 @@ class TMaze : public core::ISerializable { void update_walls(const GridPose& pose, const core::Observation& observation); /** - * @brief Return the next point the robot should go to when exploring. + * @brief Return the next point the robot should go based on the costmap. * * @param position The current position of the robot. * @param returning Whether the robot is returning to the start position. - * @return The next point the robot should go to when exploring. + * @param discover Whether the robot should discover new cells. + * @return The next point the robot should go. */ - GridPose get_next_goal(const GridPose& pose, bool returning, bool discover = true) const; - - /** - * @brief Detects walls around the robot at a given grid pose. - * - * @param pose The robotโ€™s current grid pose. - * @return The walls that are present around the robot. - */ - core::Observation get_observation(const GridPose& pose) const; + GridPose get_next_goal(const GridPose& pose, bool returning, bool discover = true); /** * @brief Check whether the robot has finished the maze. @@ -74,8 +67,6 @@ class TMaze : public core::ISerializable { */ void compute_best_route(); - void enable_discovery_costmap(); - /** * @brief Return the best route to the goal. * @@ -99,26 +90,58 @@ class TMaze : public core::ISerializable { void deserialize(const uint8_t* buffer, uint16_t size) override; private: + /** + * @brief The layers of the costmap. + */ enum Layer : uint8_t { EXPLORE = 0, RETURN = 1, - DISCOVER = 2, NUM_OF_LAYERS = 3, }; /** - * @brief Calculate the costmap for the flood fill algorithm. + * @brief Update the cell costs at the given position. + * + * @param position The position of the cell. */ - void compute_costmap(const GridPoint& reference); + void update_cell(const GridPoint& position); + /** + * @brief Get the next goal for the robot to discover more cells. + * + * @param pose The current pose of the robot. + * @return The next discovery goal for the robot. + */ + GridPose get_next_discovery_goal(const GridPose& pose) const; + + /** + * @brief Check if the cell is a dead end. + * + * @param cell The cell to check. + * @return True if the cell is a dead end, false otherwise. + */ static bool is_dead_end(const Costmap::Cell& cell); + /** + * @brief Check if the cell was visited. + * + * @param cell The cell to check. + * @return True if the cell was visited, false otherwise. + */ static bool was_visited(const Costmap::Cell& cell); + /** + * @brief Check if the cell must be visited. + * + * @param cell The cell to check. + * @param cost_threshold The cost threshold for the cell. + * @return True if the cell must be visited, false otherwise. + */ static bool must_visit(const Costmap::Cell& cell, int16_t cost_threshold); - void update_cell(const GridPoint& position); - + /** + * @brief Layered costmap for the maze. + */ Costmap costmap; /** @@ -132,7 +155,7 @@ class TMaze : public core::ISerializable { std::unordered_set goal; /** - * @brief Cost margin for the robot to explore. + * @brief Cost margin above the minimum cost that the robot should explore. */ uint16_t cost_margin; @@ -141,7 +164,10 @@ class TMaze : public core::ISerializable { */ int16_t minimum_cost{}; - bool discovery_enabled{false}; + /** + * @brief Flag indicating whether the robot has finished discovering the cells of the maze. + */ + bool finished_discovery{false}; /** * @brief Current best found route to the goal. diff --git a/micras_nav/src/costmap.cpp b/micras_nav/src/costmap.cpp index 1f7f95c..ce32f06 100644 --- a/micras_nav/src/costmap.cpp +++ b/micras_nav/src/costmap.cpp @@ -25,42 +25,6 @@ Costmap::Costmap() { } } -template -const Costmap::Cell& Costmap::get_cell(const GridPoint& position) const { - return this->cells.at(position.y).at(position.x); -} - -template -Costmap::Cell& Costmap::get_cell(const GridPoint& position) { - return this->cells.at(position.y).at(position.x); -} - -template -void Costmap::update_cost(const GridPoint& position, uint8_t layer, int16_t cost) { - this->get_cell(position).costs.at(layer) = cost; -} - -template -bool Costmap::update_wall(const GridPose& pose, bool wall) { - if (pose.position.x >= width or pose.position.y >= height or this->has_wall(pose)) { - return false; - } - - const bool updated = wall; - this->get_cell(pose.position).walls[pose.orientation] = wall ? WallState::WALL : WallState::NO_WALL; - - GridPose front_pose = pose.front(); - - if (front_pose.position.x >= width or front_pose.position.y >= height) { - return updated; - } - - this->get_cell(front_pose.position).walls[pose.turned_back().orientation] = - wall ? WallState::WALL : WallState::NO_WALL; - - return updated; -} - template void Costmap::compute(const GridPoint& reference, uint8_t layer) { std::queue queue; @@ -133,8 +97,8 @@ void Costmap::recompute(const GridPoint& reference, uint8 } template -bool Costmap::has_wall(const GridPose& pose) const { - return this->get_cell(pose.position).walls[pose.orientation] == WallState::WALL; +const Costmap::Cell& Costmap::get_cell(const GridPoint& position) const { + return this->cells.at(position.y).at(position.x); } template @@ -142,10 +106,53 @@ int16_t Costmap::get_cost(const GridPoint& position, uint return this->get_cell(position).costs[layer]; } +template +void Costmap::update_cost(const GridPoint& position, uint8_t layer, int16_t cost) { + this->cell_on_position(position).costs.at(layer) = cost; +} + +template +bool Costmap::has_wall(const GridPose& pose) const { + return this->get_cell(pose.position).walls[pose.orientation] == WallState::WALL; +} + +template +bool Costmap::update_wall(const GridPose& pose, bool wall) { + if (this->has_wall(pose)) { + return false; + } + + const bool updated = wall; + this->cell_on_position(pose.position).walls[pose.orientation] = wall ? WallState::WALL : WallState::NO_WALL; + + GridPose front_pose = pose.front(); + + if (front_pose.position.x >= width or front_pose.position.y >= height) { + return updated; + } + + this->cell_on_position(front_pose.position).walls[pose.turned_back().orientation] = + wall ? WallState::WALL : WallState::NO_WALL; + + return updated; +} + template void Costmap::add_virtual_wall(const GridPose& pose) { - this->get_cell(pose.position).walls[pose.orientation] = WallState::VIRTUAL; - this->get_cell(pose.position).walls[pose.turned_back().orientation] = WallState::VIRTUAL; + this->cell_on_position(pose.position).walls[pose.orientation] = WallState::VIRTUAL; + + GridPose front_pose = pose.front(); + + if (front_pose.position.x >= width or front_pose.position.y >= height) { + return; + } + + this->cell_on_position(front_pose.position).walls[pose.turned_back().orientation] = WallState::VIRTUAL; +} + +template +Costmap::Cell& Costmap::cell_on_position(const GridPoint& position) { + return this->cells.at(position.y).at(position.x); } } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 18cb996..5c5e37b 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -8,6 +8,7 @@ #include #include +#include "micras/nav/grid_pose.hpp" #include "micras/nav/maze.hpp" namespace micras::nav { @@ -69,43 +70,19 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ } template -void TMaze::update_cell(const GridPoint& position) { - this->costmap.recompute(position, Layer::EXPLORE); - this->costmap.recompute(position, Layer::RETURN); - - GridPoint current_position = position; - - while (this->is_dead_end(std::as_const(this->costmap).get_cell(current_position))) { - for (Side side : {Side::UP, Side::DOWN, Side::LEFT, Side::RIGHT}) { - if (this->costmap.has_wall({current_position, side})) { - continue; - } +GridPose TMaze::get_next_goal(const GridPose& pose, bool returning, bool discover) { + if (returning or this->finished_discovery) { + const auto& next_goal = this->get_next_discovery_goal(pose); - this->costmap.add_virtual_wall({current_position, side}); - this->costmap.update_cost(current_position, Layer::DISCOVER, max_cost); - current_position = current_position + side; - break; + if (next_goal != this->start) { + return next_goal; } - } - if (not this->discovery_enabled) { - return; + this->finished_discovery = true; } - if (not this->must_visit( - std::as_const(this->costmap).get_cell(current_position), this->minimum_cost + this->cost_margin - )) { - this->costmap.update_cost(current_position, Layer::DISCOVER, max_cost); - } - - this->costmap.recompute(current_position, Layer::DISCOVER); -} - -template -GridPose TMaze::get_next_goal(const GridPose& pose, bool returning, bool discover) const { - int16_t current_cost = max_cost; - GridPose next_pose = {}; - const uint8_t discovery_factor = this->discovery_enabled ? 2 : 0; + int16_t current_cost = max_cost; + GridPose next_pose = {}; for (Side side : {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, @@ -113,13 +90,10 @@ GridPose TMaze::get_next_goal(const GridPose& pose, bool returnin GridPoint front_position = pose.position + side; const int16_t flip_cost = pose.turned_back().orientation == side ? 1 : 0; const int16_t front_cost = - (returning ? this->costmap.get_cost(front_position, Layer::RETURN) + - discovery_factor * this->costmap.get_cost(front_position, Layer::DISCOVER) : - this->costmap.get_cost(front_position, Layer::EXPLORE)) + - flip_cost; + this->costmap.get_cost(front_position, returning ? Layer::RETURN : Layer::EXPLORE) + flip_cost; const bool can_go = discover ? not this->costmap.has_wall({pose.position, side}) : - was_visited(this->costmap.get_cell(front_position)); + was_visited(std::as_const(this->costmap).get_cell(front_position)); if (can_go and front_cost < current_cost) { current_cost = front_cost; @@ -131,41 +105,11 @@ GridPose TMaze::get_next_goal(const GridPose& pose, bool returnin return next_pose; } -template -core::Observation TMaze::get_observation(const GridPose& pose) const { - return { - .left = this->costmap.has_wall(pose.turned_left()), - .front = this->costmap.has_wall(pose), - .right = this->costmap.has_wall(pose.turned_right()) - }; -} - template bool TMaze::finished(const GridPoint& position, bool returning) const { return returning ? this->start.position == position : this->goal.contains(position); } -template -void TMaze::enable_discovery_costmap() { - this->compute_best_route(); - this->minimum_cost = this->best_route.size(); - - for (uint8_t y = 0; y < height; y++) { - for (uint8_t x = 0; x < width; x++) { - GridPoint position{x, y}; - - if (this->must_visit( - std::as_const(this->costmap).get_cell(position), this->minimum_cost + this->cost_margin - )) { - this->costmap.update_cost(position, Layer::DISCOVER, 0); - this->costmap.compute(position, Layer::DISCOVER); - } - } - } - - this->discovery_enabled = true; -} - template void TMaze::compute_best_route() { GridPose current_pose = this->start; @@ -176,6 +120,8 @@ void TMaze::compute_best_route() { current_pose = this->get_next_goal(current_pose, false, false); this->best_route.try_emplace(this->costmap.get_cost(current_pose.position, Layer::EXPLORE), current_pose); } + + this->minimum_cost = this->best_route.size(); } template @@ -183,6 +129,94 @@ const std::map>& TMaze::get_be return this->best_route; } +template +std::vector TMaze::serialize() const { + std::vector buffer; + buffer.reserve(3 * this->best_route.size()); + + for (const auto& [cost, grid_pose] : this->best_route) { + buffer.emplace_back(grid_pose.position.x); + buffer.emplace_back(grid_pose.position.y); + buffer.emplace_back(grid_pose.orientation); + } + + return buffer; +} + +template +void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { + this->best_route.clear(); + + for (uint32_t i = 0; i < size; i += 3) { + this->best_route.try_emplace( + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) + (size - i) / 3 - 1, GridPose{{buffer[i], buffer[i + 1]}, static_cast(buffer[i + 2])} + ); + } +} + +template +void TMaze::update_cell(const GridPoint& position) { + if (not this->goal.contains(position)) { + this->costmap.recompute(position, Layer::EXPLORE); + } + + if (position != this->start.position) { + this->costmap.recompute(position, Layer::RETURN); + } + + GridPoint dead_end_position = position; + + while (this->is_dead_end(std::as_const(this->costmap).get_cell(dead_end_position))) { + for (Side side : {Side::UP, Side::DOWN, Side::LEFT, Side::RIGHT}) { + if (this->costmap.has_wall({dead_end_position, side})) { + continue; + } + + this->costmap.add_virtual_wall({dead_end_position, side}); + dead_end_position = dead_end_position + side; + break; + } + } +} + +template +GridPose TMaze::get_next_discovery_goal(const GridPose& pose) const { + std::queue queue; + std::array, height> checked{}; + GridPose next_goal = this->start; + + for (Side side : + {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, + pose.turned_back().orientation}) { + if (not this->costmap.has_wall({pose.position, side})) { + queue.emplace(pose.position + side, side); + } + } + + while (not queue.empty()) { + GridPose current_pose = queue.front(); + queue.pop(); + checked.at(current_pose.position.y).at(current_pose.position.x) = true; + + if (this->must_visit(this->costmap.get_cell(current_pose.position), this->minimum_cost + this->cost_margin)) { + next_goal = {pose.position + current_pose.orientation, current_pose.orientation}; + break; + } + + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { + Side side = static_cast(i); + + if (not this->costmap.has_wall({current_pose.position, side}) and + not checked.at(current_pose.position.y).at(current_pose.position.x)) { + queue.emplace(current_pose.position + side, current_pose.orientation); + } + } + } + + return next_goal; +} + template bool TMaze::is_dead_end(const Costmap::Cell& cell) { uint8_t wall_count = 0; @@ -213,32 +247,6 @@ bool TMaze::must_visit( ) { return not was_visited(cell) and (cell.costs[Layer::EXPLORE] + cell.costs[Layer::RETURN] <= cost_threshold); } - -template -std::vector TMaze::serialize() const { - std::vector buffer; - buffer.reserve(3 * this->best_route.size()); - - for (const auto& [cost, grid_pose] : this->best_route) { - buffer.emplace_back(grid_pose.position.x); - buffer.emplace_back(grid_pose.position.y); - buffer.emplace_back(grid_pose.orientation); - } - - return buffer; -} - -template -void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { - this->best_route.clear(); - - for (uint32_t i = 0; i < size; i += 3) { - this->best_route.try_emplace( - // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) - (size - i) / 3 - 1, GridPose{{buffer[i], buffer[i + 1]}, static_cast(buffer[i + 2])} - ); - } -} } // namespace micras::nav #endif // MICRAS_NAV_MAZE_CPP From f5124aea30a2c351476c66ce92cb4cfc812701ef Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 9 May 2025 21:14:42 -0300 Subject: [PATCH 368/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20costmap=20bugs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/costmap.hpp | 5 +++-- micras_nav/src/costmap.cpp | 5 +++-- micras_nav/src/maze.cpp | 23 ++++++++++++----------- 3 files changed, 18 insertions(+), 15 deletions(-) diff --git a/micras_nav/include/micras/nav/costmap.hpp b/micras_nav/include/micras/nav/costmap.hpp index 127aee0..9d52d71 100644 --- a/micras_nav/include/micras/nav/costmap.hpp +++ b/micras_nav/include/micras/nav/costmap.hpp @@ -91,10 +91,11 @@ class Costmap { /** * @brief Check whether there is a wall at the front of a given pose. * - * @param pose The pose to check. + * @param pose The pose to check.\ + * @param consider_virtual Whether to consider virtual walls. * @return True if there is a wall, false otherwise. */ - bool has_wall(const GridPose& pose) const; + bool has_wall(const GridPose& pose, bool consider_virtual = false) const; /** * @brief Update the existence of a wall in the maze. diff --git a/micras_nav/src/costmap.cpp b/micras_nav/src/costmap.cpp index ce32f06..90bdfca 100644 --- a/micras_nav/src/costmap.cpp +++ b/micras_nav/src/costmap.cpp @@ -112,8 +112,9 @@ void Costmap::update_cost(const GridPoint& position, uint } template -bool Costmap::has_wall(const GridPose& pose) const { - return this->get_cell(pose.position).walls[pose.orientation] == WallState::WALL; +bool Costmap::has_wall(const GridPose& pose, bool consider_virtual) const { + return this->get_cell(pose.position).walls[pose.orientation] == WallState::WALL or + (consider_virtual and this->get_cell(pose.position).walls[pose.orientation] == WallState::VIRTUAL); } template diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 5c5e37b..a0fb970 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -93,7 +93,7 @@ GridPose TMaze::get_next_goal(const GridPose& pose, bool returnin this->costmap.get_cost(front_position, returning ? Layer::RETURN : Layer::EXPLORE) + flip_cost; const bool can_go = discover ? not this->costmap.has_wall({pose.position, side}) : - was_visited(std::as_const(this->costmap).get_cell(front_position)); + was_visited(this->costmap.get_cell(front_position)); if (can_go and front_cost < current_cost) { current_cost = front_cost; @@ -167,15 +167,13 @@ void TMaze::update_cell(const GridPoint& position) { GridPoint dead_end_position = position; - while (this->is_dead_end(std::as_const(this->costmap).get_cell(dead_end_position))) { + while (this->is_dead_end(this->costmap.get_cell(dead_end_position))) { for (Side side : {Side::UP, Side::DOWN, Side::LEFT, Side::RIGHT}) { - if (this->costmap.has_wall({dead_end_position, side})) { - continue; + if (not this->costmap.has_wall({dead_end_position, side}, true)) { + this->costmap.add_virtual_wall({dead_end_position, side}); + dead_end_position = dead_end_position + side; + break; } - - this->costmap.add_virtual_wall({dead_end_position, side}); - dead_end_position = dead_end_position + side; - break; } } } @@ -186,6 +184,8 @@ GridPose TMaze::get_next_discovery_goal(const GridPose& pose) con std::array, height> checked{}; GridPose next_goal = this->start; + checked.at(pose.position.y).at(pose.position.x) = true; + for (Side side : {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, pose.turned_back().orientation}) { @@ -205,11 +205,12 @@ GridPose TMaze::get_next_discovery_goal(const GridPose& pose) con } for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); + Side side = static_cast(i); + const GridPoint front_position = current_pose.position + side; if (not this->costmap.has_wall({current_pose.position, side}) and - not checked.at(current_pose.position.y).at(current_pose.position.x)) { - queue.emplace(current_pose.position + side, current_pose.orientation); + not checked.at(front_position.y).at(front_position.x)) { + queue.emplace(front_position, current_pose.orientation); } } } From e4737875b3c4ed4bcbfad6696818193d79729083 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 10 May 2025 03:51:10 -0300 Subject: [PATCH 369/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Improve=20recomput?= =?UTF-8?q?e=20function=20on=20costmap?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/costmap.cpp | 22 ++++++---------------- micras_nav/src/maze.cpp | 20 ++++++++++++-------- 2 files changed, 18 insertions(+), 24 deletions(-) diff --git a/micras_nav/src/costmap.cpp b/micras_nav/src/costmap.cpp index 90bdfca..2f72c5f 100644 --- a/micras_nav/src/costmap.cpp +++ b/micras_nav/src/costmap.cpp @@ -6,7 +6,6 @@ #define MICRAS_NAV_COSTMAP_CPP #include -#include #include #include "micras/nav/costmap.hpp" @@ -53,32 +52,27 @@ void Costmap::compute(const GridPoint& reference, uint8_t template void Costmap::recompute(const GridPoint& reference, uint8_t layer) { std::queue queue; - std::list compute_list; queue.push(reference); while (not queue.empty()) { GridPoint current_position = queue.front(); queue.pop(); - bool reset_cost = true; + int16_t lowest_cost = max_cost - 1; for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); - GridPoint front_position = current_position + side; + Side side = static_cast(i); - if (not this->has_wall({current_position, side}) and - this->get_cost(current_position, layer) == this->get_cost(front_position, layer) + 1) { - compute_list.emplace_back(current_position); - reset_cost = false; - break; + if (not this->has_wall({current_position, side})) { + lowest_cost = std::min(lowest_cost, this->get_cost(current_position + side, layer)); } } - if (not reset_cost) { + if (this->get_cost(current_position, layer) == lowest_cost + 1) { continue; } uint16_t old_cost = this->get_cost(current_position, layer); - this->update_cost(current_position, layer, max_cost); + this->update_cost(current_position, layer, lowest_cost + 1); for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { Side side = static_cast(i); @@ -90,10 +84,6 @@ void Costmap::recompute(const GridPoint& reference, uint8 } } } - - for (const auto& position : compute_list) { - this->compute(position, layer); - } } template diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index a0fb970..b8085ae 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -6,7 +6,6 @@ #define MICRAS_NAV_MAZE_CPP #include -#include #include "micras/nav/grid_pose.hpp" #include "micras/nav/maze.hpp" @@ -87,15 +86,16 @@ GridPose TMaze::get_next_goal(const GridPose& pose, bool returnin for (Side side : {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, pose.turned_back().orientation}) { + if (this->costmap.has_wall({pose.position, side})) { + continue; + } + GridPoint front_position = pose.position + side; const int16_t flip_cost = pose.turned_back().orientation == side ? 1 : 0; const int16_t front_cost = this->costmap.get_cost(front_position, returning ? Layer::RETURN : Layer::EXPLORE) + flip_cost; - const bool can_go = discover ? not this->costmap.has_wall({pose.position, side}) : - was_visited(this->costmap.get_cell(front_position)); - - if (can_go and front_cost < current_cost) { + if ((discover or was_visited(this->costmap.get_cell(front_position))) and front_cost < current_cost) { current_cost = front_cost; next_pose.position = front_position; next_pose.orientation = side; @@ -205,11 +205,15 @@ GridPose TMaze::get_next_discovery_goal(const GridPose& pose) con } for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); + Side side = static_cast(i); + + if (this->costmap.has_wall({current_pose.position, side})) { + continue; + } + const GridPoint front_position = current_pose.position + side; - if (not this->costmap.has_wall({current_pose.position, side}) and - not checked.at(front_position.y).at(front_position.x)) { + if (not checked.at(front_position.y).at(front_position.x)) { queue.emplace(front_position, current_pose.orientation); } } From bb605808f88b67aef31fe766a42b096613cd9975 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 10 May 2025 04:18:03 -0300 Subject: [PATCH 370/561] =?UTF-8?q?=F0=9F=90=9B=20Make=20get=5Fbest=5Frout?= =?UTF-8?q?e=20use=20bfs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/costmap.hpp | 6 ++++-- micras_nav/include/micras/nav/maze.hpp | 8 ++++---- micras_nav/src/costmap.cpp | 2 +- micras_nav/src/maze.cpp | 17 ++++++++++------- 4 files changed, 19 insertions(+), 14 deletions(-) diff --git a/micras_nav/include/micras/nav/costmap.hpp b/micras_nav/include/micras/nav/costmap.hpp index 9d52d71..6e20b3d 100644 --- a/micras_nav/include/micras/nav/costmap.hpp +++ b/micras_nav/include/micras/nav/costmap.hpp @@ -11,7 +11,7 @@ #include "micras/nav/grid_pose.hpp" namespace micras::nav { -constexpr int16_t max_cost{0x7FFF}; +constexpr int16_t max_cost{0x1FFF}; /** * @brief Class for managing a layered costmap. @@ -37,8 +37,10 @@ class Costmap { * @brief Type to store the information of a cell in the maze. */ struct Cell { + Cell() { costs.fill(max_cost); } + std::array walls{WallState::UNKNOWN}; - std::array costs{max_cost}; + std::array costs; }; /** diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 8c7679f..f7b9d31 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -48,10 +48,9 @@ class TMaze : public core::ISerializable { * * @param position The current position of the robot. * @param returning Whether the robot is returning to the start position. - * @param discover Whether the robot should discover new cells. * @return The next point the robot should go. */ - GridPose get_next_goal(const GridPose& pose, bool returning, bool discover = true); + GridPose get_next_goal(const GridPose& pose, bool returning); /** * @brief Check whether the robot has finished the maze. @@ -107,12 +106,13 @@ class TMaze : public core::ISerializable { void update_cell(const GridPoint& position); /** - * @brief Get the next goal for the robot to discover more cells. + * @brief Get the next goal for the robot using a BFS algorithm. * * @param pose The current pose of the robot. + * @param discover Whether the robot is discovering new cells. * @return The next discovery goal for the robot. */ - GridPose get_next_discovery_goal(const GridPose& pose) const; + GridPose get_next_bfs_goal(const GridPose& pose, bool discover) const; /** * @brief Check if the cell is a dead end. diff --git a/micras_nav/src/costmap.cpp b/micras_nav/src/costmap.cpp index 2f72c5f..8a51711 100644 --- a/micras_nav/src/costmap.cpp +++ b/micras_nav/src/costmap.cpp @@ -57,7 +57,7 @@ void Costmap::recompute(const GridPoint& reference, uint8 while (not queue.empty()) { GridPoint current_position = queue.front(); queue.pop(); - int16_t lowest_cost = max_cost - 1; + int16_t lowest_cost = max_cost; for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { Side side = static_cast(i); diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index b8085ae..b63794f 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -69,9 +69,9 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ } template -GridPose TMaze::get_next_goal(const GridPose& pose, bool returning, bool discover) { +GridPose TMaze::get_next_goal(const GridPose& pose, bool returning) { if (returning or this->finished_discovery) { - const auto& next_goal = this->get_next_discovery_goal(pose); + const auto& next_goal = this->get_next_bfs_goal(pose, true); if (next_goal != this->start) { return next_goal; @@ -95,7 +95,7 @@ GridPose TMaze::get_next_goal(const GridPose& pose, bool returnin const int16_t front_cost = this->costmap.get_cost(front_position, returning ? Layer::RETURN : Layer::EXPLORE) + flip_cost; - if ((discover or was_visited(this->costmap.get_cell(front_position))) and front_cost < current_cost) { + if (front_cost < current_cost) { current_cost = front_cost; next_pose.position = front_position; next_pose.orientation = side; @@ -117,7 +117,7 @@ void TMaze::compute_best_route() { this->best_route.try_emplace(this->costmap.get_cost(this->start.position, Layer::EXPLORE), this->start); while (not this->goal.contains(current_pose.position)) { - current_pose = this->get_next_goal(current_pose, false, false); + current_pose = this->get_next_bfs_goal(current_pose, false); this->best_route.try_emplace(this->costmap.get_cost(current_pose.position, Layer::EXPLORE), current_pose); } @@ -179,7 +179,7 @@ void TMaze::update_cell(const GridPoint& position) { } template -GridPose TMaze::get_next_discovery_goal(const GridPose& pose) const { +GridPose TMaze::get_next_bfs_goal(const GridPose& pose, bool discover) const { std::queue queue; std::array, height> checked{}; GridPose next_goal = this->start; @@ -199,7 +199,9 @@ GridPose TMaze::get_next_discovery_goal(const GridPose& pose) con queue.pop(); checked.at(current_pose.position.y).at(current_pose.position.x) = true; - if (this->must_visit(this->costmap.get_cell(current_pose.position), this->minimum_cost + this->cost_margin)) { + if ((discover and + this->must_visit(this->costmap.get_cell(current_pose.position), this->minimum_cost + this->cost_margin)) or + (not discover and this->goal.contains(current_pose.position))) { next_goal = {pose.position + current_pose.orientation, current_pose.orientation}; break; } @@ -213,7 +215,8 @@ GridPose TMaze::get_next_discovery_goal(const GridPose& pose) con const GridPoint front_position = current_pose.position + side; - if (not checked.at(front_position.y).at(front_position.x)) { + if ((discover or this->was_visited(this->costmap.get_cell(front_position))) and + not checked.at(front_position.y).at(front_position.x)) { queue.emplace(front_position, current_pose.orientation); } } From 5a7cc2bdf88fec3a54e89f076067ccdeb3841666 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 10 May 2025 04:33:10 -0300 Subject: [PATCH 371/561] =?UTF-8?q?=F0=9F=90=9B=20Change=20best=5Froute=20?= =?UTF-8?q?from=20map=20to=20list?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/action_queuer.hpp | 4 ++-- micras_nav/include/micras/nav/maze.hpp | 6 +++--- micras_nav/src/action_queuer.cpp | 4 ++-- micras_nav/src/maze.cpp | 14 +++++++------- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 925e383..5119f03 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -5,7 +5,7 @@ #ifndef MICRAS_NAV_ACTION_QUEUER_HPP #define MICRAS_NAV_ACTION_QUEUER_HPP -#include +#include #include #include @@ -82,7 +82,7 @@ class ActionQueuer { /** * @brief Fill the action queue with a sequence of actions to the end. */ - void recompute(const std::map>& best_route); + void recompute(const std::list& best_route); private: /** diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index f7b9d31..f57995a 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -6,7 +6,7 @@ #define MICRAS_NAV_MAZE_HPP #include -#include +#include #include #include "micras/core/serializable.hpp" @@ -71,7 +71,7 @@ class TMaze : public core::ISerializable { * * @return The best route to the goal. */ - const std::map>& get_best_route() const; + const std::list& get_best_route() const; /** * @brief Serialize the best route to the goal. @@ -172,7 +172,7 @@ class TMaze : public core::ISerializable { /** * @brief Current best found route to the goal. */ - std::map> best_route; + std::list best_route; }; } // namespace micras::nav diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 0aeff27..20768eb 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -76,7 +76,7 @@ bool ActionQueuer::empty() const { return this->action_queue.empty(); } -void ActionQueuer::recompute(const std::map>& best_route) { +void ActionQueuer::recompute(const std::list& best_route) { this->action_queue = {}; this->action_queue.emplace(start); @@ -85,7 +85,7 @@ void ActionQueuer::recompute(const std::map>& } for (auto it = std::next(best_route.begin()); std::next(it) != best_route.end(); it++) { - this->push(it->second, std::next(it)->second.position); + this->push(*it, std::next(it)->position); } } } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index b63794f..76f6713 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -70,7 +70,7 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ template GridPose TMaze::get_next_goal(const GridPose& pose, bool returning) { - if (returning or this->finished_discovery) { + if (returning and not this->finished_discovery) { const auto& next_goal = this->get_next_bfs_goal(pose, true); if (next_goal != this->start) { @@ -114,18 +114,18 @@ template void TMaze::compute_best_route() { GridPose current_pose = this->start; this->best_route.clear(); - this->best_route.try_emplace(this->costmap.get_cost(this->start.position, Layer::EXPLORE), this->start); + this->best_route.emplace_back(this->start); while (not this->goal.contains(current_pose.position)) { current_pose = this->get_next_bfs_goal(current_pose, false); - this->best_route.try_emplace(this->costmap.get_cost(current_pose.position, Layer::EXPLORE), current_pose); + this->best_route.emplace_back(current_pose); } this->minimum_cost = this->best_route.size(); } template -const std::map>& TMaze::get_best_route() const { +const std::list& TMaze::get_best_route() const { return this->best_route; } @@ -134,7 +134,7 @@ std::vector TMaze::serialize() const { std::vector buffer; buffer.reserve(3 * this->best_route.size()); - for (const auto& [cost, grid_pose] : this->best_route) { + for (const auto& grid_pose : this->best_route) { buffer.emplace_back(grid_pose.position.x); buffer.emplace_back(grid_pose.position.y); buffer.emplace_back(grid_pose.orientation); @@ -148,9 +148,9 @@ void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { this->best_route.clear(); for (uint32_t i = 0; i < size; i += 3) { - this->best_route.try_emplace( + this->best_route.emplace_back( // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) - (size - i) / 3 - 1, GridPose{{buffer[i], buffer[i + 1]}, static_cast(buffer[i + 2])} + GridPoint{buffer[i], buffer[i + 1]}, static_cast(buffer[i + 2]) ); } } From c48de289d4609227d80c4abf4850437457660e42 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 10 May 2025 13:14:31 -0300 Subject: [PATCH 372/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20virtual=20walls=20?= =?UTF-8?q?being=20overwritten?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/costmap.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/src/costmap.cpp b/micras_nav/src/costmap.cpp index 8a51711..bff20b8 100644 --- a/micras_nav/src/costmap.cpp +++ b/micras_nav/src/costmap.cpp @@ -109,7 +109,7 @@ bool Costmap::has_wall(const GridPose& pose, bool conside template bool Costmap::update_wall(const GridPose& pose, bool wall) { - if (this->has_wall(pose)) { + if (this->has_wall(pose, true)) { return false; } From 227ae6ca424689762f25f1b26dc278e105a2a16e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 10 May 2025 13:30:27 -0300 Subject: [PATCH 373/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20number=20of=20laye?= =?UTF-8?q?rs=20in=20costmap?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index f57995a..2f666a4 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -95,7 +95,7 @@ class TMaze : public core::ISerializable { enum Layer : uint8_t { EXPLORE = 0, RETURN = 1, - NUM_OF_LAYERS = 3, + NUM_OF_LAYERS = 2, }; /** From 4177c417a94122379ea4a735ede4291d949679cc Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 10 May 2025 13:39:52 -0300 Subject: [PATCH 374/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Recompute=20best?= =?UTF-8?q?=20route=20at=20each=20new=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 4 ++-- micras_nav/src/maze.cpp | 5 ++++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 2f666a4..e28fc78 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -27,7 +27,7 @@ class TMaze : public core::ISerializable { struct Config { GridPose start{}; std::unordered_set goal; - uint16_t cost_margin{}; + float cost_margin{}; }; /** @@ -157,7 +157,7 @@ class TMaze : public core::ISerializable { /** * @brief Cost margin above the minimum cost that the robot should explore. */ - uint16_t cost_margin; + float cost_margin; /** * @brief Minimum cost of path to the goal containing only visited cells. diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 76f6713..a88068b 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -71,6 +71,7 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ template GridPose TMaze::get_next_goal(const GridPose& pose, bool returning) { if (returning and not this->finished_discovery) { + this->compute_best_route(); const auto& next_goal = this->get_next_bfs_goal(pose, true); if (next_goal != this->start) { @@ -200,7 +201,9 @@ GridPose TMaze::get_next_bfs_goal(const GridPose& pose, bool disc checked.at(current_pose.position.y).at(current_pose.position.x) = true; if ((discover and - this->must_visit(this->costmap.get_cell(current_pose.position), this->minimum_cost + this->cost_margin)) or + this->must_visit( + this->costmap.get_cell(current_pose.position), std::round(this->minimum_cost * this->cost_margin) + )) or (not discover and this->goal.contains(current_pose.position))) { next_goal = {pose.position + current_pose.orientation, current_pose.orientation}; break; From e42be992abb5ef5b5cda77ebf645781befe8aa9a Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sat, 13 Dec 2025 19:13:39 -0300 Subject: [PATCH 375/561] =?UTF-8?q?=F0=9F=90=9B=20Set=20BOARD=5FVERSION=20?= =?UTF-8?q?to=20"v1"=20in=20CMakeLists.txt=20and=20adjust=20ADC=20calibrat?= =?UTF-8?q?ion=20for=20STM32H7=20in=20adc=5Fdma.cpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/adc_dma.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 2525c34..0879aba 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -9,7 +9,12 @@ namespace micras::hal { AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, handle{config.handle} { config.init_function(); + +#if defined(STM32H7) + HAL_ADCEx_Calibration_Start(this->handle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED); +#elif defined(STM32G4) HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); +#endif } void AdcDma::start_dma(std::span buffer) { From 19f13d978804a10292823abef580c19ec7b7775f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 13 Dec 2025 19:25:33 -0300 Subject: [PATCH 376/561] =?UTF-8?q?=F0=9F=94=A7Update=20ioc=20file?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/libs/lsm6dsv-pid | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_proxy/libs/lsm6dsv-pid b/micras_proxy/libs/lsm6dsv-pid index 7b98628..160252b 160000 --- a/micras_proxy/libs/lsm6dsv-pid +++ b/micras_proxy/libs/lsm6dsv-pid @@ -1 +1 @@ -Subproject commit 7b98628713090244cf14f54d9059938cb00342fd +Subproject commit 160252bebe966f584fd0e7b2c1083f619816ab0a From 603bf67d7b8037b81a95173c0ddb599fa2d31f8b Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sat, 13 Dec 2025 20:31:33 -0300 Subject: [PATCH 377/561] =?UTF-8?q?=F0=9F=9A=A7=20comment=20flash=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/flash.hpp | 3 +- micras_hal/stm32/src/flash.cpp | 46 ++++++++++++------------- 2 files changed, 25 insertions(+), 24 deletions(-) diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp index 2a4e96c..8511719 100644 --- a/micras_hal/include/micras/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -83,7 +83,8 @@ class Flash { /** * @brief Number of double words per page. */ - static constexpr uint32_t double_words_per_page{FLASH_PAGE_SIZE / 8}; + // static constexpr uint32_t double_words_per_page{FLASH_PAGE_SIZE / 8}; + static constexpr uint32_t double_words_per_page{8}; }; } // namespace micras::hal diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index d53d901..e913d42 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -32,22 +32,22 @@ void Flash::read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { HAL_FLASH_Unlock(); - const uint32_t end = base_address - 8 * address; - address = end - 8 * (size - 1); + // const uint32_t end = base_address - 8 * address; + // address = end - 8 * (size - 1); - while (address <= end) { - if (size >= double_words_per_row) { - HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, std::bit_cast(data)); - address += bytes_per_row; - data += double_words_per_row; - size -= double_words_per_row; - } else { - HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, address, *data); - address += 8; - data++; - size--; - } - } + // while (address <= end) { + // if (size >= double_words_per_row) { + // HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, std::bit_cast(data)); + // address += bytes_per_row; + // data += double_words_per_row; + // size -= double_words_per_row; + // } else { + // HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, address, *data); + // address += 8; + // data++; + // size--; + // } + // } HAL_FLASH_Lock(); } @@ -58,17 +58,17 @@ void Flash::write(uint16_t page, uint16_t page_address, const uint64_t data[], u } void Flash::erase_pages(uint16_t page, uint16_t number_of_pages) { - FLASH_EraseInitTypeDef erase_struct = { - .TypeErase = FLASH_TYPEERASE_PAGES, - .Banks = FLASH_BANK_2, - .Page = FLASH_PAGE_NB - page - number_of_pages, - .NbPages = number_of_pages, - }; + // FLASH_EraseInitTypeDef erase_struct = { + // .TypeErase = FLASH_TYPEERASE_PAGES, + // .Banks = FLASH_BANK_2, + // .Page = FLASH_PAGE_NB - page - number_of_pages, + // .NbPages = number_of_pages, + // }; - uint32_t page_error{}; + // uint32_t page_error{}; HAL_FLASH_Unlock(); - HAL_FLASHEx_Erase(&erase_struct, &page_error); + // HAL_FLASHEx_Erase(&erase_struct, &page_error); HAL_FLASH_Lock(); } } // namespace micras::hal From 0cbbb78e10cc372510eece63a72c4aa4c921a8ff Mon Sep 17 00:00:00 2001 From: PedroDeSanti Date: Sat, 13 Dec 2025 20:32:30 -0300 Subject: [PATCH 378/561] =?UTF-8?q?=F0=9F=9A=A7=20comment=20fan=20directio?= =?UTF-8?q?n=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/include/micras/proxy/fan.hpp | 6 +++--- micras_proxy/src/fan.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index 01dccc8..c452468 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -21,8 +21,8 @@ class Fan { * @brief Configuration struct for the fan. */ struct Config { - hal::Pwm::Config pwm; - hal::Gpio::Config direction_gpio; + hal::Pwm::Config pwm; + // hal::Gpio::Config direction_gpio; hal::Gpio::Config enable_gpio; float max_acceleration; }; @@ -85,7 +85,7 @@ class Fan { /** * @brief GPIO object for controlling the fan rotation direction. */ - hal::Gpio direction_gpio; + // hal::Gpio direction_gpio; /** * @brief GPIO handle for the fan enable pin. diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp index 1dfe504..87cc932 100644 --- a/micras_proxy/src/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -8,7 +8,7 @@ namespace micras::proxy { Fan::Fan(const Config& config) : pwm{config.pwm}, - direction_gpio{config.direction_gpio}, + // direction_gpio{config.direction_gpio}, enable_gpio{config.enable_gpio}, max_acceleration{config.max_acceleration} { this->stop(); @@ -53,6 +53,6 @@ void Fan::stop() { } void Fan::set_direction(RotationDirection direction) { - this->direction_gpio.write(static_cast(direction)); + // this->direction_gpio.write(static_cast(direction)); } } // namespace micras::proxy From 0f1d73366e9ffa5ada89b0f89550d5cbcfc24233 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 13 Dec 2025 22:29:22 -0300 Subject: [PATCH 379/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20flash=20memory=20h?= =?UTF-8?q?al?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/flash.hpp | 18 +++----- micras_hal/stm32/src/flash.cpp | 55 +++++++++++-------------- 2 files changed, 31 insertions(+), 42 deletions(-) diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp index 8511719..03acecb 100644 --- a/micras_hal/include/micras/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -28,7 +28,7 @@ class Flash { * @param data Pointer to store the data read. * @param size Size in double words of the data to read. */ - static void read(uint32_t address, uint64_t data[], uint32_t size = 1); + static void read(uint32_t address, uint32_t data[], uint32_t size = 1); /** * @brief Read data from flash memory at an address relative to a page. @@ -38,7 +38,7 @@ class Flash { * @param data Pointer to store the data read. * @param size Size in double words of the data to read. */ - static void read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); + static void read(uint16_t page, uint16_t page_address, uint32_t data[], uint32_t size = 1); /** * @brief Write data to flash memory at an absolute address. @@ -47,7 +47,7 @@ class Flash { * @param data Pointer to the data to write. * @param size Size in double words of the data to write. */ - static void write(uint32_t address, const uint64_t data[], uint32_t size = 1); + static void write(uint32_t address, const uint32_t data[], uint32_t size = 1); /** * @brief Write data to flash memory at an address relative to a page. @@ -57,7 +57,7 @@ class Flash { * @param data Pointer to the data to write. * @param size Size in double words of the data to write. */ - static void write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size = 1); + static void write(uint16_t page, uint16_t page_address, const uint32_t data[], uint32_t size = 1); // NOLINTEND(*-avoid-c-arrays) @@ -73,18 +73,12 @@ class Flash { /** * @brief Number of double words per row. */ - static constexpr uint32_t double_words_per_row{32}; - - /** - * @brief Number of bytes per row. - */ - static constexpr uint32_t bytes_per_row{8 * double_words_per_row}; + static constexpr uint32_t bytes_per_flashword{4 * FLASH_NB_32BITWORD_IN_FLASHWORD}; /** * @brief Number of double words per page. */ - // static constexpr uint32_t double_words_per_page{FLASH_PAGE_SIZE / 8}; - static constexpr uint32_t double_words_per_page{8}; + static constexpr uint32_t flash_words_per_sector{FLASH_SECTOR_SIZE / bytes_per_flashword}; }; } // namespace micras::hal diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index e913d42..60a194a 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -3,6 +3,7 @@ */ #include +#include #include "micras/hal/flash.hpp" @@ -15,60 +16,54 @@ namespace micras::hal { static const uint32_t base_address = FLASH_BASE + FLASH_SIZE - 8; // NOLINTNEXTLINE(*-avoid-c-arrays) -void Flash::read(uint32_t address, uint64_t data[], uint32_t size) { +void Flash::read(uint32_t address, uint32_t data[], uint32_t size) { const uint32_t end = base_address - 8 * address; for (address = end - 8 * (size - 1); address <= end; address += 8, data++) { - (*data) = *(std::bit_cast(address)); + (*data) = *(std::bit_cast(address)); } } // NOLINTNEXTLINE(*-avoid-c-arrays) -void Flash::read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size) { - read(page * double_words_per_page + page_address, data, size); +void Flash::read(uint16_t page, uint16_t page_address, uint32_t data[], uint32_t size) { + read(page * flash_words_per_sector + page_address, data, size); } // NOLINTNEXTLINE(*-avoid-c-arrays) -void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { +void Flash::write(uint32_t address, const uint32_t data[], uint32_t size) { HAL_FLASH_Unlock(); - // const uint32_t end = base_address - 8 * address; - // address = end - 8 * (size - 1); + const uint32_t end = base_address - 8 * address; + address = end - 8 * (size - 1 + (bytes_per_flashword - size % bytes_per_flashword)); - // while (address <= end) { - // if (size >= double_words_per_row) { - // HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, std::bit_cast(data)); - // address += bytes_per_row; - // data += double_words_per_row; - // size -= double_words_per_row; - // } else { - // HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, address, *data); - // address += 8; - // data++; - // size--; - // } - // } + while (size > 0) { + HAL_FLASH_Program(FLASH_TYPEPROGRAM_FLASHWORD, address, std::bit_cast(data)); + size -= bytes_per_flashword; + address += bytes_per_flashword; + data += bytes_per_flashword; + } HAL_FLASH_Lock(); } // NOLINTNEXTLINE(*-avoid-c-arrays) -void Flash::write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size) { - write(page * double_words_per_page + page_address, data, size); +void Flash::write(uint16_t page, uint16_t page_address, const uint32_t data[], uint32_t size) { + write(page * flash_words_per_sector + page_address, data, size); } void Flash::erase_pages(uint16_t page, uint16_t number_of_pages) { - // FLASH_EraseInitTypeDef erase_struct = { - // .TypeErase = FLASH_TYPEERASE_PAGES, - // .Banks = FLASH_BANK_2, - // .Page = FLASH_PAGE_NB - page - number_of_pages, - // .NbPages = number_of_pages, - // }; + FLASH_EraseInitTypeDef erase_struct = { + .TypeErase = FLASH_TYPEERASE_SECTORS, + .Banks = FLASH_BANK_1, + .Sector = FLASH_SECTOR_TOTAL - page - number_of_pages, + .NbSectors = number_of_pages, + .VoltageRange = FLASH_VOLTAGE_RANGE_4 + }; - // uint32_t page_error{}; + uint32_t sector_error{}; HAL_FLASH_Unlock(); - // HAL_FLASHEx_Erase(&erase_struct, &page_error); + HAL_FLASHEx_Erase(&erase_struct, §or_error); HAL_FLASH_Lock(); } } // namespace micras::hal From fc4a6fe05c4f13ccfc679f55dba5387419d20d82 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 15 Sep 2026 22:16:51 -0300 Subject: [PATCH 380/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20flash=20hal=20impo?= =?UTF-8?q?rt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/flash.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index 60a194a..3340f05 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -3,7 +3,7 @@ */ #include -#include +#include #include "micras/hal/flash.hpp" From fc29bb4bc354f3690c99b6bdd88246851b162e03 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 15 Sep 2026 22:17:37 -0300 Subject: [PATCH 381/561] =?UTF-8?q?=E2=AC=86=EF=B8=8F=20Update=20imu=20lib?= =?UTF-8?q?rary=20and=20implementation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/libs/lsm6dsv-pid | 2 +- micras_proxy/src/imu.cpp | 12 +++--------- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/micras_proxy/libs/lsm6dsv-pid b/micras_proxy/libs/lsm6dsv-pid index 160252b..ba840e9 160000 --- a/micras_proxy/libs/lsm6dsv-pid +++ b/micras_proxy/libs/lsm6dsv-pid @@ -1 +1 @@ -Subproject commit 160252bebe966f584fd0e7b2c1083f619816ab0a +Subproject commit ba840e93355bbce604ceb2ee8d3eee8105588dc0 diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 2161080..13b2171 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -17,22 +17,16 @@ Imu::Imu(const Config& config) : xl_factor{mg_to_mps2 * (0.061F * (1 << static_cast(config.accelerometer_scale)))} { this->dev_ctx.read_reg = platform_read; this->dev_ctx.write_reg = platform_write; + this->dev_ctx.mdelay = proxy::Stopwatch::sleep_ms; this->dev_ctx.handle = &this->spi; - proxy::Stopwatch::sleep_ms(100); + proxy::Stopwatch::sleep_ms(10); if (not this->check_whoami()) { return; } - lsm6dsv_reset_set(&(this->dev_ctx), LSM6DSV_RESTORE_CTRL_REGS); - - lsm6dsv_reset_t rst{}; - - do { - lsm6dsv_reset_get(&(this->dev_ctx), &rst); - } while (rst != LSM6DSV_READY); - + lsm6dsv_sw_por(&(this->dev_ctx)); lsm6dsv_block_data_update_set(&(this->dev_ctx), PROPERTY_ENABLE); lsm6dsv_gy_data_rate_set(&(this->dev_ctx), config.gyroscope_data_rate); From 9e4a6a76dbdf43a9c50e7aa39d3548f6306aae86 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 15 Sep 2026 22:51:20 -0300 Subject: [PATCH 382/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20storage=20proxy=20?= =?UTF-8?q?for=20flash=20changes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/src/storage.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 2072609..1f6a5b0 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -10,7 +10,7 @@ namespace micras::proxy { Storage::Storage(const Config& config) : start_page{config.start_page}, number_of_pages{config.number_of_pages} { uint64_t header{}; - hal::Flash::read(this->start_page, 0, &header); + hal::Flash::read(this->start_page, 0, std::bit_cast(&header), 2); if (header >> 48 != start_symbol) { return; @@ -20,8 +20,8 @@ Storage::Storage(const Config& config) : start_page{config.start_page}, number_o const uint16_t num_primitives = header >> 16; const uint16_t num_serializables = header; - this->buffer.resize(8L * total_size); - hal::Flash::read(this->start_page, 1, std::bit_cast(this->buffer.data()), total_size); + this->buffer.resize(4L * total_size); + hal::Flash::read(this->start_page, 1, std::bit_cast(this->buffer.data()), total_size); this->primitives = deserialize_var_map(this->buffer, num_primitives); this->serializables = deserialize_var_map(this->buffer, num_serializables); @@ -81,8 +81,8 @@ void Storage::save() { auto serialized_primitives = serialize_var_map(this->primitives); this->buffer.insert(this->buffer.begin(), serialized_primitives.begin(), serialized_primitives.end()); - this->buffer.insert(this->buffer.end(), (8 - (this->buffer.size() % 8)) % 8, 0); - const uint16_t total_size = this->buffer.size() / 8; + this->buffer.insert(this->buffer.end(), (4 - (this->buffer.size() % 4)) % 4, 0); + const uint16_t total_size = this->buffer.size() / 4; this->buffer.emplace_back(this->serializables.size()); this->buffer.emplace_back(this->serializables.size() >> 8); @@ -94,7 +94,7 @@ void Storage::save() { this->buffer.emplace_back(start_symbol); this->buffer.emplace_back(start_symbol >> 8); - hal::Flash::write(this->start_page, 0, std::bit_cast(buffer.data()), buffer.size() / 8); + hal::Flash::write(this->start_page, 0, std::bit_cast(buffer.data()), this->buffer.size() / 4); } template From 1ee36f1e3e4ef213a4b85a7ccda9201570dd580d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 16 Sep 2026 02:29:35 -0300 Subject: [PATCH 383/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20CMake=20after=20cu?= =?UTF-8?q?be=206.14?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/CMakeLists.txt | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt index 88587af..c2da5ae 100644 --- a/micras_hal/CMakeLists.txt +++ b/micras_hal/CMakeLists.txt @@ -1,15 +1,11 @@ project(micras_hal) get_target_property(CUBE_INCLUDE_DIRECTORIES stm32cubemx INTERFACE_INCLUDE_DIRECTORIES) -get_target_property(CUBE_SOURCES stm32cubemx INTERFACE_SOURCES) get_target_property(CUBE_COMPILE_DEFINITIONS stm32cubemx INTERFACE_COMPILE_DEFINITIONS) # Remove warnings from Cube sources -set_source_files_properties( - ${CUBE_SOURCES} - PROPERTIES - COMPILE_FLAGS "-w" -) +target_compile_options(cube_app PRIVATE -w) +target_compile_options(STM32_Drivers PRIVATE -w) file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") @@ -22,7 +18,8 @@ target_include_directories(${PROJECT_NAME} PUBLIC ) target_link_libraries(${PROJECT_NAME} PRIVATE - stm32cubemx + cube_app + STM32_Drivers ) target_link_libraries(${PROJECT_NAME} PUBLIC From dd369a6e02606f487f376df08f038f2dbb8935d5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 16 Sep 2026 02:32:36 -0300 Subject: [PATCH 384/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20flash?= =?UTF-8?q?=20and=20storage?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/flash.hpp | 158 ++++++++++++++---- micras_hal/stm32/src/flash.cpp | 129 ++++++++++---- micras_proxy/include/micras/proxy/storage.hpp | 48 ++++-- micras_proxy/src/storage.cpp | 147 ++++++++++++---- 4 files changed, 374 insertions(+), 108 deletions(-) diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp index 03acecb..fcdfa46 100644 --- a/micras_hal/include/micras/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -5,80 +5,166 @@ #ifndef MICRAS_HAL_FLASH_HPP #define MICRAS_HAL_FLASH_HPP +#include #include #include +#include namespace micras::hal { +/** + * @brief Class to handle the smallest unit of data that can be programmed into the flash memory. + * + * @note The buffer of the caller is used directly whenever it already covers a whole flash word, so only a + * trailing incomplete flash word is copied to the internal buffer and padded. + */ +class FlashWord { +public: +/** + * @brief Number of 32 bit words inside a flash word. + */ +#if defined(FLASH_NB_32BITWORD_IN_FLASHWORD) + static constexpr uint32_t words{FLASH_NB_32BITWORD_IN_FLASHWORD}; +#else + static constexpr uint32_t words{sizeof(uint64_t) / sizeof(uint32_t)}; +#endif + + /** + * @brief Number of bytes inside a flash word. + */ + static constexpr uint32_t size{words * sizeof(uint32_t)}; + + /** + * @brief Value of every byte of an erased flash memory. + */ + static constexpr uint8_t erased_value{0xFF}; + + /** + * @brief Construct a new Flash Word object from the beginning of a buffer. + * + * @param data Buffer to take the data from, padded with the erased value if smaller than a flash word. + */ + explicit FlashWord(std::span data); + + /** + * @brief Get the data ready to be programmed into the flash memory. + * + * @return Pointer to size bytes of data aligned to 32 bits. + */ + const uint32_t* data() const; + + /** + * @brief Check if the data had to be copied to the internal buffer. + * + * @return True if the data was padded or realigned, false otherwise. + */ + bool is_padded() const; + +private: + /** + * @brief Buffer used only when the data needs to be padded or realigned. + */ + std::array buffer{}; + + /** + * @brief Data to be programmed, pointing either to the buffer of the caller or to the internal one. + */ + const uint32_t* source{}; +}; + /** * @brief Class to handle flash memory on STM32 microcontrollers. + * + * @note Only the region of the flash memory reserved for data storage is accessible, and every address is + * relative to the beginning of that region. */ class Flash { public: + /** + * @brief Enum for the status of a flash memory operation. + */ + enum Status : uint8_t { + OK = 0, + MISALIGNED = 1, + OUT_OF_BOUNDS = 2, + ERROR = 3 + }; + /** * @brief Deleted constructor for static class. */ Flash() = delete; - // NOLINTBEGIN(*-avoid-c-arrays) + /** + * @brief Number of bytes of an erasable sector. + */ + static constexpr uint32_t sector_size{FLASH_SECTOR_SIZE}; /** - * @brief Read data from flash memory at an absolute address. - * - * @param address Address to read from (indexed by double words). - * @param data Pointer to store the data read. - * @param size Size in double words of the data to read. + * @brief Number of sectors reserved for data storage. */ - static void read(uint32_t address, uint32_t data[], uint32_t size = 1); + static constexpr uint16_t total_sectors{FLASH_SECTOR_TOTAL / 2}; /** - * @brief Read data from flash memory at an address relative to a page. - * - * @param page Page to read from (counting from the last to the first). - * @param page_address Address inside the page to read from (indexed by double words). - * @param data Pointer to store the data read. - * @param size Size in double words of the data to read. + * @brief Number of bytes reserved for data storage. */ - static void read(uint16_t page, uint16_t page_address, uint32_t data[], uint32_t size = 1); + static constexpr uint32_t total_size{total_sectors * sector_size}; /** - * @brief Write data to flash memory at an absolute address. + * @brief Read data from the flash memory at an absolute address. * - * @param address Address to write to (indexed by double words). - * @param data Pointer to the data to write. - * @param size Size in double words of the data to write. + * @param address Address to read from in bytes, relative to the beginning of the reserved region. + * @param size Number of bytes to read. + * @return View over the flash memory, empty if the data is outside of the reserved region. */ - static void write(uint32_t address, const uint32_t data[], uint32_t size = 1); + static std::span read(uint32_t address, uint32_t size); /** - * @brief Write data to flash memory at an address relative to a page. + * @brief Read data from the flash memory at an address relative to a sector. * - * @param page Page to write to (counting from the last to the first). - * @param page_address Address inside the page to write to (indexed by double words). - * @param data Pointer to the data to write. - * @param size Size in double words of the data to write. + * @param sector Sector to read from, counting from the first reserved sector. + * @param sector_address Address inside the sector to read from in bytes. + * @param size Number of bytes to read. + * @return View over the flash memory, empty if the data is outside of the reserved region. */ - static void write(uint16_t page, uint16_t page_address, const uint32_t data[], uint32_t size = 1); - - // NOLINTEND(*-avoid-c-arrays) + static std::span read(uint16_t sector, uint32_t sector_address, uint32_t size); /** - * @brief Erase flash memory pages. + * @brief Write data to the flash memory at an absolute address. + * + * @note The data is padded with the erased value up to the next flash word boundary, so the number of + * bytes actually written is the size of the data rounded up to a multiple of FlashWord::size. * - * @param page First page to erase (counting from the last to the first). - * @param number_of_pages Number of pages to erase. + * @param address Address to write to in bytes, must be a multiple of FlashWord::size. + * @param data Data to write. + * @return Status of the operation. */ - static void erase_pages(uint16_t page, uint16_t number_of_pages = 1); + static Status write(uint32_t address, std::span data); -private: /** - * @brief Number of double words per row. + * @brief Write data to the flash memory at an address relative to a sector. + * + * @note The data is padded with the erased value up to the next flash word boundary, so the number of + * bytes actually written is the size of the data rounded up to a multiple of FlashWord::size. + * + * @param sector Sector to write to, counting from the first reserved sector. + * @param sector_address Address inside the sector to write to in bytes, must be a multiple of + * FlashWord::size. + * @param data Data to write. + * @return Status of the operation. */ - static constexpr uint32_t bytes_per_flashword{4 * FLASH_NB_32BITWORD_IN_FLASHWORD}; + static Status write(uint16_t sector, uint32_t sector_address, std::span data); /** - * @brief Number of double words per page. + * @brief Erase sectors of the flash memory. + * + * @note This operation blocks the processor for around 2 s per sector, up to 4 s in the worst case, since + * the flash memory cannot be read while it is being erased. + * + * @param start_sector First sector to erase, counting from the first reserved sector. + * @param number_of_sectors Number of sectors to erase. + * @return Status of the operation. */ - static constexpr uint32_t flash_words_per_sector{FLASH_SECTOR_SIZE / bytes_per_flashword}; + static Status erase_sectors(uint16_t start_sector, uint16_t number_of_sectors = 1); }; } // namespace micras::hal diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index 3340f05..ac69e4a 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -2,6 +2,7 @@ * @file */ +#include #include #include @@ -9,61 +10,133 @@ namespace micras::hal { /** - * @brief Last address of the flash memory. + * @brief First address of the flash memory region reserved for data storage. * * @note Needs to be here because it is not defined at compile time. */ -static const uint32_t base_address = FLASH_BASE + FLASH_SIZE - 8; +static const uint32_t base_address = FLASH_BASE + FLASH_SIZE / 2; -// NOLINTNEXTLINE(*-avoid-c-arrays) -void Flash::read(uint32_t address, uint32_t data[], uint32_t size) { - const uint32_t end = base_address - 8 * address; +/** + * @brief Index of the first sector of the flash memory region reserved for data storage. + */ +static constexpr uint16_t base_sector{FLASH_SECTOR_TOTAL / 2}; + +/** + * @brief Round a number of bytes up to a whole number of flash words. + * + * @param size Number of bytes to round up. + * @return Number of bytes occupied in the flash memory. + */ +static constexpr uint32_t align_size(uint32_t size) { + return (size + FlashWord::size - 1) / FlashWord::size * FlashWord::size; +} - for (address = end - 8 * (size - 1); address <= end; address += 8, data++) { - (*data) = *(std::bit_cast(address)); +FlashWord::FlashWord(std::span data) { + if (data.size() >= size and std::bit_cast(data.data()) % alignof(uint32_t) == 0) { + this->source = std::bit_cast(data.data()); + return; } + + const std::span bytes{std::bit_cast(this->buffer.data()), size}; + + std::ranges::fill(bytes, erased_value); + std::ranges::copy(data.first(std::min(data.size(), size)), bytes.begin()); + + this->source = this->buffer.data(); } -// NOLINTNEXTLINE(*-avoid-c-arrays) -void Flash::read(uint16_t page, uint16_t page_address, uint32_t data[], uint32_t size) { - read(page * flash_words_per_sector + page_address, data, size); +const uint32_t* FlashWord::data() const { + return this->source; +} + +bool FlashWord::is_padded() const { + return this->source == this->buffer.data(); +} + +std::span Flash::read(uint32_t address, uint32_t size) { + if (address > total_size or size > total_size - address) { + return {}; + } + + return {std::bit_cast(base_address + address), size}; +} + +std::span Flash::read(uint16_t sector, uint32_t sector_address, uint32_t size) { + if (sector >= total_sectors or sector_address > sector_size) { + return {}; + } + + return read(sector * sector_size + sector_address, size); } -// NOLINTNEXTLINE(*-avoid-c-arrays) -void Flash::write(uint32_t address, const uint32_t data[], uint32_t size) { - HAL_FLASH_Unlock(); +Flash::Status Flash::write(uint32_t address, std::span data) { + if (address % FlashWord::size != 0) { + return MISALIGNED; + } + + if (address > total_size or align_size(data.size()) > total_size - address) { + return OUT_OF_BOUNDS; + } + + if (HAL_FLASH_Unlock() != HAL_OK) { + return ERROR; + } + + Status status = OK; - const uint32_t end = base_address - 8 * address; - address = end - 8 * (size - 1 + (bytes_per_flashword - size % bytes_per_flashword)); + for (uint32_t offset = 0; offset < data.size(); offset += FlashWord::size) { + const FlashWord word{data.subspan(offset)}; - while (size > 0) { - HAL_FLASH_Program(FLASH_TYPEPROGRAM_FLASHWORD, address, std::bit_cast(data)); - size -= bytes_per_flashword; - address += bytes_per_flashword; - data += bytes_per_flashword; + if (HAL_FLASH_Program( + FLASH_TYPEPROGRAM_FLASHWORD, base_address + address + offset, std::bit_cast(word.data()) + ) != HAL_OK) { + status = ERROR; + break; + } } HAL_FLASH_Lock(); + + return status; } -// NOLINTNEXTLINE(*-avoid-c-arrays) -void Flash::write(uint16_t page, uint16_t page_address, const uint32_t data[], uint32_t size) { - write(page * flash_words_per_sector + page_address, data, size); +Flash::Status Flash::write(uint16_t sector, uint32_t sector_address, std::span data) { + if (sector >= total_sectors or sector_address > sector_size) { + return OUT_OF_BOUNDS; + } + + return write(sector * sector_size + sector_address, data); } -void Flash::erase_pages(uint16_t page, uint16_t number_of_pages) { +Flash::Status Flash::erase_sectors(uint16_t start_sector, uint16_t number_of_sectors) { + if (start_sector >= total_sectors or number_of_sectors > total_sectors - start_sector) { + return OUT_OF_BOUNDS; + } + FLASH_EraseInitTypeDef erase_struct = { .TypeErase = FLASH_TYPEERASE_SECTORS, .Banks = FLASH_BANK_1, - .Sector = FLASH_SECTOR_TOTAL - page - number_of_pages, - .NbSectors = number_of_pages, + .Sector = static_cast(base_sector + start_sector), + .NbSectors = number_of_sectors, .VoltageRange = FLASH_VOLTAGE_RANGE_4 }; + if (HAL_FLASH_Unlock() != HAL_OK) { + return ERROR; + } + + pFlash.ErrorCode = HAL_FLASH_ERROR_NONE; + uint32_t sector_error{}; - HAL_FLASH_Unlock(); - HAL_FLASHEx_Erase(&erase_struct, §or_error); + const HAL_StatusTypeDef hal_status = HAL_FLASHEx_Erase(&erase_struct, §or_error); + HAL_FLASH_Lock(); + + if (hal_status != HAL_OK or sector_error != 0xFFFFFFFFU) { + return ERROR; + } + + return OK; } } // namespace micras::hal diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index 2b24a21..b9c8d69 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -26,8 +26,8 @@ class Storage { * @brief Configuration struct for the storage. */ struct Config { - uint16_t start_page; - uint16_t number_of_pages; + uint16_t start_sector; + uint16_t number_of_sectors; }; /** @@ -67,7 +67,8 @@ class Storage { */ template void sync(const std::string& name, T& data) { - if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr) { + if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr and + this->primitives.at(name).size == sizeof(T)) { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) data = reinterpret_cast(this->buffer.at(this->primitives.at(name).buffer_address)); } @@ -85,8 +86,20 @@ class Storage { /** * @brief Save the storage to the flash. + * + * @note This operation blocks the processor for a few seconds, so it must only be called with the robot + * stopped. + * + * @return True if the data was successfully written to the flash, false otherwise. + */ + bool save(); + + /** + * @brief Check if valid data was loaded from the flash. + * + * @return True if the storage was loaded from the flash, false if it started empty. */ - void save(); + bool is_valid() const; private: /** @@ -121,18 +134,26 @@ class Storage { * @brief Deserialize a map of variables. * * @tparam T Type of the variables. - * @param buffer Serialized buffer. + * @param buffer Serialized buffer, consumed up to the end of the map. * @param num_vars Number of variables. - * @return Deserialized map of variables. + * @param variables Map to store the deserialized variables. + * @return True if the buffer contained a consistent map, false otherwise. */ template - static std::unordered_map deserialize_var_map(std::vector& buffer, uint16_t num_vars); + static bool deserialize_var_map( + std::vector& buffer, uint16_t num_vars, std::unordered_map& variables + ); /** * @brief Start symbol to avoid reading garbage from flash. */ static constexpr uint16_t start_symbol = 0xABAB; + /** + * @brief Number of bytes of the header stored at the beginning of the storage. + */ + static constexpr uint16_t header_size = 8; + /** * @brief Map of primitive variables. */ @@ -149,14 +170,19 @@ class Storage { std::vector buffer; /** - * @brief Start page of the storage in the flash memory. + * @brief Start sector of the storage in the flash memory. + */ + uint16_t start_sector; + + /** + * @brief Maximum number of sectors used by the storage in the flash memory. */ - uint16_t start_page; + uint16_t number_of_sectors; /** - * @brief Maximum number of pages used by the storage in the flash memory. + * @brief Whether valid data was loaded from the flash memory. */ - uint16_t number_of_pages; + bool valid{}; }; } // namespace micras::proxy diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 1f6a5b0..6c528ca 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -2,29 +2,87 @@ * @file */ +#include #include +#include +#include #include "micras/hal/flash.hpp" #include "micras/proxy/storage.hpp" namespace micras::proxy { -Storage::Storage(const Config& config) : start_page{config.start_page}, number_of_pages{config.number_of_pages} { - uint64_t header{}; - hal::Flash::read(this->start_page, 0, std::bit_cast(&header), 2); +/** + * @brief Read a 16 bit little endian value from a buffer. + * + * @param buffer Buffer to read from. + * @param address Address of the first byte of the value. + * @return Value read from the buffer. + */ +static uint16_t read_uint16(std::span buffer, uint16_t address) { + return static_cast(buffer[address] | buffer[address + 1U] << 8); +} - if (header >> 48 != start_symbol) { +/** + * @brief Append a 16 bit little endian value to a buffer. + * + * @param buffer Buffer to append to. + * @param value Value to append. + */ +static void append_uint16(std::vector& buffer, uint16_t value) { + buffer.emplace_back(value); + buffer.emplace_back(value >> 8); +} + +/** + * @brief Check if the data of every variable of a map fits inside the buffer. + * + * @tparam T Type of the variables. + * @param variables Map of variables to check. + * @param buffer_size Number of bytes of the buffer. + * @return True if every variable is inside the buffer, false otherwise. + */ +template +static bool validate_var_map(const std::unordered_map& variables, std::size_t buffer_size) { + return std::ranges::all_of(variables, [buffer_size](const auto& variable) { + return variable.second.buffer_address + static_cast(variable.second.size) <= buffer_size; + }); +} + +Storage::Storage(const Config& config) : + start_sector{config.start_sector}, number_of_sectors{config.number_of_sectors} { + const std::span header = hal::Flash::read(this->start_sector, 0, header_size); + + if (header.size() < header_size or read_uint16(header, 0) != start_symbol) { return; } - const uint16_t total_size = header >> 32; - const uint16_t num_primitives = header >> 16; - const uint16_t num_serializables = header; + const uint16_t total_size = read_uint16(header, 2); + const uint16_t num_primitives = read_uint16(header, 4); + const uint16_t num_serializables = read_uint16(header, 6); + + const std::span payload = hal::Flash::read(this->start_sector, header_size, 4UL * total_size); + + if (payload.size() < 4UL * total_size) { + return; + } - this->buffer.resize(4L * total_size); - hal::Flash::read(this->start_page, 1, std::bit_cast(this->buffer.data()), total_size); + this->buffer.assign(payload.begin(), payload.end()); - this->primitives = deserialize_var_map(this->buffer, num_primitives); - this->serializables = deserialize_var_map(this->buffer, num_serializables); + if (not deserialize_var_map(this->buffer, num_primitives, this->primitives) or + not deserialize_var_map(this->buffer, num_serializables, this->serializables) or + not validate_var_map(this->primitives, this->buffer.size()) or + not validate_var_map(this->serializables, this->buffer.size())) { + this->primitives.clear(); + this->serializables.clear(); + this->buffer.clear(); + return; + } + + this->valid = true; +} + +bool Storage::is_valid() const { + return this->valid; } void Storage::create(const std::string& name, const core::ISerializable& data) { @@ -40,9 +98,8 @@ void Storage::sync(const std::string& name, core::ISerializable& data) { this->create(name, data); } -void Storage::save() { +bool Storage::save() { this->buffer.clear(); - hal::Flash::erase_pages(this->start_page, this->number_of_pages); for (auto it = this->primitives.begin(); it != this->primitives.end();) { auto& [name, variable] = *it; @@ -84,17 +141,29 @@ void Storage::save() { this->buffer.insert(this->buffer.end(), (4 - (this->buffer.size() % 4)) % 4, 0); const uint16_t total_size = this->buffer.size() / 4; - this->buffer.emplace_back(this->serializables.size()); - this->buffer.emplace_back(this->serializables.size() >> 8); - this->buffer.emplace_back(this->primitives.size()); - this->buffer.emplace_back(this->primitives.size() >> 8); + std::vector header; + header.reserve(header_size); + append_uint16(header, start_symbol); + append_uint16(header, total_size); + append_uint16(header, this->primitives.size()); + append_uint16(header, this->serializables.size()); + + this->buffer.insert(this->buffer.begin(), header.begin(), header.end()); + + if (this->buffer.size() > this->number_of_sectors * hal::Flash::sector_size) { + return false; + } + + if (hal::Flash::erase_sectors(this->start_sector, this->number_of_sectors) != hal::Flash::OK) { + return false; + } - this->buffer.emplace_back(total_size); - this->buffer.emplace_back(total_size >> 8); - this->buffer.emplace_back(start_symbol); - this->buffer.emplace_back(start_symbol >> 8); + if (hal::Flash::write(this->start_sector, 0, this->buffer) != hal::Flash::OK) { + return false; + } - hal::Flash::write(this->start_page, 0, std::bit_cast(buffer.data()), this->buffer.size() / 4); + this->valid = true; + return true; } template @@ -116,38 +185,50 @@ std::vector Storage::serialize_var_map(const std::unordered_map -std::unordered_map Storage::deserialize_var_map(std::vector& buffer, uint16_t num_vars) { - std::unordered_map variables; - +bool Storage::deserialize_var_map( + std::vector& buffer, uint16_t num_vars, std::unordered_map& variables +) { uint16_t current_addr = 0; for (uint16_t decoded_vars = 0; decoded_vars < num_vars; decoded_vars++) { - const uint8_t var_name_len = buffer.at(current_addr); + if (current_addr >= buffer.size()) { + return false; + } + + const uint8_t var_name_len = buffer[current_addr]; + + if (current_addr + var_name_len + 5UL > buffer.size()) { + return false; + } const std::string var_name(buffer.begin() + current_addr + 1, buffer.begin() + current_addr + 1 + var_name_len); current_addr += var_name_len + 1; - variables[var_name].buffer_address = buffer.at(current_addr) | buffer.at(current_addr + 1) << 8; + variables[var_name].buffer_address = read_uint16(buffer, current_addr); current_addr += 2; - variables.at(var_name).size = buffer.at(current_addr) | buffer.at(current_addr + 1) << 8; + variables.at(var_name).size = read_uint16(buffer, current_addr); current_addr += 2; } buffer.erase(buffer.begin(), buffer.begin() + current_addr); - return variables; + return true; } // Explicit instantiation of template functions template std::vector Storage::serialize_var_map(const std::unordered_map& variables); -template std::unordered_map - Storage::deserialize_var_map(std::vector& buffer, uint16_t num_vars); +template bool Storage::deserialize_var_map( + std::vector& buffer, uint16_t num_vars, + std::unordered_map& variables +); template std::vector Storage::serialize_var_map(const std::unordered_map& variables); -template std::unordered_map - Storage::deserialize_var_map(std::vector& buffer, uint16_t num_vars); +template bool Storage::deserialize_var_map( + std::vector& buffer, uint16_t num_vars, + std::unordered_map& variables +); } // namespace micras::proxy From 5e10da89d29fa24a5585c7029981abd19c014340 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 16 Sep 2026 22:03:42 -0300 Subject: [PATCH 385/561] =?UTF-8?q?=E2=9C=A8=20Add=20changes=20from=20micr?= =?UTF-8?q?as=201.5=20branch?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cmake/templates/run_clang_tidy.sh.in | 2 +- micras_nav/include/micras/nav/follow_wall.hpp | 14 ++++++++++++++ micras_nav/src/follow_wall.cpp | 8 ++++++++ 3 files changed, 23 insertions(+), 1 deletion(-) diff --git a/cmake/templates/run_clang_tidy.sh.in b/cmake/templates/run_clang_tidy.sh.in index 6f64bfd..8456b9e 100755 --- a/cmake/templates/run_clang_tidy.sh.in +++ b/cmake/templates/run_clang_tidy.sh.in @@ -22,7 +22,7 @@ ERRORS=$( done wait - } | grep -v ' warnings generated\.$' | awk '!seen[$0]++' + } | grep -Ev '[0-9]+ warnings? generated\.$' | awk '!seen[$0]++' ) if [[ -n "$ERRORS" ]]; then diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 09ceff9..715c493 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -57,6 +57,20 @@ class FollowWall { */ core::Observation get_observation() const; + /** + * @brief Check if the robot is currently following the left wall. + * + * @return True if following the left wall, false otherwise. + */ + bool get_is_following_left() const; + + /** + * @brief Check if the robot is currently following the right wall. + * + * @return True if following the right wall, false otherwise. + */ + bool get_is_following_right() const; + /** * @brief Reset the PID controller and the relative pose. */ diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 1b5f177..a5d13eb 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -114,4 +114,12 @@ void FollowWall::reset_displacement(bool reset_by_post) { this->last_blind_distance = 0.0F; this->reset_by_post = reset_by_post; } + +bool FollowWall::get_is_following_left() const { + return this->following_left; +} + +bool FollowWall::get_is_following_right() const { + return this->following_right; +} } // namespace micras::nav From 5d5f22cefcc254d203e5beb83750953d3b913b32 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 17 Sep 2026 03:22:05 -0300 Subject: [PATCH 386/561] =?UTF-8?q?=E2=9C=A8=20Drive=20all=20wall=20sensor?= =?UTF-8?q?=20emitters?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/proxy/wall_sensors.hpp | 20 +++++++------------ micras_proxy/src/wall_sensors.cpp | 13 ++++++------ 2 files changed, 14 insertions(+), 19 deletions(-) diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 3ce9053..8941eae 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -23,12 +23,11 @@ class TWallSensors { * @brief Configuration struct for wall sensors. */ struct Config { - hal::AdcDma::Config adc; - hal::Pwm::Config led_0_pwm; - hal::Pwm::Config led_1_pwm; - float filter_cutoff; - std::array base_readings; - float uncertainty; + hal::AdcDma::Config adc; + std::array led_pwms; + float filter_cutoff; + std::array base_readings; + float uncertainty; }; /** @@ -98,14 +97,9 @@ class TWallSensors { hal::AdcDma adc; /** - * @brief PWM handle for the even infrared LEDs. + * @brief PWM handles for the infrared LEDs, one for each sensor. */ - hal::Pwm led_0_pwm; - - /** - * @brief PWM handle for the odd infrared LEDs. - */ - hal::Pwm led_1_pwm; + std::array led_pwms; /** * @brief Buffer to store the ADC values. diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index c14b568..54d2aed 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -12,8 +12,7 @@ namespace micras::proxy { template TWallSensors::TWallSensors(const Config& config) : adc{config.adc}, - led_0_pwm{config.led_0_pwm}, - led_1_pwm{config.led_1_pwm}, + led_pwms{core::make_array(config.led_pwms)}, filters{core::make_array(config.filter_cutoff)}, base_readings{config.base_readings}, uncertainty{config.uncertainty} { @@ -23,14 +22,16 @@ TWallSensors::TWallSensors(const Config& config) : template void TWallSensors::turn_on() { - this->led_0_pwm.set_duty_cycle(50.0F); - this->led_1_pwm.set_duty_cycle(50.0F); + for (auto& led_pwm : this->led_pwms) { + led_pwm.set_duty_cycle(50.0F); + } } template void TWallSensors::turn_off() { - this->led_0_pwm.set_duty_cycle(0.0F); - this->led_1_pwm.set_duty_cycle(0.0F); + for (auto& led_pwm : this->led_pwms) { + led_pwm.set_duty_cycle(0.0F); + } } template From cc12355e675fa16ebd07ea7c6428a15bf6455da3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 17 Sep 2026 03:22:05 -0300 Subject: [PATCH 387/561] =?UTF-8?q?=F0=9F=A5=85=20Fail=20build=20on=20unsu?= =?UTF-8?q?pported=20ADC=20calibration=20platform?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/adc_dma.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 0879aba..3485c6c 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -14,6 +14,8 @@ AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, handle{c HAL_ADCEx_Calibration_Start(this->handle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED); #elif defined(STM32G4) HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); +#else + #error "ADC calibration is only supported for STM32H7 and STM32G4 platforms." #endif } From df29bad04b228aae4972179c801b58d7b3b26736 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 17 Sep 2026 03:22:05 -0300 Subject: [PATCH 388/561] =?UTF-8?q?=E2=9A=A1=20Enable=20instruction=20cach?= =?UTF-8?q?e?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/mcu.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp index 8110583..c2abbdb 100644 --- a/micras_hal/stm32/src/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -19,6 +19,8 @@ void SystemClock_Config(); namespace micras::hal { void Mcu::init() { + SCB_EnableICache(); + HAL_Init(); SystemClock_Config(); From 1be6bc0e12fa4e4102fe75a2950fb613d483a6db Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 17 Sep 2026 03:44:57 -0300 Subject: [PATCH 389/561] =?UTF-8?q?=F0=9F=90=9B=20Use=20timer=20clock=20to?= =?UTF-8?q?=20set=20PWM=20frequency?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/pwm.cpp | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index 7059462..60b3ee2 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -2,11 +2,34 @@ * @file */ +#include #include #include "micras/hal/pwm.hpp" namespace micras::hal { +/** + * @brief Get the frequency of the clock that feeds a timer. + * + * @note Timers run at twice the frequency of their APB bus when the APB prescaler is not 1. + * + * @param instance Timer peripheral. + * @return Timer clock frequency in Hz. + */ +static uint32_t get_timer_clock_frequency(const TIM_TypeDef* instance) { + RCC_ClkInitTypeDef clock_config{}; + uint32_t flash_latency{}; + HAL_RCC_GetClockConfig(&clock_config, &flash_latency); + + if (std::bit_cast(instance) >= APB2PERIPH_BASE) { + const uint32_t pclk2 = HAL_RCC_GetPCLK2Freq(); + return clock_config.APB2CLKDivider == RCC_APB2_DIV1 ? pclk2 : 2 * pclk2; + } + + const uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); + return clock_config.APB1CLKDivider == RCC_APB1_DIV1 ? pclk1 : 2 * pclk1; +} + Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel} { config.init_function(); HAL_TIM_PWM_Start(this->handle, this->channel); @@ -19,7 +42,7 @@ void Pwm::set_duty_cycle(float duty_cycle) { } void Pwm::set_frequency(uint32_t frequency) { - const uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); + const uint32_t base_freq = get_timer_clock_frequency(this->handle->Instance); const uint32_t prescaler = this->handle->Instance->PSC; const uint32_t autoreload = base_freq / ((prescaler + 1) * frequency) - 1; From 3f35f05ae7f82a600197b391eb8c3abdb6b47356 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 17 Sep 2026 03:44:57 -0300 Subject: [PATCH 390/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20clang-tidy=20crash?= =?UTF-8?q?=20in=20flash=20word=20alignment=20check?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/stm32/src/flash.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index ac69e4a..6730066 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -32,7 +32,9 @@ static constexpr uint32_t align_size(uint32_t size) { } FlashWord::FlashWord(std::span data) { - if (data.size() >= size and std::bit_cast(data.data()) % alignof(uint32_t) == 0) { + const auto data_address = std::bit_cast(data.data()); + + if (data.size() >= size and data_address % alignof(uint32_t) == 0) { this->source = std::bit_cast(data.data()); return; } From b1b1259a7a56ccea4b2d85c0ab724be23941a82e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 14 May 2025 17:10:03 -0300 Subject: [PATCH 391/561] =?UTF-8?q?=E2=9C=A8=20Add=20maze=20graph?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi --- micras_nav/include/micras/nav/maze.hpp | 12 +- micras_nav/include/micras/nav/maze_graph.hpp | 57 +++++++ micras_nav/src/maze.cpp | 63 +++++++- micras_nav/src/maze_graph.cpp | 161 +++++++++++++++++++ 4 files changed, 285 insertions(+), 8 deletions(-) create mode 100644 micras_nav/include/micras/nav/maze_graph.hpp create mode 100644 micras_nav/src/maze_graph.cpp diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index e28fc78..f9aa465 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -13,6 +13,7 @@ #include "micras/core/types.hpp" #include "micras/nav/costmap.hpp" #include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze_graph.hpp" namespace micras::nav { /** @@ -33,7 +34,12 @@ class TMaze : public core::ISerializable { /** * @brief Construct a new Maze object. */ - explicit TMaze(Config config); + explicit TMaze( + Config config, + EdgeCostFunction edge_cost_function = [](const GridPose& from, const GridPose& to) -> float { + return std::abs(from.position.x - to.position.x) + std::abs(from.position.y - to.position.y); + } + ); /** * @brief Update the maze walls with the current pose and new information. @@ -73,6 +79,8 @@ class TMaze : public core::ISerializable { */ const std::list& get_best_route() const; + void compute_graph(); + /** * @brief Serialize the best route to the goal. * @@ -144,6 +152,8 @@ class TMaze : public core::ISerializable { */ Costmap costmap; + MazeGraph graph; + /** * @brief Start pose of the robot in the maze. */ diff --git a/micras_nav/include/micras/nav/maze_graph.hpp b/micras_nav/include/micras/nav/maze_graph.hpp new file mode 100644 index 0000000..6bfb714 --- /dev/null +++ b/micras_nav/include/micras/nav/maze_graph.hpp @@ -0,0 +1,57 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MAZE_GRAPH_HPP +#define MICRAS_NAV_MAZE_GRAPH_HPP + +#include +#include + +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +using EdgeCostFunction = std::function; + +/** + * @brief + */ +class MazeGraph { +public: + struct Node { + explicit Node(GridPose pose) : pose(pose){}; + + GridPose pose; + std::unordered_map next_costs; + std::unordered_set prev; + }; + + explicit MazeGraph(EdgeCostFunction edge_cost_function = [](const GridPose& from, const GridPose& to) -> float { + return std::abs(from.position.x - to.position.x) + std::abs(from.position.y - to.position.y); + }); + + bool has_node(const GridPose& pose) const; + + void add_node(const GridPose& pose); + + void add_edge(const GridPose& from, const GridPose& to, float cost = 0.0F); + + void process_nodes(const GridPose& start); + +private: + void remove_extra_nodes(const GridPose& start); + + void add_extra_edges(const GridPose& start); + + void dijkstra(const GridPose& start, const GridPose& end); + + EdgeCostFunction get_edge_cost; + + /** + * @brief + */ + std::unordered_map graph; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_MAZE_GRAPH_HPP diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index a88068b..36afb11 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -6,13 +6,15 @@ #define MICRAS_NAV_MAZE_CPP #include +#include #include "micras/nav/grid_pose.hpp" #include "micras/nav/maze.hpp" namespace micras::nav { template -TMaze::TMaze(Config config) : start{config.start}, goal{config.goal}, cost_margin(config.cost_margin) { +TMaze::TMaze(Config config, EdgeCostFunction edge_cost_function) : + graph{std::move(edge_cost_function)}, start{config.start}, goal{config.goal}, cost_margin{config.cost_margin} { this->costmap.update_wall(this->start, false); this->costmap.update_wall(start.turned_right(), true); @@ -84,9 +86,14 @@ GridPose TMaze::get_next_goal(const GridPose& pose, bool returnin int16_t current_cost = max_cost; GridPose next_pose = {}; - for (Side side : - {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, - pose.turned_back().orientation}) { + auto sides_order = { + pose.turned_right().orientation, + pose.turned_left().orientation, + pose.orientation, + pose.turned_back().orientation, + }; + + for (Side side : sides_order) { if (this->costmap.has_wall({pose.position, side})) { continue; } @@ -130,6 +137,43 @@ const std::list& TMaze::get_best_route() const { return this->best_route; } +template +void TMaze::compute_graph() { + std::queue queue; + this->graph.add_node(this->start); + queue.push(this->start); + + while (not queue.empty()) { + GridPose current_pose = queue.front(); + queue.pop(); + + if (this->goal.contains(current_pose.position)) { + continue; + } + + auto sides_order = { + current_pose.turned_right().orientation, + current_pose.turned_left().orientation, + current_pose.orientation, + }; + + for (Side side : sides_order) { + if (this->costmap.has_wall({current_pose.position, side}, true)) { + continue; + } + + GridPose next_pose = {current_pose.position + side, side}; + + if (not this->graph.has_node(next_pose)) { + this->graph.add_node(next_pose); + queue.push(next_pose); + } + + this->graph.add_edge(current_pose, next_pose); + } + } +} + template std::vector TMaze::serialize() const { std::vector buffer; @@ -187,9 +231,14 @@ GridPose TMaze::get_next_bfs_goal(const GridPose& pose, bool disc checked.at(pose.position.y).at(pose.position.x) = true; - for (Side side : - {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, - pose.turned_back().orientation}) { + auto sides_order = { + pose.turned_right().orientation, + pose.turned_left().orientation, + pose.orientation, + pose.turned_back().orientation, + }; + + for (Side side : sides_order) { if (not this->costmap.has_wall({pose.position, side})) { queue.emplace(pose.position + side, side); } diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp new file mode 100644 index 0000000..85a99fb --- /dev/null +++ b/micras_nav/src/maze_graph.cpp @@ -0,0 +1,161 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/nav/maze_graph.hpp" +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +MazeGraph::MazeGraph(EdgeCostFunction edge_cost_function) : get_edge_cost(std::move(edge_cost_function)) { } + +bool MazeGraph::has_node(const GridPose& pose) const { + return this->graph.contains(pose); +} + +void MazeGraph::add_node(const GridPose& pose) { + this->graph.emplace(pose, Node{pose}); +} + +void MazeGraph::add_edge(const GridPose& from, const GridPose& to, float cost) { + if (this->graph.at(from).next_costs.contains(to)) { + return; + } + + this->graph.at(from).next_costs[to] = cost; + this->graph.at(to).prev.emplace(from); +} + +void MazeGraph::process_nodes(const GridPose& start) { + this->remove_extra_nodes(start); + this->add_extra_edges(start); +} + +void MazeGraph::remove_extra_nodes(const GridPose& start) { + std::unordered_set visited; + std::queue queue; + queue.push(start); + + while (not queue.empty()) { + const GridPose current_pose = queue.front(); + Node& current_node = this->graph.at(current_pose); + queue.pop(); + + visited.insert(current_pose); + + for (auto next_it = current_node.next_costs.begin(); next_it != current_node.next_costs.end();) { + Node& next_node = this->graph.at(next_it->first); + + if (next_node.next_costs.size() == 1) { + const GridPose& next_next = next_node.next_costs.begin()->first; + + if (next_next.orientation == current_pose.orientation or + next_next.orientation == current_pose.turned_back().orientation) { + current_node.next_costs.erase(next_it); + next_node.prev.erase(current_pose); + + this->add_edge(current_pose, next_next); + + if (next_node.prev.empty()) { + this->graph.erase(next_node.pose); + this->graph.at(next_next).prev.erase(next_node.pose); + } + + next_it = current_node.next_costs.begin(); + continue; + } + } + + if (not visited.contains(next_it->first)) { + queue.push(next_it->first); + } + + next_it++; + } + } +} + +void MazeGraph::add_extra_edges(const GridPose& start) { + std::unordered_set visited; + std::queue queue; + queue.push(start); + + while (not queue.empty()) { + const GridPose current_pose = queue.front(); + Node& current_node = this->graph.at(current_pose); + queue.pop(); + + visited.insert(current_pose); + + for (auto next_it = current_node.next_costs.begin(); next_it != current_node.next_costs.end();) { + if (next_it->second == 0) { + next_it->second = this->get_edge_cost(current_pose, next_it->first); + } + + GridPose next_pose = next_it->first; + + for (const auto& [next_next_pose, next_next_cost] : this->graph.at(next_pose).next_costs) { + if (current_node.next_costs.contains(next_next_pose)) { + continue; + } + + const float edge_cost = this->get_edge_cost(current_pose, next_next_pose); + + if (edge_cost > 0) { + this->add_edge(current_pose, next_next_pose, edge_cost); + next_it = current_node.next_costs.begin(); + } + } + + if (not visited.contains(next_it->first)) { + queue.push(next_it->first); + } + + next_it++; + } + } +} + +void MazeGraph::dijkstra(const GridPose& start, const GridPose& end) { + std::unordered_map distance; + std::unordered_set visited; + + auto comp_function = [&distance](const GridPose& first, const GridPose& second) { + return distance.at(first) > distance.at(second); + }; + + std::priority_queue, decltype(comp_function)> queue(comp_function); + + for (const auto& [pose, node] : this->graph) { + distance[pose] = std::numeric_limits::max(); + } + + distance[start] = 0; + queue.push(start); + + while (!queue.empty()) { + const GridPose current_pose = queue.top(); + queue.pop(); + + if (visited.contains(current_pose)) { + continue; + } + + if (current_pose == end) { + break; + } + + visited.insert(current_pose); + + for (const auto& [next_pose, edge_cost] : this->graph.at(current_pose).next_costs) { + if (distance.at(current_pose) > (distance.at(current_pose) + edge_cost)) { + distance.at(current_pose) = distance.at(current_pose) + edge_cost; + queue.push(next_pose); + } + } + } +} +} // namespace micras::nav From 0129d0e91be323883c22a32b37512e9f81a69056 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 14 May 2025 17:10:49 -0300 Subject: [PATCH 392/561] =?UTF-8?q?=E2=9C=A8=20Add=20action=20id=20and=20t?= =?UTF-8?q?otal=20time=20to=20actions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi --- .../include/micras/nav/actions/base.hpp | 29 ++++++++--- .../include/micras/nav/actions/move.hpp | 52 +++++++++++++------ .../include/micras/nav/actions/turn.hpp | 24 +++++---- 3 files changed, 75 insertions(+), 30 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 93f24ea..009a90c 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -13,12 +13,17 @@ namespace micras::nav { */ class Action { public: + struct Id { + uint8_t type; + float value; + }; + /** * @brief Constructor for the Action class. * * @param action_id The ID of the action. */ - explicit Action(uint8_t action_id) : id(action_id) { } + Action(const Id& action_id, bool follow_wall) : id{action_id}, follow_wall{follow_wall} { } /** * @brief Virtual destructor for the Action class. @@ -42,18 +47,25 @@ class Action { virtual bool finished(const Pose& pose) const = 0; /** - * @brief Check if the action allow the robot to follow walls. + * @brief Get the total time it takes to perform the action. * - * @return True if the action allows the robot to follow walls, false otherwise. + * @return The total time of the action. */ - virtual bool allow_follow_wall() const = 0; + virtual float get_total_time() const = 0; /** * @brief Get the ID of the action. * * @return The ID of the action. */ - uint8_t get_id() const { return id; } + const Id& get_id() const { return id; } + + /** + * @brief Check if the action allow the robot to follow walls. + * + * @return True if the action allows the robot to follow walls, false otherwise. + */ + bool allow_follow_wall() const { return follow_wall; } protected: /** @@ -70,7 +82,12 @@ class Action { /** * @brief The ID of the action. */ - uint8_t id; + Id id; + + /** + * @brief Whether the robot can follow walls while executing this action. + */ + bool follow_wall; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index fa9a7e3..9212187 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -28,20 +28,21 @@ class MoveAction : public Action { * @param follow_wall Whether the robot can follow wall while executing this action. */ MoveAction( - uint8_t action_id, float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, - float max_deceleration, bool follow_wall = true + uint8_t action_type, float distance, float start_speed, float end_speed, float max_speed, + float max_acceleration, float max_deceleration, bool follow_wall = true ) : - Action{action_id}, + Action{{action_type, distance}, follow_wall}, distance(distance), start_speed_2(start_speed * start_speed), end_speed_2(end_speed * end_speed), max_speed{max_speed}, max_acceleration_doubled{2.0F * max_acceleration}, max_deceleration_doubled{2.0F * max_deceleration}, - follow_wall(follow_wall), decelerate_distance{ (end_speed_2 - start_speed_2 + max_deceleration_doubled * distance) / (max_acceleration_doubled + max_deceleration_doubled) + }, + total_time{calculate_total_time(distance, start_speed, end_speed, max_speed, max_acceleration, max_deceleration) } { } /** @@ -82,14 +83,38 @@ class MoveAction : public Action { */ bool finished(const Pose& pose) const override { return pose.position.magnitude() >= this->distance; } - /** - * @brief Check if the action allows the robot to follow walls. - * - * @return True if the action allows the robot to follow walls, false otherwise. - */ - bool allow_follow_wall() const override { return this->follow_wall; } + float get_total_time() const override { return this->total_time; } + + void trim_distance(float reduction) { + this->distance -= reduction; + + this->decelerate_distance = + (this->end_speed_2 - this->start_speed_2 + this->max_deceleration_doubled * this->distance) / + (this->max_acceleration_doubled + this->max_deceleration_doubled); + + this->total_time = calculate_total_time( + this->distance, std::sqrt(this->start_speed_2), std::sqrt(this->end_speed_2), this->max_speed, + this->max_acceleration_doubled / 2.0F, this->max_deceleration_doubled / 2.0F + ); + } private: + static constexpr float calculate_total_time( + float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, + float max_deceleration + ) { + const float acceleration_time = (max_speed - start_speed) / max_acceleration; + const float deceleration_time = (max_speed - end_speed) / max_deceleration; + + const float acceleration_distance = + (start_speed * start_speed + max_speed * max_speed) / (2.0F * max_acceleration); + const float deceleration_distance = (max_speed * max_speed + end_speed * end_speed) / (2.0F * max_deceleration); + const float constant_speed_distance = distance - acceleration_distance - deceleration_distance; + const float constant_speed_time = constant_speed_distance / max_speed; + + return acceleration_time + deceleration_time + constant_speed_time; + } + /** * @brief Distance to move in meters. */ @@ -120,15 +145,12 @@ class MoveAction : public Action { */ float max_deceleration_doubled; - /** - * @brief Whether the robot can follow walls while executing this action. - */ - bool follow_wall; - /** * @brief Distance from the start where the robot should start to decelerate in meters. */ float decelerate_distance; + + float total_time; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index c7a2a18..2b0e821 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -32,13 +32,16 @@ class TurnAction : public Action { * @param linear_speed Linear speed in m/s. * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. */ - TurnAction(uint8_t action_id, float angle, float curve_radius, float linear_speed, float max_angular_acceleration) : - Action{action_id}, + TurnAction( + uint8_t action_type, float angle, float curve_radius, float linear_speed, float max_angular_acceleration + ) : + Action{{action_type, angle}, false}, start_orientation{max_angular_acceleration * 0.001F * 0.001F / 2.0F}, angle{angle}, linear_speed{linear_speed}, acceleration{max_angular_acceleration}, - max_angular_speed{calculate_max_angular_speed(angle, curve_radius, linear_speed, max_angular_acceleration)} { } + max_angular_speed{calculate_max_angular_speed(angle, curve_radius, linear_speed, max_angular_acceleration)}, + total_time{calculate_total_time(angle, max_angular_speed, max_angular_acceleration)} { } /** * @brief Get the desired speeds for the robot to complete the action. @@ -78,12 +81,7 @@ class TurnAction : public Action { */ bool finished(const Pose& pose) const override { return std::abs(pose.orientation) >= std::abs(this->angle); } - /** - * @brief Check if the action allows following the wall. - * - * @return True if the action allows following the wall, false otherwise. - */ - bool allow_follow_wall() const override { return false; } + float get_total_time() const override { return this->total_time; } private: /** @@ -113,6 +111,12 @@ class TurnAction : public Action { return correction_factor * max_angular_acceleration * (radius_speed_ratio - std::sqrt(discriminant)); } + static constexpr float calculate_total_time(float angle, float max_angular_speed, float max_angular_acceleration) { + const float acceleration_time = max_angular_speed / max_angular_acceleration; + + return std::abs(angle) / max_angular_speed + acceleration_time; + } + /** * @brief Start orientation in radians. Being zero causes the robot to not move. */ @@ -137,6 +141,8 @@ class TurnAction : public Action { * @brief Maximum angular speed in rad/s while turning. */ float max_angular_speed; + + float total_time; }; } // namespace micras::nav From 15cdd3acb97e3256af46c44eed178b358fa89cf2 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 14 May 2025 17:11:12 -0300 Subject: [PATCH 393/561] =?UTF-8?q?=E2=9C=A8=20Add=20get=5Frelative=5Fside?= =?UTF-8?q?=20to=20GridPose?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi --- micras_nav/include/micras/nav/grid_pose.hpp | 22 +++++++++++++++++++++ micras_nav/src/grid_pose.cpp | 12 +++++++++++ 2 files changed, 34 insertions(+) diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index d1c6b22..00fefe6 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -114,6 +114,14 @@ struct GridPose { */ GridPose turned_right() const; + /** + * @brief Gets the relative side of a grid point compared to current pose. + * + * @param other The grid point to check. + * @return LEFT/RIGHT for left/right of pose, UP for in front, DOWN for behind. + */ + Side get_relative_side(const GridPoint& other) const; + /** * @brief Compare two poses for equality. * @@ -148,6 +156,20 @@ struct hash { return h1 ^ (h2 << 1); } }; + +/** + * @brief Hash specialization for the GridPose type. + * + * @tparam T GridPose type. + */ +template <> +struct hash { + size_t operator()(const micras::nav::GridPose& pose) const noexcept { + const size_t h1 = hash{}(pose.position); + const size_t h2 = hash{}(pose.orientation); + return h1 ^ (h2 << 1); + } +}; } // namespace std #endif // MICRAS_NAV_GRID_POSE_HPP diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index ecb21b1..2967e15 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -75,6 +75,18 @@ GridPose GridPose::turned_right() const { return {this->position, static_cast((this->orientation + 3) % 4)}; } +Side GridPose::get_relative_side(const GridPoint& other) const { + const GridPoint front_position = this->front().position; + const float cross_product = (front_position.x - this->position.x) * (other.y - this->position.y) - + (front_position.y - this->position.y) * (other.x - this->position.x); + + if (cross_product == 0) { + return this->position.direction(other) == this->orientation ? Side::UP : Side::DOWN; + } + + return cross_product > 0 ? Side::LEFT : Side::RIGHT; +} + bool GridPose::operator==(const GridPose& other) const { return this->position == other.position and this->orientation == other.orientation; } From deb2136ec2ed9a9ce444f9d000d087d7ecdd3d64 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 14 May 2025 17:12:20 -0300 Subject: [PATCH 394/561] =?UTF-8?q?=E2=9C=A8=20Add=20push=5Fsolving=20func?= =?UTF-8?q?tion=20to=20action=20queuer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi --- .../include/micras/nav/action_queuer.hpp | 54 +++++-- micras_nav/src/action_queuer.cpp | 141 +++++++++++++++--- 2 files changed, 165 insertions(+), 30 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 5119f03..2f02678 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -5,9 +5,9 @@ #ifndef MICRAS_NAV_ACTION_QUEUER_HPP #define MICRAS_NAV_ACTION_QUEUER_HPP +#include #include #include -#include #include "micras/nav/actions/move.hpp" #include "micras/nav/actions/turn.hpp" @@ -25,10 +25,9 @@ class ActionQueuer { STOP = 0, START = 1, MOVE_FORWARD = 2, - MOVE_HALF = 3, - TURN_LEFT = 4, - TURN_RIGHT = 5, - TURN_BACK = 6, + TURN = 3, + SPIN = 4, + DIAGONAL = 5, }; /** @@ -58,12 +57,20 @@ class ActionQueuer { explicit ActionQueuer(Config config); /** - * @brief Push an action to the queue. + * @brief Push an exploring action to the queue. * * @param current_pose Current pose of the robot. * @param target_position Target position to move to. */ - void push(const GridPose& current_pose, const GridPoint& target_position); + void push_exploring(const GridPose& origin_pose, const GridPoint& target_position); + + /** + * @brief Push a solving action to the queue. + * + * @param current_pose Current pose of the robot. + * @param target_angle Target pose to move to.. + */ + void push_solving(const GridPose& origin_pose, const GridPose& target_pose); /** * @brief Pop an action from the queue. @@ -82,7 +89,14 @@ class ActionQueuer { /** * @brief Fill the action queue with a sequence of actions to the end. */ - void recompute(const std::list& best_route); + void recompute(const std::list& best_route, bool add_start = true); + + /** + * @brief Get the total time to complete the actions in the queue. + * + * @return Total time to complete the actions in the queue. + */ + float get_total_time() const; private: /** @@ -98,7 +112,12 @@ class ActionQueuer { /** * @brief Dynamic solving parameters. */ - // Config::Dynamic solving_params; + Config::Dynamic solving_params; + + /** + * @brief Solving linear speed of the robot when performing a curve. + */ + float curve_linear_speed; /** * @brief Pre-built actions to use in the exploration. @@ -116,7 +135,22 @@ class ActionQueuer { /** * @brief Queue of actions to be performed. */ - std::queue> action_queue; + std::deque> action_queue; +}; + +class ActionCost { +public: + explicit ActionCost(const ActionQueuer::Config& config) : action_queuer(config) { } + + float operator()(const GridPose& from, const GridPose& to) { + this->action_queuer.push_solving(from, to); + const float total_time = this->action_queuer.get_total_time(); + this->action_queuer.recompute({}, false); + + return total_time; + } + + ActionQueuer action_queuer; }; } // namespace micras::nav diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 20768eb..71ff65f 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -2,15 +2,18 @@ * @file */ +#include #include #include "micras/nav/action_queuer.hpp" +#include "micras/nav/actions/move.hpp" namespace micras::nav { ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, exploring_params{config.exploring}, - // solving_params{config.solving}, + solving_params{config.solving}, + curve_linear_speed{std::sqrt(config.solving.max_centrifugal_acceleration * config.solving.curve_radius)}, stop{std::make_shared( ActionType::STOP, cell_size / 2.0F, exploring_params.max_linear_speed, 0.0F, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false @@ -26,49 +29,131 @@ ActionQueuer::ActionQueuer(Config config) : exploring_params.max_linear_deceleration )}, move_half{std::make_shared( - ActionType::MOVE_HALF, cell_size / 2.0F, 0.001F * exploring_params.max_linear_acceleration, + ActionType::MOVE_FORWARD, cell_size / 2.0F, 0.001F * exploring_params.max_linear_acceleration, exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false )}, turn_left{std::make_shared( - ActionType::TURN_LEFT, std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, + ActionType::TURN, std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, exploring_params.max_angular_acceleration )}, turn_right{std::make_shared( - ActionType::TURN_RIGHT, -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, + ActionType::TURN, -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, exploring_params.max_angular_acceleration )}, turn_back{std::make_shared( - ActionType::TURN_BACK, std::numbers::pi_v, 0.0F, 0.0F, exploring_params.max_angular_acceleration + ActionType::SPIN, std::numbers::pi_v, 0.0F, 0.0F, exploring_params.max_angular_acceleration )} { } -void ActionQueuer::push(const GridPose& current_pose, const GridPoint& target_position) { - if (current_pose.front().position == target_position) { - this->action_queue.emplace(move_forward); +void ActionQueuer::push_exploring(const GridPose& origin_pose, const GridPoint& target_position) { + if (origin_pose.front().position == target_position) { + this->action_queue.emplace_back(move_forward); return; } - if (current_pose.turned_left().front().position == target_position) { - this->action_queue.emplace(turn_left); + if (origin_pose.turned_left().front().position == target_position) { + this->action_queue.emplace_back(turn_left); return; } - if (current_pose.turned_right().front().position == target_position) { - this->action_queue.emplace(turn_right); + if (origin_pose.turned_right().front().position == target_position) { + this->action_queue.emplace_back(turn_right); return; } - if (current_pose.turned_back().front().position == target_position) { - this->action_queue.emplace(stop); - this->action_queue.emplace(turn_back); - this->action_queue.emplace(move_half); + if (origin_pose.turned_back().front().position == target_position) { + this->action_queue.emplace_back(stop); + this->action_queue.emplace_back(turn_back); + this->action_queue.emplace_back(move_half); return; } } +void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& target_pose) { + const Side relative_side = origin_pose.get_relative_side(target_pose.position); + + if (relative_side == Side::UP and origin_pose.orientation == target_pose.orientation) { + const float distance = + origin_pose.position.to_vector(this->cell_size).distance(target_pose.position.to_vector(this->cell_size)); + this->action_queue.emplace_back(std::make_shared( + ActionType::MOVE_FORWARD, distance, this->curve_linear_speed, this->curve_linear_speed, + this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, + this->solving_params.max_linear_deceleration + )); + } + + if (origin_pose.turned_left().front() == target_pose) { + this->action_queue.emplace_back(std::make_shared( + ActionType::TURN, std::numbers::pi_v / 2.0F, this->cell_size / 2.0F, this->curve_linear_speed, + this->solving_params.max_angular_acceleration + )); + return; + } + + if (origin_pose.turned_right().front() == target_pose) { + this->action_queue.emplace_back(std::make_shared( + ActionType::TURN, -std::numbers::pi_v / 2.0F, this->cell_size / 2.0F, this->curve_linear_speed, + this->solving_params.max_angular_acceleration + )); + return; + } + + if (origin_pose.turned_left().front().turned_left().front() == target_pose) { + this->action_queue.emplace_back(std::make_shared( + ActionType::TURN, std::numbers::pi_v, this->cell_size / 2.0F, this->curve_linear_speed, + this->solving_params.max_angular_acceleration + )); + return; + } + + if (origin_pose.turned_right().front().turned_right().front() == target_pose) { + this->action_queue.emplace_back(std::make_shared( + ActionType::TURN, -std::numbers::pi_v, this->cell_size / 2.0F, this->curve_linear_speed, + this->solving_params.max_angular_acceleration + )); + return; + } + + const bool keep_direction = (origin_pose.orientation == target_pose.orientation); + const GridPoint target_position = (keep_direction ? target_pose : target_pose.turned_back().front()).position; + + if (std::abs(target_position.x - origin_pose.position.x) != std::abs(target_position.y - origin_pose.position.y)) { + return; + } + + float turn_angle = + relative_side == Side::LEFT ? std::numbers::pi_v / 4.0F : -std::numbers::pi_v / 4.0F; + + this->action_queue.emplace_back(std::make_shared( + ActionType::TURN, turn_angle, this->cell_size / 2.0F, this->curve_linear_speed, + this->solving_params.max_angular_acceleration + )); + + float distance = + origin_pose.position.to_vector(this->cell_size).distance(target_position.to_vector(this->cell_size)) - + std::tan(std::numbers::pi_v / 8.0F) * this->cell_size; + + if (not keep_direction) { + turn_angle *= -1.0F; + } else { + distance += std::numbers::sqrt2_v / 2; + } + + this->action_queue.emplace_back(std::make_shared( + ActionType::DIAGONAL, distance, this->curve_linear_speed, this->curve_linear_speed, + this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, + this->solving_params.max_linear_deceleration + )); + + this->action_queue.emplace_back(std::make_shared( + ActionType::TURN, turn_angle, this->cell_size / 2.0F, this->curve_linear_speed, + this->solving_params.max_angular_acceleration + )); +} + std::shared_ptr ActionQueuer::pop() { const auto& action = this->action_queue.front(); - this->action_queue.pop(); + this->action_queue.pop_front(); return action; } @@ -76,16 +161,32 @@ bool ActionQueuer::empty() const { return this->action_queue.empty(); } -void ActionQueuer::recompute(const std::list& best_route) { +void ActionQueuer::recompute(const std::list& best_route, bool add_start) { this->action_queue = {}; - this->action_queue.emplace(start); + + if (add_start) { + this->action_queue.emplace_back(start); + } if (best_route.empty()) { return; } for (auto it = std::next(best_route.begin()); std::next(it) != best_route.end(); it++) { - this->push(*it, std::next(it)->position); + this->push_solving(*it, *std::next(it)); + uint16_t last_index = this->action_queue.size() - 1; + + if (this->action_queue.back()->get_id().type == ActionType::DIAGONAL) { } + } +} + +float ActionQueuer::get_total_time() const { + float total_time = 0.0F; + + for (const auto& action : this->action_queue) { + total_time += action->get_total_time(); } + + return total_time; } } // namespace micras::nav From aadfdadf0fe093910e008df7d77c53342513205f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 14 May 2025 22:45:52 -0300 Subject: [PATCH 395/561] =?UTF-8?q?=F0=9F=93=9D=20Add=20comments=20to=20na?= =?UTF-8?q?v=20methods?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/actions/base.hpp | 7 ++- .../include/micras/nav/actions/move.hpp | 47 ++++++++++++++-- .../include/micras/nav/actions/turn.hpp | 26 ++++++--- micras_nav/include/micras/nav/maze.hpp | 18 +++++- micras_nav/include/micras/nav/maze_graph.hpp | 56 ++++++++++++++++++- micras_nav/src/action_queuer.cpp | 8 +-- micras_nav/src/maze.cpp | 41 +++++++------- 7 files changed, 158 insertions(+), 45 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 009a90c..6392acd 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -49,7 +49,7 @@ class Action { /** * @brief Get the total time it takes to perform the action. * - * @return The total time of the action. + * @return The total time of the action in seconds. */ virtual float get_total_time() const = 0; @@ -67,6 +67,11 @@ class Action { */ bool allow_follow_wall() const { return follow_wall; } + /** + * @brief Time step for the action in seconds. + */ + static constexpr float time_step{0.001F}; + protected: /** * @brief Special member functions declared as default. diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 9212187..371068c 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -18,7 +18,7 @@ class MoveAction : public Action { /** * @brief Construct a new Move Action object. * - * @param action_id The ID of the action. + * @param action_type The type of the action to be performed. * @param distance Distance to move in meters. * @param start_speed Initial speed in m/s. * @param end_speed Final speed in m/s. @@ -32,6 +32,8 @@ class MoveAction : public Action { float max_acceleration, float max_deceleration, bool follow_wall = true ) : Action{{action_type, distance}, follow_wall}, + positive_linear_speed_step{max_acceleration * Action::time_step}, + negative_linear_speed_step{max_deceleration * Action::time_step}, distance(distance), start_speed_2(start_speed * start_speed), end_speed_2(end_speed * end_speed), @@ -59,13 +61,16 @@ class MoveAction : public Action { if (current_distance < this->decelerate_distance) { twist = { - .linear = std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance), + .linear = std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance) + + this->positive_linear_speed_step, .angular = 0.0F, }; } else { twist = { - .linear = - std::sqrt(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), + .linear = std::sqrt( + this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance) + ) - + this->negative_linear_speed_step, .angular = 0.0F, }; } @@ -83,8 +88,18 @@ class MoveAction : public Action { */ bool finished(const Pose& pose) const override { return pose.position.magnitude() >= this->distance; } + /** + * @brief Get the total time it takes to perform the action. + * + * @return The total time of the action in seconds. + */ float get_total_time() const override { return this->total_time; } + /** + * @brief Reduce the distance to move and recalculate the total time. + * + * @param reduction Amount to reduce the distance in meters. + */ void trim_distance(float reduction) { this->distance -= reduction; @@ -99,6 +114,17 @@ class MoveAction : public Action { } private: + /** + * @brief Calculate the total time to complete the action. + * + * @param distance Distance to move in meters. + * @param start_speed Initial speed in m/s. + * @param end_speed Final speed in m/s. + * @param max_speed Maximum speed in m/s. + * @param max_acceleration Maximum acceleration in m/s^2. + * @param max_deceleration Maximum deceleration in m/s^2. + * @return Total time to complete the action in seconds. + */ static constexpr float calculate_total_time( float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, float max_deceleration @@ -115,6 +141,16 @@ class MoveAction : public Action { return acceleration_time + deceleration_time + constant_speed_time; } + /** + * @brief Linear speed step in m/s to the next loop when accelerating. + */ + float positive_linear_speed_step{}; + + /** + * @brief Linear speed step in m/s to the next loop when decelerating. + */ + float negative_linear_speed_step{}; + /** * @brief Distance to move in meters. */ @@ -150,6 +186,9 @@ class MoveAction : public Action { */ float decelerate_distance; + /** + * @brief Total time to complete the action in seconds. + */ float total_time; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 2b0e821..3c46e61 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -26,7 +26,7 @@ class TurnAction : public Action { /** * @brief Construct a new Turn Action object. * - * @param action_id The ID of the action. + * @param action_type The type of the action to be performed. * @param angle Angle to turn in radians. * @param curve_radius Radius of the curve in meters. * @param linear_speed Linear speed in m/s. @@ -36,7 +36,7 @@ class TurnAction : public Action { uint8_t action_type, float angle, float curve_radius, float linear_speed, float max_angular_acceleration ) : Action{{action_type, angle}, false}, - start_orientation{max_angular_acceleration * 0.001F * 0.001F / 2.0F}, + angular_speed_step{max_angular_acceleration * Action::time_step}, angle{angle}, linear_speed{linear_speed}, acceleration{max_angular_acceleration}, @@ -52,18 +52,18 @@ class TurnAction : public Action { * @details The desired velocity is calculated from the angular displacement based on the Torricelli equation. */ Twist get_speeds(const Pose& pose) const override { - Twist twist{}; - const float current_orientation = std::max(std::abs(pose.orientation), this->start_orientation); + Twist twist{}; - if (current_orientation < std::abs(this->angle)) { + if (pose.orientation < std::abs(this->angle)) { twist = { .linear = linear_speed, - .angular = std::sqrt(2.0F * this->acceleration * current_orientation), + .angular = std::sqrt(2.0F * this->acceleration * pose.orientation) + this->angular_speed_step, }; } else { twist = { .linear = linear_speed, - .angular = std::sqrt(2.0F * this->acceleration * (std::abs(this->angle) - current_orientation)), + .angular = std::sqrt(2.0F * this->acceleration * (std::abs(this->angle) - pose.orientation)) - + this->angular_speed_step, }; } @@ -81,6 +81,11 @@ class TurnAction : public Action { */ bool finished(const Pose& pose) const override { return std::abs(pose.orientation) >= std::abs(this->angle); } + /** + * @brief Get the total time it takes to perform the action. + * + * @return The total time of the action in seconds. + */ float get_total_time() const override { return this->total_time; } private: @@ -118,9 +123,9 @@ class TurnAction : public Action { } /** - * @brief Start orientation in radians. Being zero causes the robot to not move. + * @brief Angular step in radians to the next loop. */ - float start_orientation; + float angular_speed_step; /** * @brief Angle to turn in radians. @@ -142,6 +147,9 @@ class TurnAction : public Action { */ float max_angular_speed; + /** + * @brief Total time to complete the action in seconds. + */ float total_time; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index f9aa465..7b51ea0 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -37,7 +37,8 @@ class TMaze : public core::ISerializable { explicit TMaze( Config config, EdgeCostFunction edge_cost_function = [](const GridPose& from, const GridPose& to) -> float { - return std::abs(from.position.x - to.position.x) + std::abs(from.position.y - to.position.y); + float edge_cost = std::abs(from.position.x - to.position.x) + std::abs(from.position.y - to.position.y); + return edge_cost > 1.0F ? 0.0F : edge_cost; } ); @@ -79,6 +80,9 @@ class TMaze : public core::ISerializable { */ const std::list& get_best_route() const; + /** + * @brief Compute the maze graph. + */ void compute_graph(); /** @@ -106,6 +110,11 @@ class TMaze : public core::ISerializable { NUM_OF_LAYERS = 2, }; + /** + * @brief Compute the minumum cost from the start to the end considering only discoverd cells. + */ + void compute_minimum_cost(); + /** * @brief Update the cell costs at the given position. * @@ -118,9 +127,9 @@ class TMaze : public core::ISerializable { * * @param pose The current pose of the robot. * @param discover Whether the robot is discovering new cells. - * @return The next discovery goal for the robot. + * @return A pair containing the next discovery goal for the robot and the total distance. */ - GridPose get_next_bfs_goal(const GridPose& pose, bool discover) const; + std::pair get_next_bfs_goal(const GridPose& pose, bool discover) const; /** * @brief Check if the cell is a dead end. @@ -152,6 +161,9 @@ class TMaze : public core::ISerializable { */ Costmap costmap; + /** + * @brief Graph of the maze for computing the best route. + */ MazeGraph graph; /** diff --git a/micras_nav/include/micras/nav/maze_graph.hpp b/micras_nav/include/micras/nav/maze_graph.hpp index 6bfb714..c2bb4b6 100644 --- a/micras_nav/include/micras/nav/maze_graph.hpp +++ b/micras_nav/include/micras/nav/maze_graph.hpp @@ -26,25 +26,77 @@ class MazeGraph { std::unordered_set prev; }; + /** + * @brief Construct a new MazeGraph object. + * + * @param edge_cost_function Function to compute the cost of an edge. + */ explicit MazeGraph(EdgeCostFunction edge_cost_function = [](const GridPose& from, const GridPose& to) -> float { - return std::abs(from.position.x - to.position.x) + std::abs(from.position.y - to.position.y); + float edge_cost = std::abs(from.position.x - to.position.x) + std::abs(from.position.y - to.position.y); + return edge_cost > 1.0F ? 0.0F : edge_cost; }); + /** + * @brief Check if the graph has a node for a given pose. + * + * @param pose The pose to check. + * @return true if the graph has a node for the given pose, false otherwise. + */ bool has_node(const GridPose& pose) const; + /** + * @brief Add a new node to the graph. + * + * @param pose The grid pose that represents the node. + */ void add_node(const GridPose& pose); + /** + * @brief Add a new edge connecting two poses. + * + * @param from The starting node of the edge. + * @param to The ending node of the edge. + * @param cost The cost of the edge. + */ void add_edge(const GridPose& from, const GridPose& to, float cost = 0.0F); + /** + * @brief Process the nodes in the graph. + * + * @param start The starting node. + */ void process_nodes(const GridPose& start); private: + /** + * @brief Remove unnecessary nodes from the graph. + * + * @param start The starting node. + */ void remove_extra_nodes(const GridPose& start); + /** + * @brief Add extra edges representing diagonals, long curves and long straights to the graph. + * + * @param start The starting node. + */ void add_extra_edges(const GridPose& start); - void dijkstra(const GridPose& start, const GridPose& end); + /** + * @brief Compute the best route on the graph using the dijkstra algorithm. + * + * @param start The starting node. + * @param end The ending node. + */ + void get_best_route(const GridPose& start, const GridPose& end); + /** + * @brief Function to compute the cost of an edge. + * + * @param from The starting node of the edge. + * @param to The ending node of the edge. + * @return The cost of the edge. + */ EdgeCostFunction get_edge_cost; /** diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 71ff65f..0b511fc 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -19,8 +19,8 @@ ActionQueuer::ActionQueuer(Config config) : exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false )}, start{std::make_shared( - ActionType::START, cell_size - config.start_offset, 0.001F * exploring_params.max_linear_acceleration, - exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, + ActionType::START, cell_size - config.start_offset, 0.0F, exploring_params.max_linear_speed, + exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration )}, move_forward{std::make_shared( @@ -29,8 +29,8 @@ ActionQueuer::ActionQueuer(Config config) : exploring_params.max_linear_deceleration )}, move_half{std::make_shared( - ActionType::MOVE_FORWARD, cell_size / 2.0F, 0.001F * exploring_params.max_linear_acceleration, - exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, + ActionType::MOVE_FORWARD, cell_size / 2.0F, 0.0F, exploring_params.max_linear_speed, + exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false )}, turn_left{std::make_shared( diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 36afb11..c2e824f 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -73,7 +73,7 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ template GridPose TMaze::get_next_goal(const GridPose& pose, bool returning) { if (returning and not this->finished_discovery) { - this->compute_best_route(); + this->compute_minimum_cost(); const auto& next_goal = this->get_next_bfs_goal(pose, true); if (next_goal != this->start) { @@ -119,17 +119,8 @@ bool TMaze::finished(const GridPoint& position, bool returning) c } template -void TMaze::compute_best_route() { - GridPose current_pose = this->start; - this->best_route.clear(); - this->best_route.emplace_back(this->start); - - while (not this->goal.contains(current_pose.position)) { - current_pose = this->get_next_bfs_goal(current_pose, false); - this->best_route.emplace_back(current_pose); - } - - this->minimum_cost = this->best_route.size(); +void TMaze::compute_minimum_cost() { + this->minimum_cost = this->get_next_bfs_goal(this->start, false).second; } template @@ -224,12 +215,12 @@ void TMaze::update_cell(const GridPoint& position) { } template -GridPose TMaze::get_next_bfs_goal(const GridPose& pose, bool discover) const { - std::queue queue; - std::array, height> checked{}; - GridPose next_goal = this->start; +std::pair TMaze::get_next_bfs_goal(const GridPose& pose, bool discover) const { + std::queue queue; + std::array, height> distance{}; + std::pair result_pair = {this->start, 0}; - checked.at(pose.position.y).at(pose.position.x) = true; + distance.at(pose.position.y).at(pose.position.x) = 1; auto sides_order = { pose.turned_right().orientation, @@ -240,21 +231,25 @@ GridPose TMaze::get_next_bfs_goal(const GridPose& pose, bool disc for (Side side : sides_order) { if (not this->costmap.has_wall({pose.position, side})) { - queue.emplace(pose.position + side, side); + GridPoint next_position = pose.position + side; + queue.emplace(next_position, side); + distance.at(next_position.y).at(next_position.x) = 1; } } while (not queue.empty()) { GridPose current_pose = queue.front(); queue.pop(); - checked.at(current_pose.position.y).at(current_pose.position.x) = true; if ((discover and this->must_visit( this->costmap.get_cell(current_pose.position), std::round(this->minimum_cost * this->cost_margin) )) or (not discover and this->goal.contains(current_pose.position))) { - next_goal = {pose.position + current_pose.orientation, current_pose.orientation}; + result_pair = { + {pose.position + current_pose.orientation, current_pose.orientation}, + distance.at(current_pose.position.y).at(current_pose.position.x) + }; break; } @@ -268,13 +263,15 @@ GridPose TMaze::get_next_bfs_goal(const GridPose& pose, bool disc const GridPoint front_position = current_pose.position + side; if ((discover or this->was_visited(this->costmap.get_cell(front_position))) and - not checked.at(front_position.y).at(front_position.x)) { + distance.at(front_position.y).at(front_position.x) == 0) { queue.emplace(front_position, current_pose.orientation); + distance.at(front_position.y).at(front_position.x) = + distance.at(current_pose.position.y).at(current_pose.position.x) + 1; } } } - return next_goal; + return result_pair; } template From 12124efc5f938b89bc1cd483f44fc0a5965163d4 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 15 May 2025 00:40:58 -0300 Subject: [PATCH 396/561] =?UTF-8?q?=E2=9C=A8=20Make=20graph=20return=20bes?= =?UTF-8?q?t=20route?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze_graph.hpp | 18 ++++++++++-------- micras_nav/src/maze.cpp | 12 ++++++++++++ micras_nav/src/maze_graph.cpp | 19 ++++++++++++++++--- 3 files changed, 38 insertions(+), 11 deletions(-) diff --git a/micras_nav/include/micras/nav/maze_graph.hpp b/micras_nav/include/micras/nav/maze_graph.hpp index c2bb4b6..1dc00ba 100644 --- a/micras_nav/include/micras/nav/maze_graph.hpp +++ b/micras_nav/include/micras/nav/maze_graph.hpp @@ -5,6 +5,7 @@ #ifndef MICRAS_NAV_MAZE_GRAPH_HPP #define MICRAS_NAV_MAZE_GRAPH_HPP +#include #include #include @@ -67,28 +68,29 @@ class MazeGraph { */ void process_nodes(const GridPose& start); -private: /** - * @brief Remove unnecessary nodes from the graph. + * @brief Compute the best route on the graph using the dijkstra algorithm. * * @param start The starting node. + * @param end The ending node. + * @return A list of poses representing the best route from start to end. */ - void remove_extra_nodes(const GridPose& start); + std::list get_best_route(const GridPose& start, const GridPose& end); +private: /** - * @brief Add extra edges representing diagonals, long curves and long straights to the graph. + * @brief Remove unnecessary nodes from the graph. * * @param start The starting node. */ - void add_extra_edges(const GridPose& start); + void remove_extra_nodes(const GridPose& start); /** - * @brief Compute the best route on the graph using the dijkstra algorithm. + * @brief Add extra edges representing diagonals, long curves and long straights to the graph. * * @param start The starting node. - * @param end The ending node. */ - void get_best_route(const GridPose& start, const GridPose& end); + void add_extra_edges(const GridPose& start); /** * @brief Function to compute the cost of an edge. diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index c2e824f..be3fbb4 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -123,6 +123,12 @@ void TMaze::compute_minimum_cost() { this->minimum_cost = this->get_next_bfs_goal(this->start, false).second; } +template +void TMaze::compute_best_route() { + this->compute_graph(); + this->best_route = this->graph.get_best_route(this->start, this->goal); +} + template const std::list& TMaze::get_best_route() const { return this->best_route; @@ -155,6 +161,10 @@ void TMaze::compute_graph() { GridPose next_pose = {current_pose.position + side, side}; + if (not this->was_visited(this->costmap.get_cell(next_pose.position))) { + continue; + } + if (not this->graph.has_node(next_pose)) { this->graph.add_node(next_pose); queue.push(next_pose); @@ -163,6 +173,8 @@ void TMaze::compute_graph() { this->graph.add_edge(current_pose, next_pose); } } + + this->graph.process_nodes(this->start); } template diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index 85a99fb..843006f 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -119,9 +119,10 @@ void MazeGraph::add_extra_edges(const GridPose& start) { } } -void MazeGraph::dijkstra(const GridPose& start, const GridPose& end) { - std::unordered_map distance; - std::unordered_set visited; +std::list MazeGraph::get_best_route(const GridPose& start, const GridPose& end) { + std::unordered_map distance; + std::unordered_map previous; + std::unordered_set visited; auto comp_function = [&distance](const GridPose& first, const GridPose& second) { return distance.at(first) > distance.at(second); @@ -152,10 +153,22 @@ void MazeGraph::dijkstra(const GridPose& start, const GridPose& end) { for (const auto& [next_pose, edge_cost] : this->graph.at(current_pose).next_costs) { if (distance.at(current_pose) > (distance.at(current_pose) + edge_cost)) { + previous[next_pose] = current_pose; distance.at(current_pose) = distance.at(current_pose) + edge_cost; queue.push(next_pose); } } } + + std::list route; + GridPose current_pose = end; + + while (current_pose != start) { + route.push_front(current_pose); + current_pose = previous.at(current_pose); + } + + route.push_front(start); + return route; } } // namespace micras::nav From c5c650be6fea4156380d9beb0bc84c5a6cdf524d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 15 May 2025 22:46:36 -0300 Subject: [PATCH 397/561] =?UTF-8?q?=F0=9F=9A=B8=20Split=20maze=20construct?= =?UTF-8?q?or?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 22 +++++++++++++--------- micras_nav/src/maze.cpp | 8 ++++++-- 2 files changed, 19 insertions(+), 11 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 7b51ea0..ddcb22d 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -32,15 +32,19 @@ class TMaze : public core::ISerializable { }; /** - * @brief Construct a new Maze object. - */ - explicit TMaze( - Config config, - EdgeCostFunction edge_cost_function = [](const GridPose& from, const GridPose& to) -> float { - float edge_cost = std::abs(from.position.x - to.position.x) + std::abs(from.position.y - to.position.y); - return edge_cost > 1.0F ? 0.0F : edge_cost; - } - ); + * @brief Construct a new TMaze object. + * + * @param config The configuration for the maze. + * @param edge_cost_function The function to compute the cost of the edges in the maze. + */ + TMaze(Config config, EdgeCostFunction edge_cost_function); + + /** + * @brief Construct a new TMaze object. + * + * @param config The configuration for the maze. + */ + explicit TMaze(Config config); /** * @brief Update the maze walls with the current pose and new information. diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index be3fbb4..d4b679a 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -13,8 +13,12 @@ namespace micras::nav { template -TMaze::TMaze(Config config, EdgeCostFunction edge_cost_function) : - graph{std::move(edge_cost_function)}, start{config.start}, goal{config.goal}, cost_margin{config.cost_margin} { +TMaze::TMaze(Config config, EdgeCostFunction edge_cost_function) : TMaze(config) { + this->graph.set_edge_cost_function(edge_cost_function); +} + +template +TMaze::TMaze(Config config) : start{config.start}, goal{config.goal}, cost_margin{config.cost_margin} { this->costmap.update_wall(this->start, false); this->costmap.update_wall(start.turned_right(), true); From 1daeafbec8b6bef933cf4e48d62c3a07b1f37164 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 17 May 2025 19:16:17 -0300 Subject: [PATCH 398/561] =?UTF-8?q?=E2=9C=A8=20Add=20route=20optimizations?= =?UTF-8?q?=20to=20action=20queuer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi --- .../include/micras/nav/action_queuer.hpp | 13 +++++ .../include/micras/nav/actions/base.hpp | 22 ++++++++ .../include/micras/nav/actions/move.hpp | 20 +++++-- .../include/micras/nav/actions/turn.hpp | 25 +++++++++ micras_nav/include/micras/nav/maze_graph.hpp | 4 +- micras_nav/src/action_queuer.cpp | 54 +++++++++++++++++-- micras_nav/src/maze.cpp | 4 +- micras_nav/src/maze_graph.cpp | 14 ++--- 8 files changed, 137 insertions(+), 19 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 2f02678..264148c 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -99,11 +99,24 @@ class ActionQueuer { float get_total_time() const; private: + /** + * @brief Join curves in the action queue. + * + * @param turn_it Iterator to the first of the turn actions. + * @return Iterator to the merged turn action. + */ + std::deque>::iterator join_curves(std::deque>::iterator turn_it); + /** * @brief Size of the cells in the grid. */ float cell_size; + /** + * @brief Distance of the robot to the on on the start cell. + */ + float start_offset; + /** * @brief Dynamic exploring parameters. */ diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 6392acd..9b636d5 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -53,6 +53,28 @@ class Action { */ virtual float get_total_time() const = 0; + /** + * @brief Increment the action ID value. + * + * @param value_increment The increment value. + * @return A reference to the current action. + */ + virtual Action& operator+=(float value_increment) { + this->id.value += value_increment; + return *this; + } + + /** + * @brief Decrement the action ID value. + * + * @param value_decrement The decrement value. + * @return A reference to the current action. + */ + virtual Action& operator-=(float value_decrement) { + this->id.value -= value_decrement; + return *this; + } + /** * @brief Get the ID of the action. * diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 371068c..33f6997 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -96,12 +96,14 @@ class MoveAction : public Action { float get_total_time() const override { return this->total_time; } /** - * @brief Reduce the distance to move and recalculate the total time. + * @brief Increment the distance to move by a certain value. * - * @param reduction Amount to reduce the distance in meters. + * @param value_increment The increment value. + * @return A reference to the current action. */ - void trim_distance(float reduction) { - this->distance -= reduction; + MoveAction& operator+=(float value_increment) override { + Action::operator+=(value_increment); + this->distance += value_increment; this->decelerate_distance = (this->end_speed_2 - this->start_speed_2 + this->max_deceleration_doubled * this->distance) / @@ -111,8 +113,18 @@ class MoveAction : public Action { this->distance, std::sqrt(this->start_speed_2), std::sqrt(this->end_speed_2), this->max_speed, this->max_acceleration_doubled / 2.0F, this->max_deceleration_doubled / 2.0F ); + + return *this; } + /** + * @brief Decrement the distance to move by a certain value. + * + * @param value_increment The decrement value. + * @return A reference to the current action. + */ + MoveAction& operator-=(float value_increment) override { return *this += -value_increment; } + private: /** * @brief Calculate the total time to complete the action. diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 3c46e61..fa54b89 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -88,6 +88,31 @@ class TurnAction : public Action { */ float get_total_time() const override { return this->total_time; } + /** + * @brief Increment the angle to move by a certain value. + * + * @param value_increment The increment value. + * @return A reference to the current action. + */ + TurnAction& operator+=(float value_increment) override { + Action::operator+=(value_increment); + this->angle += value_increment; + + this->max_angular_speed = + calculate_max_angular_speed(this->angle, this->linear_speed, this->max_angular_speed, this->acceleration); + this->total_time = calculate_total_time(this->angle, this->max_angular_speed, this->acceleration); + + return *this; + } + + /** + * @brief Decrement the angle to move by a certain value. + * + * @param value_increment The decrement value. + * @return A reference to the current action. + */ + TurnAction& operator-=(float value_increment) override { return *this += -value_increment; } + private: /** * @brief Calculate the maximum angular speed for a given curve radius and linear speed. diff --git a/micras_nav/include/micras/nav/maze_graph.hpp b/micras_nav/include/micras/nav/maze_graph.hpp index 1dc00ba..592c5f7 100644 --- a/micras_nav/include/micras/nav/maze_graph.hpp +++ b/micras_nav/include/micras/nav/maze_graph.hpp @@ -72,10 +72,10 @@ class MazeGraph { * @brief Compute the best route on the graph using the dijkstra algorithm. * * @param start The starting node. - * @param end The ending node. + * @param end A set of ending nodes. * @return A list of poses representing the best route from start to end. */ - std::list get_best_route(const GridPose& start, const GridPose& end); + std::list get_best_route(const GridPose& start, const std::unordered_set& end); private: /** diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 0b511fc..68d9b4f 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -3,6 +3,7 @@ */ #include +#include #include #include "micras/nav/action_queuer.hpp" @@ -11,6 +12,7 @@ namespace micras::nav { ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, + start_offset{config.start_offset}, exploring_params{config.exploring}, solving_params{config.solving}, curve_linear_speed{std::sqrt(config.solving.max_centrifugal_acceleration * config.solving.curve_radius)}, @@ -172,11 +174,45 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta return; } - for (auto it = std::next(best_route.begin()); std::next(it) != best_route.end(); it++) { - this->push_solving(*it, *std::next(it)); - uint16_t last_index = this->action_queue.size() - 1; + for (auto route_it = std::next(best_route.begin()); std::next(route_it) != best_route.end(); route_it++) { + this->push_solving(*route_it, *std::next(route_it)); + } + + float start_distance = this->start_offset; + + if (this->action_queue.front()->get_id().type == ActionType::MOVE_FORWARD) { + start_distance += this->action_queue.front()->get_id().value; + this->action_queue.pop_front(); + } + + this->action_queue.emplace_front(std::make_shared( + ActionType::MOVE_FORWARD, start_distance, 0.0F, this->curve_linear_speed, this->solving_params.max_linear_speed, + this->solving_params.max_linear_acceleration, this->solving_params.max_linear_deceleration + )); + this->action_queue.emplace_back(stop); + + for (auto action_it = this->action_queue.begin(); action_it != this->action_queue.end();) { + if ((*action_it)->get_id().type != ActionType::DIAGONAL) { + auto before_diagonal_it = std::prev(action_it, 2); + + if ((*before_diagonal_it)->get_id().type == ActionType::TURN) { + action_it = std::next(this->join_curves(before_diagonal_it)); + } - if (this->action_queue.back()->get_id().type == ActionType::DIAGONAL) { } + auto last_straight = *std::prev(action_it, 2); + (*last_straight) -= std::tan(std::numbers::pi_v / 8.0F) * this->cell_size; + + auto after_diagonal_it = std::next(action_it, 2); + + if ((*after_diagonal_it)->get_id().type == ActionType::TURN) { + action_it = std::prev(this->join_curves(std::prev(after_diagonal_it))); + } + + auto next_straight = *std::next(action_it, 2); + (*next_straight) -= std::tan(std::numbers::pi_v / 8.0F) * this->cell_size; + } + + action_it++; } } @@ -189,4 +225,14 @@ float ActionQueuer::get_total_time() const { return total_time; } + +std::deque>::iterator + ActionQueuer::join_curves(std::deque>::iterator turn_it) { + const float turn_angle = (*turn_it)->get_id().value; + + turn_it = this->action_queue.erase(turn_it); + (**turn_it) += turn_angle; + + return turn_it; +} } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index d4b679a..e17f131 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -14,7 +14,7 @@ namespace micras::nav { template TMaze::TMaze(Config config, EdgeCostFunction edge_cost_function) : TMaze(config) { - this->graph.set_edge_cost_function(edge_cost_function); + this->graph = MazeGraph{std::move(edge_cost_function)}; } template @@ -78,7 +78,7 @@ template GridPose TMaze::get_next_goal(const GridPose& pose, bool returning) { if (returning and not this->finished_discovery) { this->compute_minimum_cost(); - const auto& next_goal = this->get_next_bfs_goal(pose, true); + const auto& [next_goal, _] = this->get_next_bfs_goal(pose, true); if (next_goal != this->start) { return next_goal; diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index 843006f..b94f757 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include "micras/nav/maze_graph.hpp" @@ -119,7 +120,7 @@ void MazeGraph::add_extra_edges(const GridPose& start) { } } -std::list MazeGraph::get_best_route(const GridPose& start, const GridPose& end) { +std::list MazeGraph::get_best_route(const GridPose& start, const std::unordered_set& end) { std::unordered_map distance; std::unordered_map previous; std::unordered_set visited; @@ -136,39 +137,38 @@ std::list MazeGraph::get_best_route(const GridPose& start, const GridP distance[start] = 0; queue.push(start); + GridPose current_pose = start; while (!queue.empty()) { - const GridPose current_pose = queue.top(); queue.pop(); + current_pose = queue.top(); if (visited.contains(current_pose)) { continue; } - if (current_pose == end) { + if (end.contains(current_pose.position)) { break; } visited.insert(current_pose); for (const auto& [next_pose, edge_cost] : this->graph.at(current_pose).next_costs) { - if (distance.at(current_pose) > (distance.at(current_pose) + edge_cost)) { + if (distance.at(next_pose) > distance.at(current_pose) + edge_cost) { previous[next_pose] = current_pose; - distance.at(current_pose) = distance.at(current_pose) + edge_cost; + distance[next_pose] = distance.at(current_pose) + edge_cost; queue.push(next_pose); } } } std::list route; - GridPose current_pose = end; while (current_pose != start) { route.push_front(current_pose); current_pose = previous.at(current_pose); } - route.push_front(start); return route; } } // namespace micras::nav From 40f27887ede3964aee4977747e6d2dc24a36b0ae Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 12:28:54 -0300 Subject: [PATCH 399/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20trim=20distance=20?= =?UTF-8?q?on=20diagonals?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 22 ++++++++++++++----- micras_nav/src/action_queuer.cpp | 17 ++++++++++---- 2 files changed, 30 insertions(+), 9 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 264148c..dd542df 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -99,6 +99,13 @@ class ActionQueuer { float get_total_time() const; private: + /** + * @brief Trim the edge of a straight action. + * + * @param straight_action Straight action to trim. + */ + void trim_straight(Action& straight_action) const; + /** * @brief Join curves in the action queue. * @@ -117,6 +124,16 @@ class ActionQueuer { */ float start_offset; + /** + * @brief Solving linear speed of the robot when performing a curve. + */ + float curve_linear_speed; + + /** + * @brief Distance to trim from the straight actions. + */ + float straight_trim_distance; + /** * @brief Dynamic exploring parameters. */ @@ -127,11 +144,6 @@ class ActionQueuer { */ Config::Dynamic solving_params; - /** - * @brief Solving linear speed of the robot when performing a curve. - */ - float curve_linear_speed; - /** * @brief Pre-built actions to use in the exploration. */ diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 68d9b4f..f4a8c43 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -13,9 +13,10 @@ namespace micras::nav { ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, start_offset{config.start_offset}, + curve_linear_speed{std::sqrt(config.solving.max_centrifugal_acceleration * config.solving.curve_radius)}, + straight_trim_distance{std::tan(std::numbers::pi_v / 8.0F) * cell_size / 2.0F}, exploring_params{config.exploring}, solving_params{config.solving}, - curve_linear_speed{std::sqrt(config.solving.max_centrifugal_acceleration * config.solving.curve_radius)}, stop{std::make_shared( ActionType::STOP, cell_size / 2.0F, exploring_params.max_linear_speed, 0.0F, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false @@ -133,7 +134,7 @@ void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& tar float distance = origin_pose.position.to_vector(this->cell_size).distance(target_position.to_vector(this->cell_size)) - - std::tan(std::numbers::pi_v / 8.0F) * this->cell_size; + 2.0F * this->straight_trim_distance; if (not keep_direction) { turn_angle *= -1.0F; @@ -200,7 +201,7 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta } auto last_straight = *std::prev(action_it, 2); - (*last_straight) -= std::tan(std::numbers::pi_v / 8.0F) * this->cell_size; + this->trim_straight(*last_straight); auto after_diagonal_it = std::next(action_it, 2); @@ -209,7 +210,7 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta } auto next_straight = *std::next(action_it, 2); - (*next_straight) -= std::tan(std::numbers::pi_v / 8.0F) * this->cell_size; + this->trim_straight(*next_straight); } action_it++; @@ -226,6 +227,14 @@ float ActionQueuer::get_total_time() const { return total_time; } +void ActionQueuer::trim_straight(Action& straight_action) const { + if (straight_action.get_id().type == ActionType::DIAGONAL) { + straight_action -= this->cell_size / 2.0F - this->straight_trim_distance; + } else { + straight_action -= this->straight_trim_distance; + } +} + std::deque>::iterator ActionQueuer::join_curves(std::deque>::iterator turn_it) { const float turn_angle = (*turn_it)->get_id().value; From bf0ce3834d4138101c019d99d0199182ef11504c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 17:50:31 -0300 Subject: [PATCH 400/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20turn=20action=20or?= =?UTF-8?q?ientation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/turn.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index fa54b89..6cd4491 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -52,17 +52,18 @@ class TurnAction : public Action { * @details The desired velocity is calculated from the angular displacement based on the Torricelli equation. */ Twist get_speeds(const Pose& pose) const override { - Twist twist{}; + Twist twist{}; + const float current_orientation = std::abs(pose.orientation); - if (pose.orientation < std::abs(this->angle)) { + if (current_orientation < std::abs(this->angle)) { twist = { .linear = linear_speed, - .angular = std::sqrt(2.0F * this->acceleration * pose.orientation) + this->angular_speed_step, + .angular = std::sqrt(2.0F * this->acceleration * current_orientation) + this->angular_speed_step, }; } else { twist = { .linear = linear_speed, - .angular = std::sqrt(2.0F * this->acceleration * (std::abs(this->angle) - pose.orientation)) - + .angular = std::sqrt(2.0F * this->acceleration * (std::abs(this->angle) - current_orientation)) - this->angular_speed_step, }; } From c1df2b2416d7f1be9ca7914da1507924366d8e48 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 19:28:43 -0300 Subject: [PATCH 401/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20deceleration=20ang?= =?UTF-8?q?le=20on=20turn?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/turn.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 6cd4491..4dfc4e1 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -55,7 +55,7 @@ class TurnAction : public Action { Twist twist{}; const float current_orientation = std::abs(pose.orientation); - if (current_orientation < std::abs(this->angle)) { + if (current_orientation < std::abs(this->angle) / 2.0F) { twist = { .linear = linear_speed, .angular = std::sqrt(2.0F * this->acceleration * current_orientation) + this->angular_speed_step, From a39e0775e68c14c91f5797bf54254882cc5c43ad Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 19:54:58 -0300 Subject: [PATCH 402/561] =?UTF-8?q?=F0=9F=90=9B=20Fiz=20erase=20order=20on?= =?UTF-8?q?=20maze=20graph?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze_graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index b94f757..1df0e1c 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -61,8 +61,8 @@ void MazeGraph::remove_extra_nodes(const GridPose& start) { this->add_edge(current_pose, next_next); if (next_node.prev.empty()) { - this->graph.erase(next_node.pose); this->graph.at(next_next).prev.erase(next_node.pose); + this->graph.erase(next_node.pose); } next_it = current_node.next_costs.begin(); From 3fbc5944919f288b4311dfac0330144a9142cd04 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 20:02:19 -0300 Subject: [PATCH 403/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20action=20queuer=20?= =?UTF-8?q?diagonal=20check?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index f4a8c43..e02cafe 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -193,7 +193,7 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta this->action_queue.emplace_back(stop); for (auto action_it = this->action_queue.begin(); action_it != this->action_queue.end();) { - if ((*action_it)->get_id().type != ActionType::DIAGONAL) { + if ((*action_it)->get_id().type == ActionType::DIAGONAL) { auto before_diagonal_it = std::prev(action_it, 2); if ((*before_diagonal_it)->get_id().type == ActionType::TURN) { From a3741b7b7f90a42202d2ff07c96280d3f9692d7b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 20:27:34 -0300 Subject: [PATCH 404/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20start=20action=20w?= =?UTF-8?q?hen=20solving?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 8 ++++---- micras_nav/src/maze_graph.cpp | 2 ++ 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index e02cafe..5ca5960 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -175,19 +175,19 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta return; } - for (auto route_it = std::next(best_route.begin()); std::next(route_it) != best_route.end(); route_it++) { + for (auto route_it = best_route.begin(); std::next(route_it) != best_route.end(); route_it++) { this->push_solving(*route_it, *std::next(route_it)); } - float start_distance = this->start_offset; + float start_distance = this->cell_size - this->start_offset; if (this->action_queue.front()->get_id().type == ActionType::MOVE_FORWARD) { - start_distance += this->action_queue.front()->get_id().value; + start_distance += this->action_queue.front()->get_id().value - this->cell_size; this->action_queue.pop_front(); } this->action_queue.emplace_front(std::make_shared( - ActionType::MOVE_FORWARD, start_distance, 0.0F, this->curve_linear_speed, this->solving_params.max_linear_speed, + ActionType::START, start_distance, 0.0F, this->curve_linear_speed, this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, this->solving_params.max_linear_deceleration )); this->action_queue.emplace_back(stop); diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index 1df0e1c..23563e8 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -169,6 +169,8 @@ std::list MazeGraph::get_best_route(const GridPose& start, const std:: current_pose = previous.at(current_pose); } + route.push_front(start); + return route; } } // namespace micras::nav From db09489635f3e27db069303529256fb18f45d41a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 20:47:11 -0300 Subject: [PATCH 405/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20action=20queuer=20?= =?UTF-8?q?pop?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 5ca5960..0bd598d 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -155,7 +155,7 @@ void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& tar } std::shared_ptr ActionQueuer::pop() { - const auto& action = this->action_queue.front(); + auto action = this->action_queue.front(); this->action_queue.pop_front(); return action; } From e2030d407f3d3386e0310c2a5fffe2d2b0037a4e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 22:02:39 -0300 Subject: [PATCH 406/561] =?UTF-8?q?=F0=9F=90=9B=20Remove=20180=20turns=20f?= =?UTF-8?q?rom=20remove=5Fextra=5Fnodes=20function?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 10 ++++++++++ micras_nav/src/maze_graph.cpp | 4 ++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 0bd598d..189c6dc 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -75,6 +75,10 @@ void ActionQueuer::push_exploring(const GridPose& origin_pose, const GridPoint& void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& target_pose) { const Side relative_side = origin_pose.get_relative_side(target_pose.position); + if (origin_pose.position == target_pose.position or relative_side == Side::DOWN) { + return; + } + if (relative_side == Side::UP and origin_pose.orientation == target_pose.orientation) { const float distance = origin_pose.position.to_vector(this->cell_size).distance(target_pose.position.to_vector(this->cell_size)); @@ -124,6 +128,12 @@ void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& tar return; } + if (not keep_direction and + target_pose.orientation != (relative_side == Side::LEFT ? origin_pose.turned_left().orientation : + origin_pose.turned_right().orientation)) { + return; + } + float turn_angle = relative_side == Side::LEFT ? std::numbers::pi_v / 4.0F : -std::numbers::pi_v / 4.0F; diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index 23563e8..7735a70 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -53,8 +53,8 @@ void MazeGraph::remove_extra_nodes(const GridPose& start) { if (next_node.next_costs.size() == 1) { const GridPose& next_next = next_node.next_costs.begin()->first; - if (next_next.orientation == current_pose.orientation or - next_next.orientation == current_pose.turned_back().orientation) { + if (current_pose.orientation == next_node.pose.orientation and + current_pose.orientation == next_next.orientation) { current_node.next_costs.erase(next_it); next_node.prev.erase(current_pose); From 71ac76f82b4097112f33b199136ae8b76643a487 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 22:39:08 -0300 Subject: [PATCH 407/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20dijkstra=20pop=20t?= =?UTF-8?q?op?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze_graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index 7735a70..9a0907d 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -140,8 +140,8 @@ std::list MazeGraph::get_best_route(const GridPose& start, const std:: GridPose current_pose = start; while (!queue.empty()) { - queue.pop(); current_pose = queue.top(); + queue.pop(); if (visited.contains(current_pose)) { continue; From 4ba8f2231ca9a2f6d9f869004b6d7160fc576a75 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 23:29:15 -0300 Subject: [PATCH 408/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20speed=20calculatio?= =?UTF-8?q?n=20on=20turn=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/turn.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 4dfc4e1..4298aee 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -136,10 +136,11 @@ class TurnAction : public Action { } const float radius_speed_ratio = curve_radius / linear_speed; - const float discriminant = std::pow(radius_speed_ratio, 2.0F) - - (2.0F * (1 - std::cos(angle))) / (correction_factor * max_angular_acceleration); + const float quadratic_term = + 1.0F / (std::pow(1 - std::cos(angle), 2.0F) * correction_factor * max_angular_acceleration); + const float discriminant = std::pow(radius_speed_ratio, 2.0F) - 4.0F * quadratic_term; - return correction_factor * max_angular_acceleration * (radius_speed_ratio - std::sqrt(discriminant)); + return (radius_speed_ratio - std::sqrt(discriminant)) / (2.0F * quadratic_term); } static constexpr float calculate_total_time(float angle, float max_angular_speed, float max_angular_acceleration) { From 9d29ece88c05eb0781abec0772fee940e8398ffe Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 18 May 2025 23:45:06 -0300 Subject: [PATCH 409/561] =?UTF-8?q?=F0=9F=90=9B=20Disable=20diagonal=20fol?= =?UTF-8?q?low=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 189c6dc..c0081fa 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -155,7 +155,7 @@ void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& tar this->action_queue.emplace_back(std::make_shared( ActionType::DIAGONAL, distance, this->curve_linear_speed, this->curve_linear_speed, this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, - this->solving_params.max_linear_deceleration + this->solving_params.max_linear_deceleration, false )); this->action_queue.emplace_back(std::make_shared( From 62b79a65443a7d2ac77413f8fd3d3b9f19d28e7f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 00:14:27 -0300 Subject: [PATCH 410/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20correction=20facto?= =?UTF-8?q?r=20on=20turn=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/turn.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 4298aee..bff7897 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -16,7 +16,7 @@ namespace micras::nav { * * @details This factor is determined by analyzing the graph of the maximum angular speed. */ -static constexpr float correction_factor{0.95F}; +static constexpr float correction_factor{1.9F}; /** * @brief Action to turn the robot following a curve radius. From 656d97fe5f0210796a22db6979689ce40c8e1f0d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 02:34:30 -0300 Subject: [PATCH 411/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Improve=20correcti?= =?UTF-8?q?on=20factor=20on=20turn=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/turn.hpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index bff7897..900911c 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -11,13 +11,6 @@ #include "micras/nav/actions/base.hpp" namespace micras::nav { -/** - * @brief Correction factor to adjust the maximum angular speed. - * - * @details This factor is determined by analyzing the graph of the maximum angular speed. - */ -static constexpr float correction_factor{1.9F}; - /** * @brief Action to turn the robot following a curve radius. */ @@ -134,7 +127,8 @@ class TurnAction : public Action { if (linear_speed == 0.0F) { return max_angular_acceleration * 0.01F; } - + const float correction_factor = + 14.35F - 13.57F * std::cos(std::abs(angle) - 2) - 10.0F / max_angular_acceleration; const float radius_speed_ratio = curve_radius / linear_speed; const float quadratic_term = 1.0F / (std::pow(1 - std::cos(angle), 2.0F) * correction_factor * max_angular_acceleration); From 839ee9a9da301414a510b88ae2636f3977a04a0f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 10:31:15 -0300 Subject: [PATCH 412/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20diagonal=20distanc?= =?UTF-8?q?e=20and=20total=20time=20on=20action=20queuer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index c0081fa..648d331 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -124,7 +124,8 @@ void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& tar const bool keep_direction = (origin_pose.orientation == target_pose.orientation); const GridPoint target_position = (keep_direction ? target_pose : target_pose.turned_back().front()).position; - if (std::abs(target_position.x - origin_pose.position.x) != std::abs(target_position.y - origin_pose.position.y)) { + if (std::abs(target_position.x - origin_pose.position.x) != std::abs(target_position.y - origin_pose.position.y) or + origin_pose.turned_right().get_relative_side(target_position) != Side::LEFT) { return; } @@ -148,8 +149,7 @@ void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& tar if (not keep_direction) { turn_angle *= -1.0F; - } else { - distance += std::numbers::sqrt2_v / 2; + distance += this->cell_size * std::numbers::sqrt2_v / 2; } this->action_queue.emplace_back(std::make_shared( @@ -234,6 +234,10 @@ float ActionQueuer::get_total_time() const { total_time += action->get_total_time(); } + if (this->action_queue.size() == 3 and this->action_queue.at(1)->get_id().type == ActionType::DIAGONAL) { + total_time -= 2.0F * this->straight_trim_distance / this->solving_params.max_linear_speed; + } + return total_time; } From 5fd581d64f5a1e54634140bdea25c2249b68e2b6 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 14:01:15 -0300 Subject: [PATCH 413/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Change=20actions?= =?UTF-8?q?=20to=20be=20time=20based?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/actions/base.hpp | 12 +-- .../include/micras/nav/actions/move.hpp | 92 ++++++++----------- .../include/micras/nav/actions/turn.hpp | 42 +++++---- 3 files changed, 62 insertions(+), 84 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 9b636d5..faa15f6 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -33,18 +33,17 @@ class Action { /** * @brief Get the desired speeds for the robot. * - * @param pose The current pose of the robot. + * @param time_step The time step for the action in seconds. * @return The desired speeds for the robot. */ - virtual Twist get_speeds(const Pose& pose) const = 0; + virtual Twist get_speeds(float time_step) = 0; /** * @brief Check if the action is finished. * - * @param pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - virtual bool finished(const Pose& pose) const = 0; + virtual bool finished() const = 0; /** * @brief Get the total time it takes to perform the action. @@ -89,11 +88,6 @@ class Action { */ bool allow_follow_wall() const { return follow_wall; } - /** - * @brief Time step for the action in seconds. - */ - static constexpr float time_step{0.001F}; - protected: /** * @brief Special member functions declared as default. diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 33f6997..da65bb4 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -32,50 +32,39 @@ class MoveAction : public Action { float max_acceleration, float max_deceleration, bool follow_wall = true ) : Action{{action_type, distance}, follow_wall}, - positive_linear_speed_step{max_acceleration * Action::time_step}, - negative_linear_speed_step{max_deceleration * Action::time_step}, distance(distance), - start_speed_2(start_speed * start_speed), - end_speed_2(end_speed * end_speed), + start_speed(start_speed), + end_speed(end_speed), max_speed{max_speed}, - max_acceleration_doubled{2.0F * max_acceleration}, - max_deceleration_doubled{2.0F * max_deceleration}, - decelerate_distance{ - (end_speed_2 - start_speed_2 + max_deceleration_doubled * distance) / - (max_acceleration_doubled + max_deceleration_doubled) - }, + max_acceleration{max_acceleration}, + max_deceleration{max_deceleration}, total_time{calculate_total_time(distance, start_speed, end_speed, max_speed, max_acceleration, max_deceleration) - } { } + }, + half_time{(end_speed - start_speed + max_deceleration * total_time) / (max_acceleration + max_deceleration)} { } /** * @brief Get the desired speeds for the robot to complete the action. * - * @param pose The current pose of the robot. + * @param time_step The time step for the action in seconds. * @return The desired speeds for the robot to complete the action. - * - * @details The desired velocity is calculated from the linear displacement based on the Torricelli equation. */ - Twist get_speeds(const Pose& pose) const override { - const float current_distance = pose.position.magnitude(); - Twist twist{}; + Twist get_speeds(float time_step) override { + this->elapsed_time += time_step; + Twist twist{}; - if (current_distance < this->decelerate_distance) { + if (this->elapsed_time < this->half_time) { twist = { - .linear = std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance) + - this->positive_linear_speed_step, + .linear = this->start_speed + this->max_acceleration * this->elapsed_time, .angular = 0.0F, }; } else { twist = { - .linear = std::sqrt( - this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance) - ) - - this->negative_linear_speed_step, + .linear = this->end_speed + this->max_deceleration * (this->total_time - this->elapsed_time), .angular = 0.0F, }; } - twist.linear = std::fminf(twist.linear, this->max_speed); + twist.linear = std::clamp(twist.linear, 0.0F, this->max_speed); return twist; } @@ -83,10 +72,9 @@ class MoveAction : public Action { /** * @brief Check if the action is finished. * - * @param pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - bool finished(const Pose& pose) const override { return pose.position.magnitude() >= this->distance; } + bool finished() const override { return this->elapsed_time >= this->total_time; } /** * @brief Get the total time it takes to perform the action. @@ -105,15 +93,14 @@ class MoveAction : public Action { Action::operator+=(value_increment); this->distance += value_increment; - this->decelerate_distance = - (this->end_speed_2 - this->start_speed_2 + this->max_deceleration_doubled * this->distance) / - (this->max_acceleration_doubled + this->max_deceleration_doubled); - this->total_time = calculate_total_time( - this->distance, std::sqrt(this->start_speed_2), std::sqrt(this->end_speed_2), this->max_speed, - this->max_acceleration_doubled / 2.0F, this->max_deceleration_doubled / 2.0F + this->distance, this->start_speed, this->end_speed, this->max_speed, this->max_acceleration, + this->max_deceleration ); + this->half_time = (this->end_speed - this->start_speed + this->max_deceleration * this->total_time) / + (this->max_acceleration + this->max_deceleration); + return *this; } @@ -153,30 +140,20 @@ class MoveAction : public Action { return acceleration_time + deceleration_time + constant_speed_time; } - /** - * @brief Linear speed step in m/s to the next loop when accelerating. - */ - float positive_linear_speed_step{}; - - /** - * @brief Linear speed step in m/s to the next loop when decelerating. - */ - float negative_linear_speed_step{}; - /** * @brief Distance to move in meters. */ float distance; /** - * @brief Initial speed squared. + * @brief Initial linear speed in m/s. */ - float start_speed_2; + float start_speed; /** - * @brief Final speed squared. + * @brief Final linear speed in m/s. */ - float end_speed_2; + float end_speed; /** * @brief Maximum linear speed in m/s. @@ -184,24 +161,29 @@ class MoveAction : public Action { float max_speed; /** - * @brief Maximum linear acceleration multiplied by 2. + * @brief Maximum linear acceleration. */ - float max_acceleration_doubled; + float max_acceleration; /** - * @brief Maximum linear deceleration multiplied by 2. + * @brief Maximum linear deceleration. */ - float max_deceleration_doubled; + float max_deceleration; /** - * @brief Distance from the start where the robot should start to decelerate in meters. + * @brief Total time to complete the action in seconds. */ - float decelerate_distance; + float total_time; /** - * @brief Total time to complete the action in seconds. + * @brief Threshold time to start the deceleration phase. */ - float total_time; + float half_time{}; + + /** + * @brief Elapsed time in seconds since the action started. + */ + float elapsed_time{}; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 900911c..073c393 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -29,7 +29,6 @@ class TurnAction : public Action { uint8_t action_type, float angle, float curve_radius, float linear_speed, float max_angular_acceleration ) : Action{{action_type, angle}, false}, - angular_speed_step{max_angular_acceleration * Action::time_step}, angle{angle}, linear_speed{linear_speed}, acceleration{max_angular_acceleration}, @@ -39,30 +38,26 @@ class TurnAction : public Action { /** * @brief Get the desired speeds for the robot to complete the action. * - * @param pose Current pose of the robot. + * @param time_step The time step for the action in seconds. * @return The desired speeds for the robot to complete the action. - * - * @details The desired velocity is calculated from the angular displacement based on the Torricelli equation. */ - Twist get_speeds(const Pose& pose) const override { - Twist twist{}; - const float current_orientation = std::abs(pose.orientation); + Twist get_speeds(float time_step) override { + this->elapsed_time += time_step; + Twist twist{}; - if (current_orientation < std::abs(this->angle) / 2.0F) { + if (this->elapsed_time < this->total_time / 2.0F) { twist = { .linear = linear_speed, - .angular = std::sqrt(2.0F * this->acceleration * current_orientation) + this->angular_speed_step, + .angular = this->acceleration * this->elapsed_time, }; } else { twist = { .linear = linear_speed, - .angular = std::sqrt(2.0F * this->acceleration * (std::abs(this->angle) - current_orientation)) - - this->angular_speed_step, + .angular = this->acceleration * (this->total_time - this->elapsed_time), }; } - twist.angular = - std::copysign(std::clamp(twist.angular, -this->max_angular_speed, this->max_angular_speed), this->angle); + twist.angular = std::copysign(std::clamp(twist.angular, 0.0F, this->max_angular_speed), this->angle); return twist; } @@ -70,10 +65,9 @@ class TurnAction : public Action { /** * @brief Check if the action is finished. * - * @param pose Current pose of the robot. * @return True if the action is finished, false otherwise. */ - bool finished(const Pose& pose) const override { return std::abs(pose.orientation) >= std::abs(this->angle); } + bool finished() const override { return this->elapsed_time >= this->total_time; } /** * @brief Get the total time it takes to perform the action. @@ -137,17 +131,20 @@ class TurnAction : public Action { return (radius_speed_ratio - std::sqrt(discriminant)) / (2.0F * quadratic_term); } + /** + * @brief Calculate the total time to complete the action. + * + * @param angle Angle to turn in radians. + * @param max_angular_speed Maximum angular speed in rad/s. + * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * @return Total time to complete the action in seconds. + */ static constexpr float calculate_total_time(float angle, float max_angular_speed, float max_angular_acceleration) { const float acceleration_time = max_angular_speed / max_angular_acceleration; return std::abs(angle) / max_angular_speed + acceleration_time; } - /** - * @brief Angular step in radians to the next loop. - */ - float angular_speed_step; - /** * @brief Angle to turn in radians. */ @@ -172,6 +169,11 @@ class TurnAction : public Action { * @brief Total time to complete the action in seconds. */ float total_time; + + /** + * @brief Elapsed time in seconds since the action started. + */ + float elapsed_time{}; }; } // namespace micras::nav From 926d85236ef402fdef95652ee029c7b5ab22246b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 15:54:26 -0300 Subject: [PATCH 414/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Make=20move=20acti?= =?UTF-8?q?on=20use=20current=20robot=20pose?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/actions/base.hpp | 37 ++++++-- .../include/micras/nav/actions/move.hpp | 91 +++++++++---------- .../include/micras/nav/actions/turn.hpp | 31 +++---- 3 files changed, 81 insertions(+), 78 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index faa15f6..5c63bd2 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -22,8 +22,11 @@ class Action { * @brief Constructor for the Action class. * * @param action_id The ID of the action. + * @param follow_wall Whether the robot can follow walls while executing this action. + * @param total_time The total time it takes to perform the action. */ - Action(const Id& action_id, bool follow_wall) : id{action_id}, follow_wall{follow_wall} { } + Action(const Id& action_id, bool follow_wall, float total_time = 0.0F) : + id{action_id}, follow_wall{follow_wall}, total_time{total_time} { } /** * @brief Virtual destructor for the Action class. @@ -33,24 +36,19 @@ class Action { /** * @brief Get the desired speeds for the robot. * + * @param current_pose The current pose of the robot. * @param time_step The time step for the action in seconds. * @return The desired speeds for the robot. */ - virtual Twist get_speeds(float time_step) = 0; + virtual Twist get_speeds(const Pose& current_pose, float time_step) = 0; /** * @brief Check if the action is finished. * + * @param current_pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - virtual bool finished() const = 0; - - /** - * @brief Get the total time it takes to perform the action. - * - * @return The total time of the action in seconds. - */ - virtual float get_total_time() const = 0; + virtual bool finished(const Pose& current_pose) const = 0; /** * @brief Increment the action ID value. @@ -88,6 +86,20 @@ class Action { */ bool allow_follow_wall() const { return follow_wall; } + /** + * @brief Get the total time it takes to perform the action. + * + * @return The total time of the action in seconds. + */ + float get_total_time() const { return this->total_time; }; + + /** + * @brief Set the total time it takes to perform the action. + * + * @param total_time The total time of the action in seconds. + */ + void set_total_time(float total_time) { this->total_time = total_time; } + protected: /** * @brief Special member functions declared as default. @@ -109,6 +121,11 @@ class Action { * @brief Whether the robot can follow walls while executing this action. */ bool follow_wall; + + /** + * @brief The total time it takes to perform the action. + */ + float total_time; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index da65bb4..b0c1b4f 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -31,35 +31,45 @@ class MoveAction : public Action { uint8_t action_type, float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, float max_deceleration, bool follow_wall = true ) : - Action{{action_type, distance}, follow_wall}, + Action{ + {action_type, distance}, + follow_wall, + calculate_total_time(distance, start_speed, end_speed, max_speed, max_acceleration, max_deceleration) + }, distance(distance), - start_speed(start_speed), - end_speed(end_speed), + start_speed_2(start_speed * start_speed), + end_speed_2(end_speed * end_speed), max_speed{max_speed}, - max_acceleration{max_acceleration}, - max_deceleration{max_deceleration}, - total_time{calculate_total_time(distance, start_speed, end_speed, max_speed, max_acceleration, max_deceleration) - }, - half_time{(end_speed - start_speed + max_deceleration * total_time) / (max_acceleration + max_deceleration)} { } + max_acceleration_doubled{2.0F * max_acceleration}, + max_deceleration_doubled{2.0F * max_deceleration}, + decelerate_distance{ + (end_speed_2 - start_speed_2 + max_deceleration_doubled * distance) / + (max_acceleration_doubled + max_deceleration_doubled) + } { } /** * @brief Get the desired speeds for the robot to complete the action. * + * @param current_pose The current pose of the robot. * @param time_step The time step for the action in seconds. * @return The desired speeds for the robot to complete the action. + * + * @details The desired velocity is calculated from the linear displacement based on the Torricelli equation. */ - Twist get_speeds(float time_step) override { - this->elapsed_time += time_step; - Twist twist{}; + Twist get_speeds(const Pose& current_pose, float time_step) override { + const float current_distance = current_pose.position.magnitude(); + Twist twist{}; - if (this->elapsed_time < this->half_time) { + if (current_distance < this->decelerate_distance) { twist = { - .linear = this->start_speed + this->max_acceleration * this->elapsed_time, + .linear = std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance) + + time_step * this->max_acceleration_doubled / 2.0F, .angular = 0.0F, }; } else { twist = { - .linear = this->end_speed + this->max_deceleration * (this->total_time - this->elapsed_time), + .linear = + std::sqrt(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), .angular = 0.0F, }; } @@ -72,16 +82,10 @@ class MoveAction : public Action { /** * @brief Check if the action is finished. * + * @param pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - bool finished() const override { return this->elapsed_time >= this->total_time; } - - /** - * @brief Get the total time it takes to perform the action. - * - * @return The total time of the action in seconds. - */ - float get_total_time() const override { return this->total_time; } + bool finished(const Pose& pose) const override { return pose.position.magnitude() >= this->distance; } /** * @brief Increment the distance to move by a certain value. @@ -93,13 +97,14 @@ class MoveAction : public Action { Action::operator+=(value_increment); this->distance += value_increment; - this->total_time = calculate_total_time( - this->distance, this->start_speed, this->end_speed, this->max_speed, this->max_acceleration, - this->max_deceleration - ); + this->set_total_time(calculate_total_time( + this->distance, std::sqrt(this->start_speed_2), std::sqrt(this->end_speed_2), this->max_speed, + this->max_acceleration_doubled / 2.0F, this->max_deceleration_doubled / 2.0F + )); - this->half_time = (this->end_speed - this->start_speed + this->max_deceleration * this->total_time) / - (this->max_acceleration + this->max_deceleration); + this->decelerate_distance = + (this->end_speed_2 - this->start_speed_2 + this->max_deceleration_doubled * this->distance) / + (this->max_acceleration_doubled + this->max_deceleration_doubled); return *this; } @@ -146,14 +151,14 @@ class MoveAction : public Action { float distance; /** - * @brief Initial linear speed in m/s. + * @brief Initial linear speed squared. */ - float start_speed; + float start_speed_2; /** - * @brief Final linear speed in m/s. + * @brief Final linear speed squared. */ - float end_speed; + float end_speed_2; /** * @brief Maximum linear speed in m/s. @@ -161,29 +166,19 @@ class MoveAction : public Action { float max_speed; /** - * @brief Maximum linear acceleration. - */ - float max_acceleration; - - /** - * @brief Maximum linear deceleration. - */ - float max_deceleration; - - /** - * @brief Total time to complete the action in seconds. + * @brief Maximum linear acceleration multiplied by 2. */ - float total_time; + float max_acceleration_doubled; /** - * @brief Threshold time to start the deceleration phase. + * @brief Maximum linear deceleration multiplied by 2. */ - float half_time{}; + float max_deceleration_doubled; /** - * @brief Elapsed time in seconds since the action started. + * @brief Distance from the start where the robot should start to decelerate in meters. */ - float elapsed_time{}; + float decelerate_distance; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 073c393..76bd080 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -32,20 +32,22 @@ class TurnAction : public Action { angle{angle}, linear_speed{linear_speed}, acceleration{max_angular_acceleration}, - max_angular_speed{calculate_max_angular_speed(angle, curve_radius, linear_speed, max_angular_acceleration)}, - total_time{calculate_total_time(angle, max_angular_speed, max_angular_acceleration)} { } + max_angular_speed{calculate_max_angular_speed(angle, curve_radius, linear_speed, max_angular_acceleration)} { + this->set_total_time(calculate_total_time(angle, max_angular_speed, max_angular_acceleration)); + } /** * @brief Get the desired speeds for the robot to complete the action. * + * @param current_pose The current pose of the robot. * @param time_step The time step for the action in seconds. * @return The desired speeds for the robot to complete the action. */ - Twist get_speeds(float time_step) override { + Twist get_speeds(const Pose& /*current_pose*/, float time_step) override { this->elapsed_time += time_step; Twist twist{}; - if (this->elapsed_time < this->total_time / 2.0F) { + if (this->elapsed_time < this->get_total_time() / 2.0F) { twist = { .linear = linear_speed, .angular = this->acceleration * this->elapsed_time, @@ -53,7 +55,7 @@ class TurnAction : public Action { } else { twist = { .linear = linear_speed, - .angular = this->acceleration * (this->total_time - this->elapsed_time), + .angular = this->acceleration * (this->get_total_time() - this->elapsed_time), }; } @@ -65,16 +67,10 @@ class TurnAction : public Action { /** * @brief Check if the action is finished. * + * @param current_pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - bool finished() const override { return this->elapsed_time >= this->total_time; } - - /** - * @brief Get the total time it takes to perform the action. - * - * @return The total time of the action in seconds. - */ - float get_total_time() const override { return this->total_time; } + bool finished(const Pose& /*current_pose*/) const override { return this->elapsed_time >= this->get_total_time(); } /** * @brief Increment the angle to move by a certain value. @@ -85,10 +81,10 @@ class TurnAction : public Action { TurnAction& operator+=(float value_increment) override { Action::operator+=(value_increment); this->angle += value_increment; - this->max_angular_speed = calculate_max_angular_speed(this->angle, this->linear_speed, this->max_angular_speed, this->acceleration); - this->total_time = calculate_total_time(this->angle, this->max_angular_speed, this->acceleration); + + this->set_total_time(calculate_total_time(this->angle, this->max_angular_speed, this->acceleration)); return *this; } @@ -165,11 +161,6 @@ class TurnAction : public Action { */ float max_angular_speed; - /** - * @brief Total time to complete the action in seconds. - */ - float total_time; - /** * @brief Elapsed time in seconds since the action started. */ From 968ffed9eb3402cee2abbc3b743a0af4361d8d56 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 16:00:15 -0300 Subject: [PATCH 415/561] =?UTF-8?q?=F0=9F=90=9B=20Remove=20const=20from=20?= =?UTF-8?q?action=20finished?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/base.hpp | 2 +- micras_nav/include/micras/nav/actions/move.hpp | 2 +- micras_nav/include/micras/nav/actions/turn.hpp | 9 ++++++++- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 5c63bd2..8c5fa56 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -48,7 +48,7 @@ class Action { * @param current_pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - virtual bool finished(const Pose& current_pose) const = 0; + virtual bool finished(const Pose& current_pose) = 0; /** * @brief Increment the action ID value. diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index b0c1b4f..d2aad14 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -85,7 +85,7 @@ class MoveAction : public Action { * @param pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - bool finished(const Pose& pose) const override { return pose.position.magnitude() >= this->distance; } + bool finished(const Pose& pose) override { return pose.position.magnitude() >= this->distance; } /** * @brief Increment the distance to move by a certain value. diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 76bd080..3b99eb3 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -70,7 +70,14 @@ class TurnAction : public Action { * @param current_pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - bool finished(const Pose& /*current_pose*/) const override { return this->elapsed_time >= this->get_total_time(); } + bool finished(const Pose& /*current_pose*/) override { + if (this->elapsed_time >= this->get_total_time()) { + this->elapsed_time = 0.0F; + return true; + } + + return false; + } /** * @brief Increment the angle to move by a certain value. From 3438169f91f04a0ab526049c6b7fe4f0166a517a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 18:20:39 -0300 Subject: [PATCH 416/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20get=5Frelative=5Fs?= =?UTF-8?q?ide=20function?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/grid_pose.cpp | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index 2967e15..3c664f0 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -76,15 +76,32 @@ GridPose GridPose::turned_right() const { } Side GridPose::get_relative_side(const GridPoint& other) const { - const GridPoint front_position = this->front().position; - const float cross_product = (front_position.x - this->position.x) * (other.y - this->position.y) - - (front_position.y - this->position.y) * (other.x - this->position.x); + switch (this->orientation) { + case Side::RIGHT: + if (this->position.y == other.y) { + return this->position.x < other.x ? Side::UP : Side::DOWN; + } - if (cross_product == 0) { - return this->position.direction(other) == this->orientation ? Side::UP : Side::DOWN; - } + return this->position.y < other.y ? Side::LEFT : Side::RIGHT; + case Side::UP: + if (this->position.x == other.x) { + return this->position.y < other.y ? Side::UP : Side::DOWN; + } + + return this->position.x > other.x ? Side::LEFT : Side::RIGHT; + case Side::LEFT: + if (this->position.y == other.y) { + return this->position.x > other.x ? Side::UP : Side::DOWN; + } + + return this->position.y > other.y ? Side::LEFT : Side::RIGHT; + case Side::DOWN: + if (this->position.x == other.x) { + return this->position.y > other.y ? Side::UP : Side::DOWN; + } - return cross_product > 0 ? Side::LEFT : Side::RIGHT; + return this->position.x < other.x ? Side::LEFT : Side::RIGHT; + } } bool GridPose::operator==(const GridPose& other) const { From f7511ef576e9502a38294e2d17efdad39887ee41 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 19:01:22 -0300 Subject: [PATCH 417/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20second=20turn=20an?= =?UTF-8?q?gle=20on=20diagonal?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 648d331..c5f3091 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -147,8 +147,9 @@ void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& tar origin_pose.position.to_vector(this->cell_size).distance(target_position.to_vector(this->cell_size)) - 2.0F * this->straight_trim_distance; - if (not keep_direction) { + if (keep_direction) { turn_angle *= -1.0F; + } else { distance += this->cell_size * std::numbers::sqrt2_v / 2; } From 22068695f13db73a9b213c539c1e4410ce94b661 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 21:26:34 -0300 Subject: [PATCH 418/561] =?UTF-8?q?=E2=9C=A8=20Fix=20trim=20distances=20be?= =?UTF-8?q?fore=20and=20after=20diagonals?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi --- .../include/micras/nav/action_queuer.hpp | 12 +-- .../include/micras/nav/actions/turn.hpp | 2 +- micras_nav/src/action_queuer.cpp | 81 +++++++++++-------- 3 files changed, 54 insertions(+), 41 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index dd542df..ea45552 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -100,11 +100,12 @@ class ActionQueuer { private: /** - * @brief Trim the edge of a straight action. + * @brief Get the trim distances for a turn action. * - * @param straight_action Straight action to trim. + * @param turn_action Turn action to get the trim distances for. + * @return Pair of trim distances before and after the turn. */ - void trim_straight(Action& straight_action) const; + std::pair get_trim_distances(const Action& turn_action) const; /** * @brief Join curves in the action queue. @@ -129,11 +130,6 @@ class ActionQueuer { */ float curve_linear_speed; - /** - * @brief Distance to trim from the straight actions. - */ - float straight_trim_distance; - /** * @brief Dynamic exploring parameters. */ diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 3b99eb3..431332e 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -104,7 +104,6 @@ class TurnAction : public Action { */ TurnAction& operator-=(float value_increment) override { return *this += -value_increment; } -private: /** * @brief Calculate the maximum angular speed for a given curve radius and linear speed. * @@ -134,6 +133,7 @@ class TurnAction : public Action { return (radius_speed_ratio - std::sqrt(discriminant)) / (2.0F * quadratic_term); } +private: /** * @brief Calculate the total time to complete the action. * diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index c5f3091..f805fc0 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -7,14 +7,12 @@ #include #include "micras/nav/action_queuer.hpp" -#include "micras/nav/actions/move.hpp" namespace micras::nav { ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, start_offset{config.start_offset}, curve_linear_speed{std::sqrt(config.solving.max_centrifugal_acceleration * config.solving.curve_radius)}, - straight_trim_distance{std::tan(std::numbers::pi_v / 8.0F) * cell_size / 2.0F}, exploring_params{config.exploring}, solving_params{config.solving}, stop{std::make_shared( @@ -144,8 +142,7 @@ void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& tar )); float distance = - origin_pose.position.to_vector(this->cell_size).distance(target_position.to_vector(this->cell_size)) - - 2.0F * this->straight_trim_distance; + origin_pose.position.to_vector(this->cell_size).distance(target_position.to_vector(this->cell_size)); if (keep_direction) { turn_angle *= -1.0F; @@ -203,28 +200,22 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta )); this->action_queue.emplace_back(stop); - for (auto action_it = this->action_queue.begin(); action_it != this->action_queue.end();) { - if ((*action_it)->get_id().type == ActionType::DIAGONAL) { - auto before_diagonal_it = std::prev(action_it, 2); - - if ((*before_diagonal_it)->get_id().type == ActionType::TURN) { - action_it = std::next(this->join_curves(before_diagonal_it)); - } - - auto last_straight = *std::prev(action_it, 2); - this->trim_straight(*last_straight); - - auto after_diagonal_it = std::next(action_it, 2); - - if ((*after_diagonal_it)->get_id().type == ActionType::TURN) { - action_it = std::prev(this->join_curves(std::prev(after_diagonal_it))); - } + for (auto action_it = this->action_queue.begin(); action_it != this->action_queue.end(); action_it++) { + if ((*action_it)->get_id().type != ActionType::TURN or + std::abs((*action_it)->get_id().value) != std::numbers::pi_v / 4.0F) { + continue; + } - auto next_straight = *std::next(action_it, 2); - this->trim_straight(*next_straight); + if ((*std::prev(action_it))->get_id().type == ActionType::TURN) { + action_it = this->join_curves(std::prev(action_it)); + } else if ((*std::next(action_it))->get_id().type == ActionType::TURN) { + action_it = this->join_curves(action_it); } - action_it++; + auto [trim_before_distance, trim_after_distance] = this->get_trim_distances(**action_it); + + **std::prev(action_it) -= trim_before_distance; + **std::next(action_it) -= trim_after_distance; } } @@ -235,19 +226,45 @@ float ActionQueuer::get_total_time() const { total_time += action->get_total_time(); } - if (this->action_queue.size() == 3 and this->action_queue.at(1)->get_id().type == ActionType::DIAGONAL) { - total_time -= 2.0F * this->straight_trim_distance / this->solving_params.max_linear_speed; - } - return total_time; } -void ActionQueuer::trim_straight(Action& straight_action) const { - if (straight_action.get_id().type == ActionType::DIAGONAL) { - straight_action -= this->cell_size / 2.0F - this->straight_trim_distance; - } else { - straight_action -= this->straight_trim_distance; +std::pair ActionQueuer::get_trim_distances(const Action& turn_action) const { + const float turn_angle = std::abs(turn_action.get_id().value); + const float side_displacement = (1.0F - std::cos(turn_angle)) * this->cell_size / (2.0F * this->curve_linear_speed); + + const float correction_factor = 14.12F - 13.3F * std::cos(turn_angle - 1.145F); + const float max_angular_speed = TurnAction::calculate_max_angular_speed( + turn_angle, this->cell_size / 2.0F, this->curve_linear_speed, this->solving_params.max_angular_acceleration + ); + const float forward_displacement = + std::sin(turn_angle) / max_angular_speed + + max_angular_speed / (std::sin(turn_angle) * correction_factor * this->solving_params.max_angular_acceleration); + + if (turn_angle == std::numbers::pi_v / 4.0F) { + const float trim_before_distance = forward_displacement - side_displacement; + const float trim_after_distance = side_displacement * std::numbers::sqrt2_v; + + return {trim_before_distance, trim_after_distance}; + } + + if (turn_angle == std::numbers::pi_v / 2.0F) { + const float trim_distance = side_displacement; + + return {trim_distance, trim_distance}; } + + if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { + const float trim_before_distance = + forward_displacement + side_displacement - (this->cell_size * std::numbers::sqrt2_v / 2.0F); + const float trim_after_distance = + (side_displacement - (this->cell_size * std::numbers::sqrt2_v / 2.0F)) * + std::numbers::sqrt2_v; + + return {trim_before_distance, trim_after_distance}; + } + + return {0.0F, 0.0F}; } std::deque>::iterator From 5948932ba21d81ff852aa67a94a865c90aed9ca1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 22:02:04 -0300 Subject: [PATCH 419/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20side=20displacemen?= =?UTF-8?q?t=20on=20action=20queuer=20trim=20distance?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index f805fc0..9ef427e 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -231,7 +231,7 @@ float ActionQueuer::get_total_time() const { std::pair ActionQueuer::get_trim_distances(const Action& turn_action) const { const float turn_angle = std::abs(turn_action.get_id().value); - const float side_displacement = (1.0F - std::cos(turn_angle)) * this->cell_size / (2.0F * this->curve_linear_speed); + const float side_displacement = (1.0F - std::cos(turn_angle)) * this->cell_size / 2.0F; const float correction_factor = 14.12F - 13.3F * std::cos(turn_angle - 1.145F); const float max_angular_speed = TurnAction::calculate_max_angular_speed( @@ -257,9 +257,7 @@ std::pair ActionQueuer::get_trim_distances(const Action& turn_acti if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { const float trim_before_distance = forward_displacement + side_displacement - (this->cell_size * std::numbers::sqrt2_v / 2.0F); - const float trim_after_distance = - (side_displacement - (this->cell_size * std::numbers::sqrt2_v / 2.0F)) * - std::numbers::sqrt2_v; + const float trim_after_distance = std::numbers::sqrt2_v * side_displacement - this->cell_size; return {trim_before_distance, trim_after_distance}; } From b24edcd14e0c43624ff0279768417f3c1d729ffe Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 19 May 2025 22:28:34 -0300 Subject: [PATCH 420/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20forward=20displace?= =?UTF-8?q?ment=20from=20turn=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 9ef427e..1457630 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -238,8 +238,9 @@ std::pair ActionQueuer::get_trim_distances(const Action& turn_acti turn_angle, this->cell_size / 2.0F, this->curve_linear_speed, this->solving_params.max_angular_acceleration ); const float forward_displacement = - std::sin(turn_angle) / max_angular_speed + - max_angular_speed / (std::sin(turn_angle) * correction_factor * this->solving_params.max_angular_acceleration); + this->curve_linear_speed * (std::sin(turn_angle) / max_angular_speed + + max_angular_speed / (std::sin(turn_angle) * correction_factor * + this->solving_params.max_angular_acceleration)); if (turn_angle == std::numbers::pi_v / 4.0F) { const float trim_before_distance = forward_displacement - side_displacement; From 21eb37e18b64a2fad0ace537bd8138cf9b642823 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 20 May 2025 22:51:25 -0300 Subject: [PATCH 421/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20action=20queuer=20?= =?UTF-8?q?get=5Ftrim=5Fdistances?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 3 +- micras_nav/src/action_queuer.cpp | 32 +++++++++++++------ 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index ea45552..8c7fbed 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -102,10 +102,11 @@ class ActionQueuer { /** * @brief Get the trim distances for a turn action. * + * @param action_before Id of the action before the turn action. * @param turn_action Turn action to get the trim distances for. * @return Pair of trim distances before and after the turn. */ - std::pair get_trim_distances(const Action& turn_action) const; + std::pair get_trim_distances(const Action::Id& action_before, const Action& turn_action) const; /** * @brief Join curves in the action queue. diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 1457630..2ab942d 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -201,8 +201,7 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta this->action_queue.emplace_back(stop); for (auto action_it = this->action_queue.begin(); action_it != this->action_queue.end(); action_it++) { - if ((*action_it)->get_id().type != ActionType::TURN or - std::abs((*action_it)->get_id().value) != std::numbers::pi_v / 4.0F) { + if ((*action_it)->get_id().type != ActionType::TURN) { continue; } @@ -212,7 +211,8 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta action_it = this->join_curves(action_it); } - auto [trim_before_distance, trim_after_distance] = this->get_trim_distances(**action_it); + auto [trim_before_distance, trim_after_distance] = + this->get_trim_distances((*std::prev(action_it))->get_id(), **action_it); **std::prev(action_it) -= trim_before_distance; **std::next(action_it) -= trim_after_distance; @@ -229,7 +229,8 @@ float ActionQueuer::get_total_time() const { return total_time; } -std::pair ActionQueuer::get_trim_distances(const Action& turn_action) const { +std::pair + ActionQueuer::get_trim_distances(const Action::Id& action_before, const Action& turn_action) const { const float turn_angle = std::abs(turn_action.get_id().value); const float side_displacement = (1.0F - std::cos(turn_angle)) * this->cell_size / 2.0F; @@ -250,15 +251,28 @@ std::pair ActionQueuer::get_trim_distances(const Action& turn_acti } if (turn_angle == std::numbers::pi_v / 2.0F) { - const float trim_distance = side_displacement; + float trim_before_distance = forward_displacement; + float trim_after_distance = side_displacement; - return {trim_distance, trim_distance}; + if (action_before.type == ActionType::MOVE_FORWARD) { + trim_before_distance -= this->cell_size / 2.0F; + trim_after_distance -= this->cell_size / 2.0F; + } + + return {trim_before_distance, trim_after_distance}; } if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { - const float trim_before_distance = - forward_displacement + side_displacement - (this->cell_size * std::numbers::sqrt2_v / 2.0F); - const float trim_after_distance = std::numbers::sqrt2_v * side_displacement - this->cell_size; + float trim_before_distance = forward_displacement + side_displacement; + float trim_after_distance = std::numbers::sqrt2_v * side_displacement; + + if (action_before.type == ActionType::DIAGONAL) { + trim_before_distance -= this->cell_size * std::numbers::sqrt2_v / 2.0F; + trim_after_distance -= this->cell_size; + } else { + trim_before_distance -= this->cell_size; + trim_after_distance -= this->cell_size * std::numbers::sqrt2_v / 2.0F; + } return {trim_before_distance, trim_after_distance}; } From 2af20c9b0a5ffa4f8c050f166e0b04211da5f95f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 20 May 2025 23:50:58 -0300 Subject: [PATCH 422/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20turn=20max=5Fangul?= =?UTF-8?q?ar=5Fspeed=20calculation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/turn.hpp | 10 ++++++++-- micras_nav/src/action_queuer.cpp | 2 +- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 431332e..d352a5e 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -32,7 +32,8 @@ class TurnAction : public Action { angle{angle}, linear_speed{linear_speed}, acceleration{max_angular_acceleration}, - max_angular_speed{calculate_max_angular_speed(angle, curve_radius, linear_speed, max_angular_acceleration)} { + max_angular_speed{calculate_max_angular_speed(angle, curve_radius, linear_speed, max_angular_acceleration)}, + curve_radius{curve_radius} { this->set_total_time(calculate_total_time(angle, max_angular_speed, max_angular_acceleration)); } @@ -89,7 +90,7 @@ class TurnAction : public Action { Action::operator+=(value_increment); this->angle += value_increment; this->max_angular_speed = - calculate_max_angular_speed(this->angle, this->linear_speed, this->max_angular_speed, this->acceleration); + calculate_max_angular_speed(this->angle, this->curve_radius, this->linear_speed, this->acceleration); this->set_total_time(calculate_total_time(this->angle, this->max_angular_speed, this->acceleration)); @@ -168,6 +169,11 @@ class TurnAction : public Action { */ float max_angular_speed; + /** + * @brief Radius of the equivalent curve in meters. + */ + float curve_radius; + /** * @brief Elapsed time in seconds since the action started. */ diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 2ab942d..239e8e4 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -254,7 +254,7 @@ std::pair float trim_before_distance = forward_displacement; float trim_after_distance = side_displacement; - if (action_before.type == ActionType::MOVE_FORWARD) { + if (action_before.type != ActionType::DIAGONAL) { trim_before_distance -= this->cell_size / 2.0F; trim_after_distance -= this->cell_size / 2.0F; } From 914a5d47748c82f0256c5420b02c95c969ce4265 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 21 May 2025 01:21:25 -0300 Subject: [PATCH 423/561] =?UTF-8?q?=F0=9F=A5=85=20Add=20reset=20function?= =?UTF-8?q?=20to=20maze=5Fgraph?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze_graph.hpp | 5 +++++ micras_nav/src/maze.cpp | 2 ++ micras_nav/src/maze_graph.cpp | 4 ++++ 3 files changed, 11 insertions(+) diff --git a/micras_nav/include/micras/nav/maze_graph.hpp b/micras_nav/include/micras/nav/maze_graph.hpp index 592c5f7..30447c2 100644 --- a/micras_nav/include/micras/nav/maze_graph.hpp +++ b/micras_nav/include/micras/nav/maze_graph.hpp @@ -77,6 +77,11 @@ class MazeGraph { */ std::list get_best_route(const GridPose& start, const std::unordered_set& end); + /** + * @brief Clear the contents of the graph. + */ + void reset(); + private: /** * @brief Remove unnecessary nodes from the graph. diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index e17f131..1cdfa12 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -141,6 +141,8 @@ const std::list& TMaze::get_best_route() const { template void TMaze::compute_graph() { std::queue queue; + this->graph.reset(); + this->graph.add_node(this->start); queue.push(this->start); diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index 9a0907d..cc23957 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -173,4 +173,8 @@ std::list MazeGraph::get_best_route(const GridPose& start, const std:: return route; } + +void MazeGraph::reset() { + this->graph.clear(); +} } // namespace micras::nav From 89828ddad29cccd4725bfb1e10ea5afb89271cd8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 21 May 2025 12:12:18 -0300 Subject: [PATCH 424/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20total=20time=20cal?= =?UTF-8?q?culations=20on=20actions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/move.hpp | 10 ++++++++++ micras_nav/include/micras/nav/actions/turn.hpp | 6 ++++++ 2 files changed, 16 insertions(+) diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index d2aad14..631c709 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -133,6 +133,16 @@ class MoveAction : public Action { float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, float max_deceleration ) { + const float peak_velocity = std::sqrt( + (2.0F * distance * max_acceleration * max_deceleration + start_speed * start_speed * max_deceleration + + end_speed * end_speed * max_acceleration) / + (max_acceleration + max_deceleration) + ); + + if (peak_velocity < max_speed) { + return (peak_velocity - start_speed) / max_acceleration + (peak_velocity - end_speed) / max_deceleration; + } + const float acceleration_time = (max_speed - start_speed) / max_acceleration; const float deceleration_time = (max_speed - end_speed) / max_deceleration; diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index d352a5e..ce7ab7b 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -144,6 +144,12 @@ class TurnAction : public Action { * @return Total time to complete the action in seconds. */ static constexpr float calculate_total_time(float angle, float max_angular_speed, float max_angular_acceleration) { + const float peak_velocity = std::sqrt(angle * max_angular_acceleration); + + if (peak_velocity < max_angular_speed) { + return 2.0F * peak_velocity / max_angular_acceleration; + } + const float acceleration_time = max_angular_speed / max_angular_acceleration; return std::abs(angle) / max_angular_speed + acceleration_time; From cf42b7728073cb21aa57cd6264fbabf98196e0b2 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 21 May 2025 21:04:57 -0300 Subject: [PATCH 425/561] =?UTF-8?q?=E2=9C=A8=20Add=20variable=20radius=20t?= =?UTF-8?q?o=20action=20queuer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 67 +++--- .../include/micras/nav/actions/base.hpp | 22 -- .../include/micras/nav/actions/move.hpp | 30 --- .../include/micras/nav/actions/turn.hpp | 109 +++++---- micras_nav/src/action_queuer.cpp | 213 ++++++++++-------- 5 files changed, 209 insertions(+), 232 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 8c7fbed..55cd4d3 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include "micras/nav/actions/move.hpp" #include "micras/nav/actions/turn.hpp" @@ -38,7 +39,6 @@ class ActionQueuer { float max_linear_speed; float max_linear_acceleration; float max_linear_deceleration; - float curve_radius; float max_centrifugal_acceleration; float max_angular_acceleration; }; @@ -47,6 +47,10 @@ class ActionQueuer { float start_offset; Dynamic exploring; Dynamic solving; + + float radius_45; + float radius_90; + float radius_135; }; /** @@ -65,12 +69,13 @@ class ActionQueuer { void push_exploring(const GridPose& origin_pose, const GridPoint& target_position); /** - * @brief Push a solving action to the queue. + * @brief Get the actions to perform from the origin pose to the target pose. * - * @param current_pose Current pose of the robot. - * @param target_angle Target pose to move to.. + * @param origin_pose Origin pose of the robot. + * @param target_pose Target pose to move to. + * @return List of actions to perform. */ - void push_solving(const GridPose& origin_pose, const GridPose& target_pose); + std::list get_actions(const GridPose& origin_pose, const GridPose& target_pose) const; /** * @brief Pop an action from the queue. @@ -99,6 +104,17 @@ class ActionQueuer { float get_total_time() const; private: + /** + * @brief Struct to hold the parameters for a curve. + */ + struct CurveParameters { + float curve_radius; + float max_angular_speed; + float linear_speed; + float side_displacement; + float forward_displacement; + }; + /** * @brief Get the trim distances for a turn action. * @@ -106,15 +122,7 @@ class ActionQueuer { * @param turn_action Turn action to get the trim distances for. * @return Pair of trim distances before and after the turn. */ - std::pair get_trim_distances(const Action::Id& action_before, const Action& turn_action) const; - - /** - * @brief Join curves in the action queue. - * - * @param turn_it Iterator to the first of the turn actions. - * @return Iterator to the merged turn action. - */ - std::deque>::iterator join_curves(std::deque>::iterator turn_it); + std::pair get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) const; /** * @brief Size of the cells in the grid. @@ -126,11 +134,6 @@ class ActionQueuer { */ float start_offset; - /** - * @brief Solving linear speed of the robot when performing a curve. - */ - float curve_linear_speed; - /** * @brief Dynamic exploring parameters. */ @@ -141,6 +144,16 @@ class ActionQueuer { */ Config::Dynamic solving_params; + /** + * @brief Radius of the exploration curve. + */ + float exploration_curve_radius; + + /** + * @brief Pre-computed curve parameters for different angles. + */ + std::unordered_map curves_parameters; + /** * @brief Pre-built actions to use in the exploration. */ @@ -149,6 +162,7 @@ class ActionQueuer { std::shared_ptr start; std::shared_ptr move_forward; std::shared_ptr move_half; + std::shared_ptr move_to_turn; std::shared_ptr turn_left; std::shared_ptr turn_right; std::shared_ptr turn_back; @@ -159,21 +173,6 @@ class ActionQueuer { */ std::deque> action_queue; }; - -class ActionCost { -public: - explicit ActionCost(const ActionQueuer::Config& config) : action_queuer(config) { } - - float operator()(const GridPose& from, const GridPose& to) { - this->action_queuer.push_solving(from, to); - const float total_time = this->action_queuer.get_total_time(); - this->action_queuer.recompute({}, false); - - return total_time; - } - - ActionQueuer action_queuer; -}; } // namespace micras::nav #endif // MICRAS_NAV_ACTION_QUEUER_HPP diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 8c5fa56..6f978d5 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -50,28 +50,6 @@ class Action { */ virtual bool finished(const Pose& current_pose) = 0; - /** - * @brief Increment the action ID value. - * - * @param value_increment The increment value. - * @return A reference to the current action. - */ - virtual Action& operator+=(float value_increment) { - this->id.value += value_increment; - return *this; - } - - /** - * @brief Decrement the action ID value. - * - * @param value_decrement The decrement value. - * @return A reference to the current action. - */ - virtual Action& operator-=(float value_decrement) { - this->id.value -= value_decrement; - return *this; - } - /** * @brief Get the ID of the action. * diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 631c709..70bb71f 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -87,36 +87,6 @@ class MoveAction : public Action { */ bool finished(const Pose& pose) override { return pose.position.magnitude() >= this->distance; } - /** - * @brief Increment the distance to move by a certain value. - * - * @param value_increment The increment value. - * @return A reference to the current action. - */ - MoveAction& operator+=(float value_increment) override { - Action::operator+=(value_increment); - this->distance += value_increment; - - this->set_total_time(calculate_total_time( - this->distance, std::sqrt(this->start_speed_2), std::sqrt(this->end_speed_2), this->max_speed, - this->max_acceleration_doubled / 2.0F, this->max_deceleration_doubled / 2.0F - )); - - this->decelerate_distance = - (this->end_speed_2 - this->start_speed_2 + this->max_deceleration_doubled * this->distance) / - (this->max_acceleration_doubled + this->max_deceleration_doubled); - - return *this; - } - - /** - * @brief Decrement the distance to move by a certain value. - * - * @param value_increment The decrement value. - * @return A reference to the current action. - */ - MoveAction& operator-=(float value_increment) override { return *this += -value_increment; } - private: /** * @brief Calculate the total time to complete the action. diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index ce7ab7b..45b74bf 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -21,21 +21,18 @@ class TurnAction : public Action { * * @param action_type The type of the action to be performed. * @param angle Angle to turn in radians. - * @param curve_radius Radius of the curve in meters. + * @param max_angular_speed Maximum angular speed in rad/s. * @param linear_speed Linear speed in m/s. * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. */ TurnAction( - uint8_t action_type, float angle, float curve_radius, float linear_speed, float max_angular_acceleration + uint8_t action_type, float angle, float max_angular_speed, float linear_speed, float max_angular_acceleration ) : - Action{{action_type, angle}, false}, + Action{{action_type, angle}, false, calculate_total_time(angle, max_angular_speed, max_angular_acceleration)}, angle{angle}, + max_angular_speed{max_angular_speed}, linear_speed{linear_speed}, - acceleration{max_angular_acceleration}, - max_angular_speed{calculate_max_angular_speed(angle, curve_radius, linear_speed, max_angular_acceleration)}, - curve_radius{curve_radius} { - this->set_total_time(calculate_total_time(angle, max_angular_speed, max_angular_acceleration)); - } + acceleration{max_angular_acceleration} { } /** * @brief Get the desired speeds for the robot to complete the action. @@ -81,57 +78,74 @@ class TurnAction : public Action { } /** - * @brief Increment the angle to move by a certain value. + * @brief Calculate the maximum angular speed for a given curve radius and linear speed. * - * @param value_increment The increment value. - * @return A reference to the current action. + * @param angle Angle to turn in radians. + * @param curve_radius Radius of the curve in meters. + * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * @param max_centripetal_acceleration Maximum centripetal acceleration in m/s^2. + * @return Maximum angular speed in rad/s. + * + * @details Maximum angular velocity is computed to generate a curve equivalent in displacement to one that + * maintains the desired radius of curvature throughout the entire turn. + * If linear speed is zero, a minimal angular speed is assigned. */ - TurnAction& operator+=(float value_increment) override { - Action::operator+=(value_increment); - this->angle += value_increment; - this->max_angular_speed = - calculate_max_angular_speed(this->angle, this->curve_radius, this->linear_speed, this->acceleration); + static constexpr float calculate_max_angular_speed( + float angle, float curve_radius, float max_angular_acceleration, float max_centripetal_acceleration + ) { + if (curve_radius == 0.0F) { + return max_angular_acceleration * 0.01F; + } + const float correction_factor = + 14.35F - 13.57F * std::cos(std::abs(angle) - 2) - 10.0F / max_angular_acceleration; - this->set_total_time(calculate_total_time(this->angle, this->max_angular_speed, this->acceleration)); + const float transformed_acceleration = + std::pow(1.0F - std::cos(angle), 2.0F) * correction_factor * max_angular_acceleration; - return *this; + return std::sqrt( + (max_centripetal_acceleration * transformed_acceleration) / + (curve_radius * transformed_acceleration - max_centripetal_acceleration) + ); } /** - * @brief Decrement the angle to move by a certain value. + * @brief Calculate the total side displacement for a curve. * - * @param value_increment The decrement value. - * @return A reference to the current action. + * @param angle Angle to turn in radians. + * @param max_angular_speed Maximum angular speed in rad/s. + * @param linear_speed Linear speed in m/s. + * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * @return Side displacement in meters. */ - TurnAction& operator-=(float value_increment) override { return *this += -value_increment; } + static constexpr float calculate_side_displacement( + float angle, float max_angular_speed, float linear_speed, float max_angular_acceleration + ) { + const float transformed_speed = max_angular_speed / (1.0F - std::cos(angle)); + const float correction_factor = + 14.35F - 13.57F * std::cos(std::abs(angle) - 2) - 10.0F / max_angular_acceleration; + + return linear_speed * + (1.0F / transformed_speed + transformed_speed / (correction_factor * max_angular_acceleration)); + } /** - * @brief Calculate the maximum angular speed for a given curve radius and linear speed. + * @brief Calculate the total forward displacement for a curve. * * @param angle Angle to turn in radians. - * @param curve_radius Radius of the curve in meters. + * @param max_angular_speed Maximum angular speed in rad/s. * @param linear_speed Linear speed in m/s. * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. - * @return Maximum angular speed in rad/s. - * - * @details Maximum angular velocity is computed to generate a curve equivalent in displacement to one that - * maintains the desired radius of curvature throughout the entire turn. - * If linear speed is zero, a minimal angular speed is assigned. + * @return Forward displacement in meters. */ - static constexpr float calculate_max_angular_speed( - float angle, float curve_radius, float linear_speed, float max_angular_acceleration + static constexpr float calculate_forward_displacement( + float angle, float max_angular_speed, float linear_speed, float max_angular_acceleration ) { - if (linear_speed == 0.0F) { - return max_angular_acceleration * 0.01F; - } + const float transformed_speed = max_angular_speed / std::sin(angle); const float correction_factor = - 14.35F - 13.57F * std::cos(std::abs(angle) - 2) - 10.0F / max_angular_acceleration; - const float radius_speed_ratio = curve_radius / linear_speed; - const float quadratic_term = - 1.0F / (std::pow(1 - std::cos(angle), 2.0F) * correction_factor * max_angular_acceleration); - const float discriminant = std::pow(radius_speed_ratio, 2.0F) - 4.0F * quadratic_term; + 14.12F - 13.3F * std::cos(std::abs(angle) - 1.146F) - 10.0F / max_angular_acceleration; - return (radius_speed_ratio - std::sqrt(discriminant)) / (2.0F * quadratic_term); + return linear_speed * + (1.0F / transformed_speed + transformed_speed / (correction_factor * max_angular_acceleration)); } private: @@ -160,6 +174,11 @@ class TurnAction : public Action { */ float angle; + /** + * @brief Maximum angular speed in rad/s. + */ + float max_angular_speed; + /** * @brief Linear speed in m/s while turning. */ @@ -170,16 +189,6 @@ class TurnAction : public Action { */ float acceleration; - /** - * @brief Maximum angular speed in rad/s while turning. - */ - float max_angular_speed; - - /** - * @brief Radius of the equivalent curve in meters. - */ - float curve_radius; - /** * @brief Elapsed time in seconds since the action started. */ diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 239e8e4..4c4d780 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -12,9 +12,12 @@ namespace micras::nav { ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, start_offset{config.start_offset}, - curve_linear_speed{std::sqrt(config.solving.max_centrifugal_acceleration * config.solving.curve_radius)}, exploring_params{config.exploring}, solving_params{config.solving}, + exploration_curve_radius{ + exploring_params.max_linear_speed * exploring_params.max_linear_speed / + exploring_params.max_centrifugal_acceleration + }, stop{std::make_shared( ActionType::STOP, cell_size / 2.0F, exploring_params.max_linear_speed, 0.0F, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false @@ -34,17 +37,45 @@ ActionQueuer::ActionQueuer(Config config) : exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false )}, + move_to_turn{std::make_shared( + ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, exploring_params.max_linear_speed, + exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, + exploring_params.max_linear_deceleration + )}, turn_left{std::make_shared( - ActionType::TURN, std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, + ActionType::TURN, std::numbers::pi_v / 2.0F, exploration_curve_radius, exploring_params.max_linear_speed, exploring_params.max_angular_acceleration )}, turn_right{std::make_shared( - ActionType::TURN, -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, - exploring_params.max_angular_acceleration + ActionType::TURN, -std::numbers::pi_v / 2.0F, exploration_curve_radius, + exploring_params.max_linear_speed, exploring_params.max_angular_acceleration )}, turn_back{std::make_shared( ActionType::SPIN, std::numbers::pi_v, 0.0F, 0.0F, exploring_params.max_angular_acceleration - )} { } + )} { + for (const auto& [angle, curve_radius] : { + std::pair{std::numbers::pi_v / 4.0F, config.radius_45}, + std::pair{std::numbers::pi_v / 2.0F, config.radius_90}, + std::pair{3.0F * std::numbers::pi_v / 4.0F, config.radius_135}, + std::pair{std::numbers::pi_v, this->cell_size / 2.0F}, + }) { + const float max_angular_speed = TurnAction::calculate_max_angular_speed( + angle, curve_radius, config.solving.max_angular_acceleration, config.solving.max_centrifugal_acceleration + ); + + this->curves_parameters[angle] = { + .curve_radius = curve_radius, + .max_angular_speed = max_angular_speed, + .linear_speed = this->solving_params.max_centrifugal_acceleration / max_angular_speed, + .side_displacement = TurnAction::calculate_side_displacement( + angle, max_angular_speed, this->solving_params.max_linear_speed, config.solving.max_angular_acceleration + ), + .forward_displacement = TurnAction::calculate_forward_displacement( + angle, max_angular_speed, this->solving_params.max_linear_speed, config.solving.max_angular_acceleration + ), + }; + } +} void ActionQueuer::push_exploring(const GridPose& origin_pose, const GridPoint& target_position) { if (origin_pose.front().position == target_position) { @@ -53,12 +84,16 @@ void ActionQueuer::push_exploring(const GridPose& origin_pose, const GridPoint& } if (origin_pose.turned_left().front().position == target_position) { + this->action_queue.emplace_back(move_to_turn); this->action_queue.emplace_back(turn_left); + this->action_queue.emplace_back(move_to_turn); return; } if (origin_pose.turned_right().front().position == target_position) { + this->action_queue.emplace_back(move_to_turn); this->action_queue.emplace_back(turn_right); + this->action_queue.emplace_back(move_to_turn); return; } @@ -70,53 +105,33 @@ void ActionQueuer::push_exploring(const GridPose& origin_pose, const GridPoint& } } -void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& target_pose) { +std::list ActionQueuer::get_actions(const GridPose& origin_pose, const GridPose& target_pose) const { const Side relative_side = origin_pose.get_relative_side(target_pose.position); if (origin_pose.position == target_pose.position or relative_side == Side::DOWN) { - return; + return {}; } if (relative_side == Side::UP and origin_pose.orientation == target_pose.orientation) { const float distance = origin_pose.position.to_vector(this->cell_size).distance(target_pose.position.to_vector(this->cell_size)); - this->action_queue.emplace_back(std::make_shared( - ActionType::MOVE_FORWARD, distance, this->curve_linear_speed, this->curve_linear_speed, - this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, - this->solving_params.max_linear_deceleration - )); + return {{ActionType::MOVE_FORWARD, distance}}; } if (origin_pose.turned_left().front() == target_pose) { - this->action_queue.emplace_back(std::make_shared( - ActionType::TURN, std::numbers::pi_v / 2.0F, this->cell_size / 2.0F, this->curve_linear_speed, - this->solving_params.max_angular_acceleration - )); - return; + return {{ActionType::TURN, std::numbers::pi_v / 2.0F}}; } if (origin_pose.turned_right().front() == target_pose) { - this->action_queue.emplace_back(std::make_shared( - ActionType::TURN, -std::numbers::pi_v / 2.0F, this->cell_size / 2.0F, this->curve_linear_speed, - this->solving_params.max_angular_acceleration - )); - return; + return {{ActionType::TURN, -std::numbers::pi_v / 2.0F}}; } if (origin_pose.turned_left().front().turned_left().front() == target_pose) { - this->action_queue.emplace_back(std::make_shared( - ActionType::TURN, std::numbers::pi_v, this->cell_size / 2.0F, this->curve_linear_speed, - this->solving_params.max_angular_acceleration - )); - return; + return {{ActionType::TURN, std::numbers::pi_v}}; } if (origin_pose.turned_right().front().turned_right().front() == target_pose) { - this->action_queue.emplace_back(std::make_shared( - ActionType::TURN, -std::numbers::pi_v, this->cell_size / 2.0F, this->curve_linear_speed, - this->solving_params.max_angular_acceleration - )); - return; + return {{ActionType::TURN, -std::numbers::pi_v}}; } const bool keep_direction = (origin_pose.orientation == target_pose.orientation); @@ -124,22 +139,21 @@ void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& tar if (std::abs(target_position.x - origin_pose.position.x) != std::abs(target_position.y - origin_pose.position.y) or origin_pose.turned_right().get_relative_side(target_position) != Side::LEFT) { - return; + return {}; } if (not keep_direction and target_pose.orientation != (relative_side == Side::LEFT ? origin_pose.turned_left().orientation : origin_pose.turned_right().orientation)) { - return; + return {}; } + std::list actions; + float turn_angle = relative_side == Side::LEFT ? std::numbers::pi_v / 4.0F : -std::numbers::pi_v / 4.0F; - this->action_queue.emplace_back(std::make_shared( - ActionType::TURN, turn_angle, this->cell_size / 2.0F, this->curve_linear_speed, - this->solving_params.max_angular_acceleration - )); + actions.emplace_back(Action::Id{ActionType::TURN, turn_angle}); float distance = origin_pose.position.to_vector(this->cell_size).distance(target_position.to_vector(this->cell_size)); @@ -150,16 +164,10 @@ void ActionQueuer::push_solving(const GridPose& origin_pose, const GridPose& tar distance += this->cell_size * std::numbers::sqrt2_v / 2; } - this->action_queue.emplace_back(std::make_shared( - ActionType::DIAGONAL, distance, this->curve_linear_speed, this->curve_linear_speed, - this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, - this->solving_params.max_linear_deceleration, false - )); + actions.emplace_back(Action::Id{ActionType::DIAGONAL, distance}); + actions.emplace_back(Action::Id{ActionType::TURN, turn_angle}); - this->action_queue.emplace_back(std::make_shared( - ActionType::TURN, turn_angle, this->cell_size / 2.0F, this->curve_linear_speed, - this->solving_params.max_angular_acceleration - )); + return actions; } std::shared_ptr ActionQueuer::pop() { @@ -183,39 +191,71 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta return; } + std::list actions; + for (auto route_it = best_route.begin(); std::next(route_it) != best_route.end(); route_it++) { - this->push_solving(*route_it, *std::next(route_it)); + actions.splice(actions.end(), this->get_actions(*route_it, *std::next(route_it))); } - float start_distance = this->cell_size - this->start_offset; + for (auto action_it = actions.begin(); action_it != actions.end(); action_it++) { + if (action_it->type != ActionType::TURN) { + continue; + } + + if (std::next(action_it)->type == ActionType::TURN) { + const float turn_angle = action_it->value; + action_it = actions.erase(action_it); + action_it->value += turn_angle; + } + + auto [trim_before_distance, trim_after_distance] = this->get_trim_distances(*std::prev(action_it), *action_it); - if (this->action_queue.front()->get_id().type == ActionType::MOVE_FORWARD) { - start_distance += this->action_queue.front()->get_id().value - this->cell_size; - this->action_queue.pop_front(); + std::prev(action_it)->value -= trim_before_distance; + std::next(action_it)->value -= trim_after_distance; + } + + const float start_distance = this->cell_size - this->start_offset; + + if (actions.front().type == ActionType::MOVE_FORWARD) { + actions.front().value += start_distance; + } else { + actions.emplace_front(ActionType::MOVE_FORWARD, start_distance); } - this->action_queue.emplace_front(std::make_shared( - ActionType::START, start_distance, 0.0F, this->curve_linear_speed, this->solving_params.max_linear_speed, - this->solving_params.max_linear_acceleration, this->solving_params.max_linear_deceleration - )); - this->action_queue.emplace_back(stop); + float start_speed = 0.0F; - for (auto action_it = this->action_queue.begin(); action_it != this->action_queue.end(); action_it++) { - if ((*action_it)->get_id().type != ActionType::TURN) { + for (auto action_it = std::next(actions.begin()); action_it != actions.end(); action_it++) { + if (action_it->type != ActionType::TURN) { continue; } - if ((*std::prev(action_it))->get_id().type == ActionType::TURN) { - action_it = this->join_curves(std::prev(action_it)); - } else if ((*std::next(action_it))->get_id().type == ActionType::TURN) { - action_it = this->join_curves(action_it); - } + const auto& curve_parameters = this->curves_parameters.at(std::abs(action_it->value)); + + this->action_queue.emplace_back(std::make_shared( + std::prev(action_it)->type, std::prev(action_it)->value, start_speed, curve_parameters.linear_speed, + this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, + this->solving_params.max_linear_deceleration + )); + + this->action_queue.emplace_back(std::make_shared( + action_it->type, action_it->value, curve_parameters.max_angular_speed, curve_parameters.linear_speed, + this->solving_params.max_angular_acceleration + )); - auto [trim_before_distance, trim_after_distance] = - this->get_trim_distances((*std::prev(action_it))->get_id(), **action_it); + start_speed = curve_parameters.linear_speed; + } - **std::prev(action_it) -= trim_before_distance; - **std::next(action_it) -= trim_after_distance; + if (actions.back().type == ActionType::MOVE_FORWARD) { + this->action_queue.emplace_back(std::make_shared( + ActionType::STOP, actions.back().value + this->cell_size / 2.0F, start_speed, 0.0F, + this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, + this->solving_params.max_linear_deceleration + )); + } else { + this->action_queue.emplace_back(std::make_shared( + ActionType::STOP, this->cell_size / 2.0F, start_speed, 0.0F, this->solving_params.max_linear_speed, + this->solving_params.max_linear_acceleration, this->solving_params.max_linear_deceleration + )); } } @@ -230,29 +270,20 @@ float ActionQueuer::get_total_time() const { } std::pair - ActionQueuer::get_trim_distances(const Action::Id& action_before, const Action& turn_action) const { - const float turn_angle = std::abs(turn_action.get_id().value); - const float side_displacement = (1.0F - std::cos(turn_angle)) * this->cell_size / 2.0F; - - const float correction_factor = 14.12F - 13.3F * std::cos(turn_angle - 1.145F); - const float max_angular_speed = TurnAction::calculate_max_angular_speed( - turn_angle, this->cell_size / 2.0F, this->curve_linear_speed, this->solving_params.max_angular_acceleration - ); - const float forward_displacement = - this->curve_linear_speed * (std::sin(turn_angle) / max_angular_speed + - max_angular_speed / (std::sin(turn_angle) * correction_factor * - this->solving_params.max_angular_acceleration)); + ActionQueuer::get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) const { + const float turn_angle = std::abs(turn_action.value); + const auto& curve_parameters = this->curves_parameters.at(turn_angle); if (turn_angle == std::numbers::pi_v / 4.0F) { - const float trim_before_distance = forward_displacement - side_displacement; - const float trim_after_distance = side_displacement * std::numbers::sqrt2_v; + const float trim_before_distance = curve_parameters.forward_displacement - curve_parameters.side_displacement; + const float trim_after_distance = curve_parameters.side_displacement * std::numbers::sqrt2_v; return {trim_before_distance, trim_after_distance}; } if (turn_angle == std::numbers::pi_v / 2.0F) { - float trim_before_distance = forward_displacement; - float trim_after_distance = side_displacement; + float trim_before_distance = curve_parameters.forward_displacement; + float trim_after_distance = curve_parameters.side_displacement; if (action_before.type != ActionType::DIAGONAL) { trim_before_distance -= this->cell_size / 2.0F; @@ -263,8 +294,8 @@ std::pair } if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { - float trim_before_distance = forward_displacement + side_displacement; - float trim_after_distance = std::numbers::sqrt2_v * side_displacement; + float trim_before_distance = curve_parameters.forward_displacement + curve_parameters.side_displacement; + float trim_after_distance = std::numbers::sqrt2_v * curve_parameters.side_displacement; if (action_before.type == ActionType::DIAGONAL) { trim_before_distance -= this->cell_size * std::numbers::sqrt2_v / 2.0F; @@ -279,14 +310,4 @@ std::pair return {0.0F, 0.0F}; } - -std::deque>::iterator - ActionQueuer::join_curves(std::deque>::iterator turn_it) { - const float turn_angle = (*turn_it)->get_id().value; - - turn_it = this->action_queue.erase(turn_it); - (**turn_it) += turn_angle; - - return turn_it; -} } // namespace micras::nav From 1c0b6bec803b1dc8a16c650d05b0d5ab7dce37a9 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 21 May 2025 23:47:16 -0300 Subject: [PATCH 426/561] =?UTF-8?q?=E2=9C=A8=20Improve=20maze=20graph=20no?= =?UTF-8?q?de=20connections?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 19 ++- .../include/micras/nav/actions/move.hpp | 2 +- .../include/micras/nav/actions/turn.hpp | 2 +- micras_nav/include/micras/nav/maze.hpp | 12 +- micras_nav/include/micras/nav/maze_graph.hpp | 87 +++++++--- micras_nav/src/action_queuer.cpp | 132 +++++++------- micras_nav/src/maze.cpp | 8 +- micras_nav/src/maze_graph.cpp | 161 +++++++++++------- 8 files changed, 246 insertions(+), 177 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 55cd4d3..ba226f4 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -68,15 +68,6 @@ class ActionQueuer { */ void push_exploring(const GridPose& origin_pose, const GridPoint& target_position); - /** - * @brief Get the actions to perform from the origin pose to the target pose. - * - * @param origin_pose Origin pose of the robot. - * @param target_pose Target pose to move to. - * @return List of actions to perform. - */ - std::list get_actions(const GridPose& origin_pose, const GridPose& target_pose) const; - /** * @brief Pop an action from the queue. * @@ -103,6 +94,16 @@ class ActionQueuer { */ float get_total_time() const; + /** + * @brief Get the actions to perform from the origin pose to the target pose. + * + * @param origin_pose Origin pose of the robot. + * @param target_pose Target pose to move to. + * @param cell_size Size of the cells in the grid. + * @return List of actions to perform. + */ + static std::list get_actions(const GridPose& origin_pose, const GridPose& target_pose, float cell_size); + private: /** * @brief Struct to hold the parameters for a curve. diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 70bb71f..eca5827 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -87,7 +87,6 @@ class MoveAction : public Action { */ bool finished(const Pose& pose) override { return pose.position.magnitude() >= this->distance; } -private: /** * @brief Calculate the total time to complete the action. * @@ -125,6 +124,7 @@ class MoveAction : public Action { return acceleration_time + deceleration_time + constant_speed_time; } +private: /** * @brief Distance to move in meters. */ diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 45b74bf..ab41f68 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -148,7 +148,6 @@ class TurnAction : public Action { (1.0F / transformed_speed + transformed_speed / (correction_factor * max_angular_acceleration)); } -private: /** * @brief Calculate the total time to complete the action. * @@ -169,6 +168,7 @@ class TurnAction : public Action { return std::abs(angle) / max_angular_speed + acceleration_time; } +private: /** * @brief Angle to turn in radians. */ diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index ddcb22d..f6c8d5f 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -25,20 +25,16 @@ namespace micras::nav { template class TMaze : public core::ISerializable { public: + /** + * @brief Configuration structure for the maze. + */ struct Config { GridPose start{}; std::unordered_set goal; float cost_margin{}; + MazeGraph::Config graph_config{}; }; - /** - * @brief Construct a new TMaze object. - * - * @param config The configuration for the maze. - * @param edge_cost_function The function to compute the cost of the edges in the maze. - */ - TMaze(Config config, EdgeCostFunction edge_cost_function); - /** * @brief Construct a new TMaze object. * diff --git a/micras_nav/include/micras/nav/maze_graph.hpp b/micras_nav/include/micras/nav/maze_graph.hpp index 30447c2..7cb750f 100644 --- a/micras_nav/include/micras/nav/maze_graph.hpp +++ b/micras_nav/include/micras/nav/maze_graph.hpp @@ -9,33 +9,40 @@ #include #include +#include "micras/nav/action_queuer.hpp" #include "micras/nav/grid_pose.hpp" namespace micras::nav { -using EdgeCostFunction = std::function; - /** * @brief */ class MazeGraph { public: + /** + * @brief Node structure representing a node in the maze graph. + */ struct Node { explicit Node(GridPose pose) : pose(pose){}; - GridPose pose; - std::unordered_map next_costs; - std::unordered_set prev; + GridPose pose; + std::unordered_set next; + std::unordered_set prev; + }; + + /** + * @brief Configuration structure for the maze graph. + */ + struct Config { + float cell_size; + ActionQueuer::Config::Dynamic cost_params; }; /** * @brief Construct a new MazeGraph object. * - * @param edge_cost_function Function to compute the cost of an edge. + * @param config The configuration for the maze graph. */ - explicit MazeGraph(EdgeCostFunction edge_cost_function = [](const GridPose& from, const GridPose& to) -> float { - float edge_cost = std::abs(from.position.x - to.position.x) + std::abs(from.position.y - to.position.y); - return edge_cost > 1.0F ? 0.0F : edge_cost; - }); + explicit MazeGraph(Config config); /** * @brief Check if the graph has a node for a given pose. @@ -59,7 +66,15 @@ class MazeGraph { * @param to The ending node of the edge. * @param cost The cost of the edge. */ - void add_edge(const GridPose& from, const GridPose& to, float cost = 0.0F); + void add_edge(const GridPose& from, const GridPose& to); + + /** + * @brief Remove an edge connecting two poses. + * + * @param from The starting node of the edge. + * @param to The ending node of the edge. + */ + void remove_edge(const GridPose& from, const GridPose& to); /** * @brief Process the nodes in the graph. @@ -84,27 +99,59 @@ class MazeGraph { private: /** - * @brief Remove unnecessary nodes from the graph. + * @brief Add extra edges representing diagonals, long curves and long straights to the graph. * * @param start The starting node. */ - void remove_extra_nodes(const GridPose& start); + void add_extra_edges(const GridPose& start); /** - * @brief Add extra edges representing diagonals, long curves and long straights to the graph. + * @brief Check if a skip is possible between three poses. * - * @param start The starting node. + * @param first The first pose. + * @param second The second pose. + * @param third The third pose. + * @return true if a skip is possible, false otherwise. */ - void add_extra_edges(const GridPose& start); + bool can_skip(const GridPose& first, const GridPose& second, const GridPose& third) const; /** - * @brief Function to compute the cost of an edge. + * @brief Get the cost of an edge between two poses. * - * @param from The starting node of the edge. - * @param to The ending node of the edge. + * @param from The starting pose. + * @param to The ending pose. * @return The cost of the edge. */ - EdgeCostFunction get_edge_cost; + float get_edge_cost(const GridPose& from, const GridPose& to) const; + + /** + * @brief Get the main action type that connects two grid poses. + * + * @param from The starting pose. + * @param to The ending pose. + * @return The main action type. + */ + ActionQueuer::ActionType get_main_action_type(const GridPose& from, const GridPose& to) const; + + /** + * @brief The size of the cells in the maze. + */ + float cell_size; + + /** + * @brief The configuration for calculating the edge costs. + */ + ActionQueuer::Config::Dynamic cost_params; + + /** + * @brief The radius of the curve when performing a turn. + */ + float curve_radius; + + /** + * @brief The linear speed of the robot while turning. + */ + float curve_linear_speed; /** * @brief diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 4c4d780..0853bc2 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -105,71 +105,6 @@ void ActionQueuer::push_exploring(const GridPose& origin_pose, const GridPoint& } } -std::list ActionQueuer::get_actions(const GridPose& origin_pose, const GridPose& target_pose) const { - const Side relative_side = origin_pose.get_relative_side(target_pose.position); - - if (origin_pose.position == target_pose.position or relative_side == Side::DOWN) { - return {}; - } - - if (relative_side == Side::UP and origin_pose.orientation == target_pose.orientation) { - const float distance = - origin_pose.position.to_vector(this->cell_size).distance(target_pose.position.to_vector(this->cell_size)); - return {{ActionType::MOVE_FORWARD, distance}}; - } - - if (origin_pose.turned_left().front() == target_pose) { - return {{ActionType::TURN, std::numbers::pi_v / 2.0F}}; - } - - if (origin_pose.turned_right().front() == target_pose) { - return {{ActionType::TURN, -std::numbers::pi_v / 2.0F}}; - } - - if (origin_pose.turned_left().front().turned_left().front() == target_pose) { - return {{ActionType::TURN, std::numbers::pi_v}}; - } - - if (origin_pose.turned_right().front().turned_right().front() == target_pose) { - return {{ActionType::TURN, -std::numbers::pi_v}}; - } - - const bool keep_direction = (origin_pose.orientation == target_pose.orientation); - const GridPoint target_position = (keep_direction ? target_pose : target_pose.turned_back().front()).position; - - if (std::abs(target_position.x - origin_pose.position.x) != std::abs(target_position.y - origin_pose.position.y) or - origin_pose.turned_right().get_relative_side(target_position) != Side::LEFT) { - return {}; - } - - if (not keep_direction and - target_pose.orientation != (relative_side == Side::LEFT ? origin_pose.turned_left().orientation : - origin_pose.turned_right().orientation)) { - return {}; - } - - std::list actions; - - float turn_angle = - relative_side == Side::LEFT ? std::numbers::pi_v / 4.0F : -std::numbers::pi_v / 4.0F; - - actions.emplace_back(Action::Id{ActionType::TURN, turn_angle}); - - float distance = - origin_pose.position.to_vector(this->cell_size).distance(target_position.to_vector(this->cell_size)); - - if (keep_direction) { - turn_angle *= -1.0F; - } else { - distance += this->cell_size * std::numbers::sqrt2_v / 2; - } - - actions.emplace_back(Action::Id{ActionType::DIAGONAL, distance}); - actions.emplace_back(Action::Id{ActionType::TURN, turn_angle}); - - return actions; -} - std::shared_ptr ActionQueuer::pop() { auto action = this->action_queue.front(); this->action_queue.pop_front(); @@ -194,7 +129,7 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta std::list actions; for (auto route_it = best_route.begin(); std::next(route_it) != best_route.end(); route_it++) { - actions.splice(actions.end(), this->get_actions(*route_it, *std::next(route_it))); + actions.splice(actions.end(), get_actions(*route_it, *std::next(route_it), this->cell_size)); } for (auto action_it = actions.begin(); action_it != actions.end(); action_it++) { @@ -269,6 +204,71 @@ float ActionQueuer::get_total_time() const { return total_time; } +std::list + ActionQueuer::get_actions(const GridPose& origin_pose, const GridPose& target_pose, float cell_size) { + const Side relative_side = origin_pose.get_relative_side(target_pose.position); + + if (origin_pose.position == target_pose.position or relative_side == Side::DOWN) { + return {}; + } + + if (relative_side == Side::UP and origin_pose.orientation == target_pose.orientation) { + const float distance = + origin_pose.position.to_vector(cell_size).distance(target_pose.position.to_vector(cell_size)); + return {{ActionType::MOVE_FORWARD, distance}}; + } + + if (origin_pose.turned_left().front() == target_pose) { + return {{ActionType::TURN, std::numbers::pi_v / 2.0F}}; + } + + if (origin_pose.turned_right().front() == target_pose) { + return {{ActionType::TURN, -std::numbers::pi_v / 2.0F}}; + } + + if (origin_pose.turned_left().front().turned_left().front() == target_pose) { + return {{ActionType::TURN, std::numbers::pi_v}}; + } + + if (origin_pose.turned_right().front().turned_right().front() == target_pose) { + return {{ActionType::TURN, -std::numbers::pi_v}}; + } + + const bool keep_direction = (origin_pose.orientation == target_pose.orientation); + const GridPoint target_position = (keep_direction ? target_pose : target_pose.turned_back().front()).position; + + if (std::abs(target_position.x - origin_pose.position.x) != std::abs(target_position.y - origin_pose.position.y) or + origin_pose.turned_right().get_relative_side(target_position) != Side::LEFT) { + return {}; + } + + if (not keep_direction and + target_pose.orientation != (relative_side == Side::LEFT ? origin_pose.turned_left().orientation : + origin_pose.turned_right().orientation)) { + return {}; + } + + std::list actions; + + float turn_angle = + relative_side == Side::LEFT ? std::numbers::pi_v / 4.0F : -std::numbers::pi_v / 4.0F; + + actions.emplace_back(Action::Id{ActionType::TURN, turn_angle}); + + float distance = origin_pose.position.to_vector(cell_size).distance(target_position.to_vector(cell_size)); + + if (keep_direction) { + turn_angle *= -1.0F; + } else { + distance += cell_size * std::numbers::sqrt2_v / 2; + } + + actions.emplace_back(Action::Id{ActionType::DIAGONAL, distance}); + actions.emplace_back(Action::Id{ActionType::TURN, turn_angle}); + + return actions; +} + std::pair ActionQueuer::get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) const { const float turn_angle = std::abs(turn_action.value); diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 1cdfa12..3181f22 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -13,12 +13,8 @@ namespace micras::nav { template -TMaze::TMaze(Config config, EdgeCostFunction edge_cost_function) : TMaze(config) { - this->graph = MazeGraph{std::move(edge_cost_function)}; -} - -template -TMaze::TMaze(Config config) : start{config.start}, goal{config.goal}, cost_margin{config.cost_margin} { +TMaze::TMaze(Config config) : + graph(config.graph_config), start{config.start}, goal{config.goal}, cost_margin{config.cost_margin} { this->costmap.update_wall(this->start, false); this->costmap.update_wall(start.turned_right(), true); diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index cc23957..d825c1a 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -2,16 +2,19 @@ * @file */ +#include #include #include -#include -#include +#include "micras/nav/action_queuer.hpp" #include "micras/nav/maze_graph.hpp" -#include "micras/nav/grid_pose.hpp" namespace micras::nav { -MazeGraph::MazeGraph(EdgeCostFunction edge_cost_function) : get_edge_cost(std::move(edge_cost_function)) { } +MazeGraph::MazeGraph(Config config) : + cell_size{config.cell_size}, + cost_params{config.cost_params}, + curve_radius{this->cell_size / 2.0F}, + curve_linear_speed{std::sqrt(cost_params.max_centrifugal_acceleration * curve_radius)} { } bool MazeGraph::has_node(const GridPose& pose) const { return this->graph.contains(pose); @@ -21,21 +24,25 @@ void MazeGraph::add_node(const GridPose& pose) { this->graph.emplace(pose, Node{pose}); } -void MazeGraph::add_edge(const GridPose& from, const GridPose& to, float cost) { - if (this->graph.at(from).next_costs.contains(to)) { +void MazeGraph::add_edge(const GridPose& from, const GridPose& to) { + if (this->graph.at(from).next.contains(to)) { return; } - this->graph.at(from).next_costs[to] = cost; + this->graph.at(from).next.emplace(to); this->graph.at(to).prev.emplace(from); } +void MazeGraph::remove_edge(const GridPose& from, const GridPose& to) { + this->graph.at(from).next.erase(to); + this->graph.at(to).prev.erase(from); +} + void MazeGraph::process_nodes(const GridPose& start) { - this->remove_extra_nodes(start); this->add_extra_edges(start); } -void MazeGraph::remove_extra_nodes(const GridPose& start) { +void MazeGraph::add_extra_edges(const GridPose& start) { std::unordered_set visited; std::queue queue; queue.push(start); @@ -46,73 +53,40 @@ void MazeGraph::remove_extra_nodes(const GridPose& start) { queue.pop(); visited.insert(current_pose); + bool skipped = false; - for (auto next_it = current_node.next_costs.begin(); next_it != current_node.next_costs.end();) { - Node& next_node = this->graph.at(next_it->first); - - if (next_node.next_costs.size() == 1) { - const GridPose& next_next = next_node.next_costs.begin()->first; - - if (current_pose.orientation == next_node.pose.orientation and - current_pose.orientation == next_next.orientation) { - current_node.next_costs.erase(next_it); - next_node.prev.erase(current_pose); - - this->add_edge(current_pose, next_next); - - if (next_node.prev.empty()) { - this->graph.at(next_next).prev.erase(next_node.pose); - this->graph.erase(next_node.pose); - } + for (auto next_it = current_node.next.begin(); next_it != current_node.next.end();) { + GridPose next_pose = *next_it; - next_it = current_node.next_costs.begin(); + for (const GridPose& next_next_pose : this->graph.at(next_pose).next) { + if (current_node.next.contains(next_next_pose)) { continue; } - } - - if (not visited.contains(next_it->first)) { - queue.push(next_it->first); - } - - next_it++; - } - } -} - -void MazeGraph::add_extra_edges(const GridPose& start) { - std::unordered_set visited; - std::queue queue; - queue.push(start); - while (not queue.empty()) { - const GridPose current_pose = queue.front(); - Node& current_node = this->graph.at(current_pose); - queue.pop(); + if (this->can_skip(current_pose, next_pose, next_next_pose)) { + skipped = true; + this->add_edge(current_pose, next_next_pose); + next_it = current_node.next.begin(); - visited.insert(current_pose); - - for (auto next_it = current_node.next_costs.begin(); next_it != current_node.next_costs.end();) { - if (next_it->second == 0) { - next_it->second = this->get_edge_cost(current_pose, next_it->first); - } + if (this->graph.at(next_pose).next.size() == 1) { + this->remove_edge(current_pose, next_pose); + next_it = current_node.next.begin(); - GridPose next_pose = next_it->first; - - for (const auto& [next_next_pose, next_next_cost] : this->graph.at(next_pose).next_costs) { - if (current_node.next_costs.contains(next_next_pose)) { - continue; + if (this->graph.at(next_pose).prev.empty()) { + this->graph.erase(next_pose); + break; + } + } } + } - const float edge_cost = this->get_edge_cost(current_pose, next_next_pose); - - if (edge_cost > 0) { - this->add_edge(current_pose, next_next_pose, edge_cost); - next_it = current_node.next_costs.begin(); - } + if (skipped) { + skipped = false; + continue; } - if (not visited.contains(next_it->first)) { - queue.push(next_it->first); + if (not visited.contains(next_pose)) { + queue.push(next_pose); } next_it++; @@ -153,7 +127,9 @@ std::list MazeGraph::get_best_route(const GridPose& start, const std:: visited.insert(current_pose); - for (const auto& [next_pose, edge_cost] : this->graph.at(current_pose).next_costs) { + for (const GridPose& next_pose : this->graph.at(current_pose).next) { + float edge_cost = this->get_edge_cost(current_pose, next_pose); + if (distance.at(next_pose) > distance.at(current_pose) + edge_cost) { previous[next_pose] = current_pose; distance[next_pose] = distance.at(current_pose) + edge_cost; @@ -177,4 +153,57 @@ std::list MazeGraph::get_best_route(const GridPose& start, const std:: void MazeGraph::reset() { this->graph.clear(); } + +bool MazeGraph::can_skip(const GridPose& first, const GridPose& second, const GridPose& third) const { + auto skip_action = this->get_main_action_type(first, third); + + if (skip_action == ActionQueuer::ActionType::STOP) { + return false; + } + + if (skip_action != ActionQueuer::ActionType::MOVE_FORWARD) { + return true; + } + + return this->get_main_action_type(first, second) == ActionQueuer::ActionType::MOVE_FORWARD; +} + +float MazeGraph::get_edge_cost(const GridPose& from, const GridPose& to) const { + auto actions = ActionQueuer::get_actions(from, to, this->cell_size); + float total_time = 0.0F; + + for (const auto& action : actions) { + if (action.type == ActionQueuer::ActionType::TURN) { + const float max_angular_speed = TurnAction::calculate_max_angular_speed( + action.value, this->curve_radius, this->cost_params.max_angular_acceleration, + this->cost_params.max_centrifugal_acceleration + ); + + total_time += TurnAction::calculate_total_time( + action.value, max_angular_speed, this->cost_params.max_angular_acceleration + ); + } else { + total_time += MoveAction::calculate_total_time( + action.value, this->curve_linear_speed, this->curve_linear_speed, this->cost_params.max_linear_speed, + this->cost_params.max_linear_acceleration, this->cost_params.max_linear_deceleration + ); + } + } + + return total_time; +} + +ActionQueuer::ActionType MazeGraph::get_main_action_type(const GridPose& from, const GridPose& to) const { + auto actions = ActionQueuer::get_actions(from, to, this->cell_size); + + if (actions.empty()) { + return ActionQueuer::ActionType::STOP; + } + + if (actions.size() == 3) { + return ActionQueuer::ActionType::DIAGONAL; + } + + return static_cast(actions.front().type); +} } // namespace micras::nav From 12c9642a15012640d008c16168c1a26824b7efd9 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 22 May 2025 00:16:40 -0300 Subject: [PATCH 427/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20max=20angular=20sp?= =?UTF-8?q?eed=20on=20exploration=20turn=20actions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/actions/turn.hpp | 5 +++-- micras_nav/src/action_queuer.cpp | 21 +++++++++++++++---- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index ab41f68..ca23ca9 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -96,6 +96,7 @@ class TurnAction : public Action { if (curve_radius == 0.0F) { return max_angular_acceleration * 0.01F; } + const float correction_factor = 14.35F - 13.57F * std::cos(std::abs(angle) - 2) - 10.0F / max_angular_acceleration; @@ -140,7 +141,7 @@ class TurnAction : public Action { static constexpr float calculate_forward_displacement( float angle, float max_angular_speed, float linear_speed, float max_angular_acceleration ) { - const float transformed_speed = max_angular_speed / std::sin(angle); + const float transformed_speed = max_angular_speed / std::sin(std::abs(angle)); const float correction_factor = 14.12F - 13.3F * std::cos(std::abs(angle) - 1.146F) - 10.0F / max_angular_acceleration; @@ -157,7 +158,7 @@ class TurnAction : public Action { * @return Total time to complete the action in seconds. */ static constexpr float calculate_total_time(float angle, float max_angular_speed, float max_angular_acceleration) { - const float peak_velocity = std::sqrt(angle * max_angular_acceleration); + const float peak_velocity = std::sqrt(std::abs(angle) * max_angular_acceleration); if (peak_velocity < max_angular_speed) { return 2.0F * peak_velocity / max_angular_acceleration; diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 0853bc2..2991d5e 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -43,15 +43,28 @@ ActionQueuer::ActionQueuer(Config config) : exploring_params.max_linear_deceleration )}, turn_left{std::make_shared( - ActionType::TURN, std::numbers::pi_v / 2.0F, exploration_curve_radius, exploring_params.max_linear_speed, - exploring_params.max_angular_acceleration + ActionType::TURN, std::numbers::pi_v / 2.0F, + TurnAction::calculate_max_angular_speed( + std::numbers::pi_v / 2.0F, exploration_curve_radius, exploring_params.max_angular_acceleration, + exploring_params.max_centrifugal_acceleration + ), + exploring_params.max_linear_speed, exploring_params.max_angular_acceleration )}, turn_right{std::make_shared( - ActionType::TURN, -std::numbers::pi_v / 2.0F, exploration_curve_radius, + ActionType::TURN, -std::numbers::pi_v / 2.0F, + TurnAction::calculate_max_angular_speed( + -std::numbers::pi_v / 2.0F, exploration_curve_radius, exploring_params.max_angular_acceleration, + exploring_params.max_centrifugal_acceleration + ), exploring_params.max_linear_speed, exploring_params.max_angular_acceleration )}, turn_back{std::make_shared( - ActionType::SPIN, std::numbers::pi_v, 0.0F, 0.0F, exploring_params.max_angular_acceleration + ActionType::SPIN, std::numbers::pi_v, + TurnAction::calculate_max_angular_speed( + std::numbers::pi_v, 0.0F, exploring_params.max_angular_acceleration, + exploring_params.max_centrifugal_acceleration + ), + 0.0F, exploring_params.max_angular_acceleration )} { for (const auto& [angle, curve_radius] : { std::pair{std::numbers::pi_v / 4.0F, config.radius_45}, From 5a2cc9042c71db61076fd98bb009ad52822f5e99 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 22 May 2025 01:03:57 -0300 Subject: [PATCH 428/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20displacement=20cal?= =?UTF-8?q?culation=20from=20solving=20turn=20actions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 2991d5e..5ba4aa8 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -75,16 +75,17 @@ ActionQueuer::ActionQueuer(Config config) : const float max_angular_speed = TurnAction::calculate_max_angular_speed( angle, curve_radius, config.solving.max_angular_acceleration, config.solving.max_centrifugal_acceleration ); + const float curve_linear_speed = this->solving_params.max_centrifugal_acceleration / max_angular_speed; this->curves_parameters[angle] = { .curve_radius = curve_radius, .max_angular_speed = max_angular_speed, - .linear_speed = this->solving_params.max_centrifugal_acceleration / max_angular_speed, + .linear_speed = curve_linear_speed, .side_displacement = TurnAction::calculate_side_displacement( - angle, max_angular_speed, this->solving_params.max_linear_speed, config.solving.max_angular_acceleration + angle, max_angular_speed, curve_linear_speed, config.solving.max_angular_acceleration ), .forward_displacement = TurnAction::calculate_forward_displacement( - angle, max_angular_speed, this->solving_params.max_linear_speed, config.solving.max_angular_acceleration + angle, max_angular_speed, curve_linear_speed, config.solving.max_angular_acceleration ), }; } From d82dcc1c2961734b124f0657af3b6f8781e2b41d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 22 May 2025 01:11:30 -0300 Subject: [PATCH 429/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20solve=20start=20ac?= =?UTF-8?q?tion=20distance?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 5ba4aa8..65ab589 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -163,14 +163,7 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta std::next(action_it)->value -= trim_after_distance; } - const float start_distance = this->cell_size - this->start_offset; - - if (actions.front().type == ActionType::MOVE_FORWARD) { - actions.front().value += start_distance; - } else { - actions.emplace_front(ActionType::MOVE_FORWARD, start_distance); - } - + actions.front().value -= this->start_offset; float start_speed = 0.0F; for (auto action_it = std::next(actions.begin()); action_it != actions.end(); action_it++) { From 8e457a780d6262a6023da1d3b6c7030bd501ebb7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 22 May 2025 01:23:17 -0300 Subject: [PATCH 430/561] =?UTF-8?q?=F0=9F=90=9B=20Disable=20follow=20wall?= =?UTF-8?q?=20on=20diagonal?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 65ab589..62cac3e 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -176,7 +176,7 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta this->action_queue.emplace_back(std::make_shared( std::prev(action_it)->type, std::prev(action_it)->value, start_speed, curve_parameters.linear_speed, this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, - this->solving_params.max_linear_deceleration + this->solving_params.max_linear_deceleration, std::prev(action_it)->type != ActionType::DIAGONAL )); this->action_queue.emplace_back(std::make_shared( From a3eb191231667f68511c5a648e9d83cf5487b1e3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 22 May 2025 23:28:07 -0300 Subject: [PATCH 431/561] =?UTF-8?q?=E2=9C=A8=20Add=20remove=5Fnode=20metho?= =?UTF-8?q?d=20to=20maze=5Fgraph?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze_graph.hpp | 7 +++++++ micras_nav/src/action_queuer.cpp | 6 +++++- micras_nav/src/maze_graph.cpp | 22 +++++++++++++++----- 3 files changed, 29 insertions(+), 6 deletions(-) diff --git a/micras_nav/include/micras/nav/maze_graph.hpp b/micras_nav/include/micras/nav/maze_graph.hpp index 7cb750f..a1307de 100644 --- a/micras_nav/include/micras/nav/maze_graph.hpp +++ b/micras_nav/include/micras/nav/maze_graph.hpp @@ -59,6 +59,13 @@ class MazeGraph { */ void add_node(const GridPose& pose); + /** + * @brief Remove a node from the graph. + * + * @param pose The grid pose that represents the node. + */ + void remove_node(const GridPose& pose); + /** * @brief Add a new edge connecting two poses. * diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 62cac3e..c1dd8ad 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -219,7 +219,11 @@ std::list return {}; } - if (relative_side == Side::UP and origin_pose.orientation == target_pose.orientation) { + if (relative_side == Side::UP) { + if (origin_pose.orientation != target_pose.orientation) { + return {}; + } + const float distance = origin_pose.position.to_vector(cell_size).distance(target_pose.position.to_vector(cell_size)); return {{ActionType::MOVE_FORWARD, distance}}; diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index d825c1a..fb0a21d 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -24,6 +24,18 @@ void MazeGraph::add_node(const GridPose& pose) { this->graph.emplace(pose, Node{pose}); } +void MazeGraph::remove_node(const GridPose& pose) { + for (const GridPose& next_pose : this->graph.at(pose).next) { + if (this->graph.at(next_pose).prev.size() == 1) { + this->remove_node(next_pose); + } else { + this->graph.at(next_pose).prev.erase(pose); + } + } + + this->graph.erase(pose); +} + void MazeGraph::add_edge(const GridPose& from, const GridPose& to) { if (this->graph.at(from).next.contains(to)) { return; @@ -36,6 +48,10 @@ void MazeGraph::add_edge(const GridPose& from, const GridPose& to) { void MazeGraph::remove_edge(const GridPose& from, const GridPose& to) { this->graph.at(from).next.erase(to); this->graph.at(to).prev.erase(from); + + if (this->graph.at(to).prev.empty()) { + this->remove_node(to); + } } void MazeGraph::process_nodes(const GridPose& start) { @@ -71,11 +87,7 @@ void MazeGraph::add_extra_edges(const GridPose& start) { if (this->graph.at(next_pose).next.size() == 1) { this->remove_edge(current_pose, next_pose); next_it = current_node.next.begin(); - - if (this->graph.at(next_pose).prev.empty()) { - this->graph.erase(next_pose); - break; - } + break; } } } From 00790a58a39d1fa6269b0e4d3857d159f3ab35b5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Thu, 22 May 2025 23:51:53 -0300 Subject: [PATCH 432/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Remove=20time=20fr?= =?UTF-8?q?om=20diagonal=20trim=20on=20get=5Fedge=5Fcost?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/base.hpp | 7 ------- micras_nav/src/maze_graph.cpp | 5 +++++ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 6f978d5..d14334f 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -71,13 +71,6 @@ class Action { */ float get_total_time() const { return this->total_time; }; - /** - * @brief Set the total time it takes to perform the action. - * - * @param total_time The total time of the action in seconds. - */ - void set_total_time(float total_time) { this->total_time = total_time; } - protected: /** * @brief Special member functions declared as default. diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index fb0a21d..4477bf8 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -194,6 +194,11 @@ float MazeGraph::get_edge_cost(const GridPose& from, const GridPose& to) const { total_time += TurnAction::calculate_total_time( action.value, max_angular_speed, this->cost_params.max_angular_acceleration ); + + if (std::abs(action.value) == std::numbers::pi_v / 4.0F) { + total_time -= + this->cell_size * (std::numbers::sqrt2_v - 1.0F) / this->cost_params.max_linear_speed; + } } else { total_time += MoveAction::calculate_total_time( action.value, this->curve_linear_speed, this->curve_linear_speed, this->cost_params.max_linear_speed, From b4cf46ba02fe01e9039443c66ea52478d0fe0c9b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 23 May 2025 22:10:19 -0300 Subject: [PATCH 433/561] =?UTF-8?q?=E2=9C=A8=20Add=20fan=20events=20to=20i?= =?UTF-8?q?nterface?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/fsm.hpp | 4 ++-- micras_core/src/fsm.cpp | 10 +++++----- micras_nav/src/grid_pose.cpp | 21 +++++---------------- micras_nav/src/maze_graph.cpp | 5 ++--- 4 files changed, 14 insertions(+), 26 deletions(-) diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 95023e0..9755c21 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -10,7 +10,7 @@ #include namespace micras::core { -class FSM { +class Fsm { public: class State { public: @@ -67,7 +67,7 @@ class FSM { * * @param initial_state_id The id of the initial state. */ - explicit FSM(uint8_t initial_state_id); + explicit Fsm(uint8_t initial_state_id); /** * @brief Add a state to the FSM. diff --git a/micras_core/src/fsm.cpp b/micras_core/src/fsm.cpp index f0d1b4c..9517969 100644 --- a/micras_core/src/fsm.cpp +++ b/micras_core/src/fsm.cpp @@ -5,19 +5,19 @@ #include "micras/core/fsm.hpp" namespace micras::core { -FSM::State::State(uint8_t id) : id{id} { } +Fsm::State::State(uint8_t id) : id{id} { } -uint8_t FSM::State::get_id() const { +uint8_t Fsm::State::get_id() const { return this->id; } -FSM::FSM(uint8_t initial_state_id) : current_state_id{initial_state_id} { } +Fsm::Fsm(uint8_t initial_state_id) : current_state_id{initial_state_id} { } -void FSM::add_state(std::unique_ptr state) { +void Fsm::add_state(std::unique_ptr state) { this->states.emplace(state->get_id(), std::move(state)); } -void FSM::update() { +void Fsm::update() { if (this->current_state_id != this->previous_state_id) { this->states.at(this->current_state_id)->on_entry(); } diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index 3c664f0..e75989a 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -76,30 +76,19 @@ GridPose GridPose::turned_right() const { } Side GridPose::get_relative_side(const GridPoint& other) const { + if (((this->orientation == Side::RIGHT or this->orientation == Side::LEFT) and this->position.y == other.y) or + ((this->orientation == Side::UP or this->orientation == Side::DOWN) and this->position.x == other.x)) { + return this->position.direction(other) == this->orientation ? Side::UP : Side::DOWN; + } + switch (this->orientation) { case Side::RIGHT: - if (this->position.y == other.y) { - return this->position.x < other.x ? Side::UP : Side::DOWN; - } - return this->position.y < other.y ? Side::LEFT : Side::RIGHT; case Side::UP: - if (this->position.x == other.x) { - return this->position.y < other.y ? Side::UP : Side::DOWN; - } - return this->position.x > other.x ? Side::LEFT : Side::RIGHT; case Side::LEFT: - if (this->position.y == other.y) { - return this->position.x > other.x ? Side::UP : Side::DOWN; - } - return this->position.y > other.y ? Side::LEFT : Side::RIGHT; case Side::DOWN: - if (this->position.x == other.x) { - return this->position.y > other.y ? Side::UP : Side::DOWN; - } - return this->position.x < other.x ? Side::LEFT : Side::RIGHT; } } diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index 4477bf8..8ec4eb6 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -5,7 +5,6 @@ #include #include #include -#include "micras/nav/action_queuer.hpp" #include "micras/nav/maze_graph.hpp" @@ -72,7 +71,7 @@ void MazeGraph::add_extra_edges(const GridPose& start) { bool skipped = false; for (auto next_it = current_node.next.begin(); next_it != current_node.next.end();) { - GridPose next_pose = *next_it; + const GridPose next_pose = *next_it; for (const GridPose& next_next_pose : this->graph.at(next_pose).next) { if (current_node.next.contains(next_next_pose)) { @@ -140,7 +139,7 @@ std::list MazeGraph::get_best_route(const GridPose& start, const std:: visited.insert(current_pose); for (const GridPose& next_pose : this->graph.at(current_pose).next) { - float edge_cost = this->get_edge_cost(current_pose, next_pose); + const float edge_cost = this->get_edge_cost(current_pose, next_pose); if (distance.at(next_pose) > distance.at(current_pose) + edge_cost) { previous[next_pose] = current_pose; From 9ee46b26d8fa73d7e884b4b2e551b18dab8b4520 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 24 May 2025 03:56:01 -0300 Subject: [PATCH 434/561] =?UTF-8?q?=E2=9C=A8=20Add=20curve=20safety=20marg?= =?UTF-8?q?in=20to=20action=20queuer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Pedro de Santi --- micras_core/include/micras/core/types.hpp | 2 +- .../include/micras/nav/action_queuer.hpp | 70 +++++- .../include/micras/nav/actions/move.hpp | 37 +-- .../include/micras/nav/actions/turn.hpp | 29 ++- micras_nav/include/micras/nav/costmap.hpp | 2 +- micras_nav/src/action_queuer.cpp | 223 +++++++++++------- micras_nav/src/grid_pose.cpp | 1 + micras_proxy/include/micras/proxy/button.hpp | 6 +- micras_proxy/include/micras/proxy/fan.hpp | 2 +- micras_proxy/include/micras/proxy/imu.hpp | 2 +- micras_proxy/src/button.cpp | 8 +- 11 files changed, 249 insertions(+), 133 deletions(-) diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index 5a29bba..981df50 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -20,7 +20,7 @@ struct Observation { /** * @brief Possible objectives of the robot. */ -enum Objective : uint8_t { +enum class Objective : uint8_t { EXPLORE = 0, RETURN = 1, SOLVE = 2 diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index ba226f4..3893a1d 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -8,7 +8,6 @@ #include #include #include -#include #include "micras/nav/actions/move.hpp" #include "micras/nav/actions/turn.hpp" @@ -45,12 +44,9 @@ class ActionQueuer { float cell_size; float start_offset; + float curve_safety_margin; Dynamic exploring; Dynamic solving; - - float radius_45; - float radius_90; - float radius_135; }; /** @@ -116,6 +112,18 @@ class ActionQueuer { float forward_displacement; }; + /** + * @brief Enum for the types of curves. + */ + enum CurveType : uint8_t { + REGULAR_45 = 0, + REGULAR_90 = 1, + DIAGONAL_90 = 2, + REGULAR_135 = 3, + REGULAR_180 = 4, + NUMBER_OF_CURVES = 5, + }; + /** * @brief Get the trim distances for a turn action. * @@ -125,6 +133,34 @@ class ActionQueuer { */ std::pair get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) const; + /** + * @brief Get the curve parameters for a given angle and diagonal status. + * + * @param angle Angle of the curve. + * @param is_diagonal True if the curve is diagonal, false otherwise. + * @return Reference to the CurveParameters struct for the given angle and diagonal status. + */ + CurveParameters& get_curve_parameters(float angle, bool is_diagonal); + + /** + * @brief Compute the curve parameters for a given angle and diagonal status. + * + * @param angle Angle of the curve. + * @param is_diagonal True if the curve is diagonal, false otherwise. + */ + void compute_curve_parameters(float angle, bool is_diagonal); + + /** + * @brief Calculate the radius of the curve based on the angle, cell size, and safety margin. + * + * @param angle Angle of the curve in radians. + * @param cell_size Size of the maze cells. + * @param safety_margin Minimum distance between the robot and the wall during the curve. + * @param is_diagonal True if the curve is diagonal, false otherwise. + * @return Calculated radius of the curve. + */ + static constexpr float calculate_curve_radius(float angle, float cell_size, float safety_margin, bool is_diagonal); + /** * @brief Size of the cells in the grid. */ @@ -136,24 +172,34 @@ class ActionQueuer { float start_offset; /** - * @brief Dynamic exploring parameters. + * @brief Minimum distance between the robot and the wall during the curve. */ - Config::Dynamic exploring_params; + float curve_safety_margin; /** - * @brief Dynamic solving parameters. + * @brief Radius of the exploration curve. */ - Config::Dynamic solving_params; + float exploration_curve_radius; /** - * @brief Radius of the exploration curve. + * @brief Configuration for the exploring move action during exploration. */ - float exploration_curve_radius; + MoveAction::Config exploring_move_config; + + /** + * @brief Configuration for the turn action during exploration. + */ + TurnAction::Config exploring_turn_config; + + /** + * @brief Dynamic solving parameters. + */ + Config::Dynamic solving_params; /** * @brief Pre-computed curve parameters for different angles. */ - std::unordered_map curves_parameters; + std::array curves_parameters{}; /** * @brief Pre-built actions to use in the exploration. diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index eca5827..0011493 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -15,33 +15,40 @@ namespace micras::nav { */ class MoveAction : public Action { public: + /** + * @brief Configuration parameters for the MoveAction. + */ + struct Config { + float start_speed; + float end_speed; + float max_speed; + float max_acceleration; + float max_deceleration; + }; + /** * @brief Construct a new Move Action object. * * @param action_type The type of the action to be performed. * @param distance Distance to move in meters. - * @param start_speed Initial speed in m/s. - * @param end_speed Final speed in m/s. - * @param max_speed Maximum speed in m/s. - * @param max_acceleration Maximum acceleration in m/s^2. - * @param max_deceleration Maximum deceleration in m/s^2. + * @param config Dynamic parameters for the action. * @param follow_wall Whether the robot can follow wall while executing this action. */ - MoveAction( - uint8_t action_type, float distance, float start_speed, float end_speed, float max_speed, - float max_acceleration, float max_deceleration, bool follow_wall = true - ) : + MoveAction(uint8_t action_type, float distance, const Config& config, bool follow_wall = true) : Action{ {action_type, distance}, follow_wall, - calculate_total_time(distance, start_speed, end_speed, max_speed, max_acceleration, max_deceleration) + calculate_total_time( + distance, config.start_speed, config.end_speed, config.max_speed, config.max_acceleration, + config.max_deceleration + ) }, distance(distance), - start_speed_2(start_speed * start_speed), - end_speed_2(end_speed * end_speed), - max_speed{max_speed}, - max_acceleration_doubled{2.0F * max_acceleration}, - max_deceleration_doubled{2.0F * max_deceleration}, + start_speed_2(config.start_speed * config.start_speed), + end_speed_2(config.end_speed * config.end_speed), + max_speed{config.max_speed}, + max_acceleration_doubled{2.0F * config.max_acceleration}, + max_deceleration_doubled{2.0F * config.max_deceleration}, decelerate_distance{ (end_speed_2 - start_speed_2 + max_deceleration_doubled * distance) / (max_acceleration_doubled + max_deceleration_doubled) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index ca23ca9..0cba72c 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -16,23 +16,32 @@ namespace micras::nav { */ class TurnAction : public Action { public: + /** + * @brief Configuration parameters for the TurnAction. + */ + struct Config { + float max_angular_speed; + float linear_speed; + float max_angular_acceleration; + }; + /** * @brief Construct a new Turn Action object. * * @param action_type The type of the action to be performed. * @param angle Angle to turn in radians. - * @param max_angular_speed Maximum angular speed in rad/s. - * @param linear_speed Linear speed in m/s. - * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * @param config Dynamic parameters for the action. */ - TurnAction( - uint8_t action_type, float angle, float max_angular_speed, float linear_speed, float max_angular_acceleration - ) : - Action{{action_type, angle}, false, calculate_total_time(angle, max_angular_speed, max_angular_acceleration)}, + TurnAction(uint8_t action_type, float angle, const Config& config) : + Action{ + {action_type, angle}, + false, + calculate_total_time(angle, config.max_angular_speed, config.max_angular_acceleration) + }, angle{angle}, - max_angular_speed{max_angular_speed}, - linear_speed{linear_speed}, - acceleration{max_angular_acceleration} { } + max_angular_speed{config.max_angular_speed}, + linear_speed{config.linear_speed}, + acceleration{config.max_angular_acceleration} { } /** * @brief Get the desired speeds for the robot to complete the action. diff --git a/micras_nav/include/micras/nav/costmap.hpp b/micras_nav/include/micras/nav/costmap.hpp index 6e20b3d..2506db4 100644 --- a/micras_nav/include/micras/nav/costmap.hpp +++ b/micras_nav/include/micras/nav/costmap.hpp @@ -26,7 +26,7 @@ class Costmap { /** * @brief Type to store the state of a wall. */ - enum WallState : uint8_t { + enum class WallState : uint8_t { UNKNOWN = 0, NO_WALL = 1, WALL = 2, diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index c1dd8ad..566d40d 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -12,83 +12,46 @@ namespace micras::nav { ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, start_offset{config.start_offset}, - exploring_params{config.exploring}, - solving_params{config.solving}, + curve_safety_margin{config.curve_safety_margin}, exploration_curve_radius{ - exploring_params.max_linear_speed * exploring_params.max_linear_speed / - exploring_params.max_centrifugal_acceleration + config.exploring.max_linear_speed * config.exploring.max_linear_speed / + config.exploring.max_centrifugal_acceleration }, - stop{std::make_shared( - ActionType::STOP, cell_size / 2.0F, exploring_params.max_linear_speed, 0.0F, exploring_params.max_linear_speed, - exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false - )}, - start{std::make_shared( - ActionType::START, cell_size - config.start_offset, 0.0F, exploring_params.max_linear_speed, - exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, - exploring_params.max_linear_deceleration - )}, - move_forward{std::make_shared( - ActionType::MOVE_FORWARD, cell_size, exploring_params.max_linear_speed, exploring_params.max_linear_speed, - exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, - exploring_params.max_linear_deceleration - )}, - move_half{std::make_shared( - ActionType::MOVE_FORWARD, cell_size / 2.0F, 0.0F, exploring_params.max_linear_speed, - exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, - exploring_params.max_linear_deceleration, false - )}, - move_to_turn{std::make_shared( - ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, exploring_params.max_linear_speed, - exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, - exploring_params.max_linear_deceleration - )}, - turn_left{std::make_shared( - ActionType::TURN, std::numbers::pi_v / 2.0F, - TurnAction::calculate_max_angular_speed( - std::numbers::pi_v / 2.0F, exploration_curve_radius, exploring_params.max_angular_acceleration, - exploring_params.max_centrifugal_acceleration - ), - exploring_params.max_linear_speed, exploring_params.max_angular_acceleration - )}, - turn_right{std::make_shared( - ActionType::TURN, -std::numbers::pi_v / 2.0F, + exploring_move_config{ + config.exploring.max_linear_speed, config.exploring.max_linear_speed, config.exploring.max_linear_speed, + config.exploring.max_linear_acceleration, config.exploring.max_linear_deceleration + }, + exploring_turn_config{ TurnAction::calculate_max_angular_speed( - -std::numbers::pi_v / 2.0F, exploration_curve_radius, exploring_params.max_angular_acceleration, - exploring_params.max_centrifugal_acceleration + std::numbers::pi_v / 2.0F, exploration_curve_radius, config.exploring.max_angular_acceleration, + config.exploring.max_centrifugal_acceleration ), - exploring_params.max_linear_speed, exploring_params.max_angular_acceleration + config.exploring.max_linear_speed, config.exploring.max_angular_acceleration + }, + solving_params{config.solving}, + stop{std::make_shared(ActionType::STOP, cell_size / 2.0F, exploring_move_config, false)}, + start{std::make_shared(ActionType::START, cell_size - config.start_offset, exploring_move_config)}, + move_forward{std::make_shared(ActionType::MOVE_FORWARD, cell_size, exploring_move_config)}, + move_half{std::make_shared(ActionType::MOVE_FORWARD, cell_size / 2.0F, exploring_move_config, false)}, + move_to_turn{std::make_shared( + ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, exploring_move_config )}, + turn_left{std::make_shared(ActionType::TURN, std::numbers::pi_v / 2.0F, exploring_turn_config)}, + turn_right{std::make_shared(ActionType::TURN, -std::numbers::pi_v / 2.0F, exploring_turn_config) + }, turn_back{std::make_shared( ActionType::SPIN, std::numbers::pi_v, - TurnAction::calculate_max_angular_speed( - std::numbers::pi_v, 0.0F, exploring_params.max_angular_acceleration, - exploring_params.max_centrifugal_acceleration - ), - 0.0F, exploring_params.max_angular_acceleration + TurnAction::Config{ + .max_angular_speed = config.exploring.max_angular_acceleration * 0.01F, + .linear_speed = 0.0F, + .max_angular_acceleration = config.exploring.max_angular_acceleration, + } )} { - for (const auto& [angle, curve_radius] : { - std::pair{std::numbers::pi_v / 4.0F, config.radius_45}, - std::pair{std::numbers::pi_v / 2.0F, config.radius_90}, - std::pair{3.0F * std::numbers::pi_v / 4.0F, config.radius_135}, - std::pair{std::numbers::pi_v, this->cell_size / 2.0F}, - }) { - const float max_angular_speed = TurnAction::calculate_max_angular_speed( - angle, curve_radius, config.solving.max_angular_acceleration, config.solving.max_centrifugal_acceleration - ); - const float curve_linear_speed = this->solving_params.max_centrifugal_acceleration / max_angular_speed; - - this->curves_parameters[angle] = { - .curve_radius = curve_radius, - .max_angular_speed = max_angular_speed, - .linear_speed = curve_linear_speed, - .side_displacement = TurnAction::calculate_side_displacement( - angle, max_angular_speed, curve_linear_speed, config.solving.max_angular_acceleration - ), - .forward_displacement = TurnAction::calculate_forward_displacement( - angle, max_angular_speed, curve_linear_speed, config.solving.max_angular_acceleration - ), - }; - } + this->compute_curve_parameters(std::numbers::pi_v / 4.0F, false); + this->compute_curve_parameters(std::numbers::pi_v / 2.0F, false); + this->compute_curve_parameters(std::numbers::pi_v / 2.0F, true); + this->compute_curve_parameters(3.0F * std::numbers::pi_v / 4.0F, false); + this->compute_curve_parameters(std::numbers::pi_v, false); } void ActionQueuer::push_exploring(const GridPose& origin_pose, const GridPoint& target_position) { @@ -173,31 +136,46 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta const auto& curve_parameters = this->curves_parameters.at(std::abs(action_it->value)); + const MoveAction::Config move_config = { + .start_speed = start_speed, + .end_speed = curve_parameters.linear_speed, + .max_speed = this->solving_params.max_linear_speed, + .max_acceleration = this->solving_params.max_linear_acceleration, + .max_deceleration = this->solving_params.max_linear_deceleration + }; + + const TurnAction::Config turn_config = { + .max_angular_speed = curve_parameters.max_angular_speed, + .linear_speed = curve_parameters.linear_speed, + .max_angular_acceleration = this->solving_params.max_angular_acceleration + }; + this->action_queue.emplace_back(std::make_shared( - std::prev(action_it)->type, std::prev(action_it)->value, start_speed, curve_parameters.linear_speed, - this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, - this->solving_params.max_linear_deceleration, std::prev(action_it)->type != ActionType::DIAGONAL + std::prev(action_it)->type, std::prev(action_it)->value, move_config, + std::prev(action_it)->type != ActionType::DIAGONAL )); - this->action_queue.emplace_back(std::make_shared( - action_it->type, action_it->value, curve_parameters.max_angular_speed, curve_parameters.linear_speed, - this->solving_params.max_angular_acceleration - )); + this->action_queue.emplace_back(std::make_shared(action_it->type, action_it->value, turn_config)); start_speed = curve_parameters.linear_speed; } + const MoveAction::Config stop_config = { + .start_speed = start_speed, + .end_speed = 0.0F, + .max_speed = this->solving_params.max_linear_speed, + .max_acceleration = this->solving_params.max_linear_acceleration, + .max_deceleration = this->solving_params.max_linear_deceleration + }; + if (actions.back().type == ActionType::MOVE_FORWARD) { - this->action_queue.emplace_back(std::make_shared( - ActionType::STOP, actions.back().value + this->cell_size / 2.0F, start_speed, 0.0F, - this->solving_params.max_linear_speed, this->solving_params.max_linear_acceleration, - this->solving_params.max_linear_deceleration - )); + this->action_queue.emplace_back( + std::make_shared(ActionType::STOP, actions.back().value + this->cell_size / 2.0F, stop_config) + ); } else { - this->action_queue.emplace_back(std::make_shared( - ActionType::STOP, this->cell_size / 2.0F, start_speed, 0.0F, this->solving_params.max_linear_speed, - this->solving_params.max_linear_acceleration, this->solving_params.max_linear_deceleration - )); + this->action_queue.emplace_back( + std::make_shared(ActionType::STOP, this->cell_size / 2.0F, stop_config) + ); } } @@ -319,6 +297,81 @@ std::pair return {trim_before_distance, trim_after_distance}; } + if (turn_angle == std::numbers::pi_v) { + const float trim_distance = this->cell_size / 2.0F - this->curve_safety_margin; + + return {trim_distance, trim_distance}; + } + return {0.0F, 0.0F}; } + +ActionQueuer::CurveParameters& ActionQueuer::get_curve_parameters(float angle, bool is_diagonal) { + const float turn_angle = std::abs(angle); + + if (turn_angle == std::numbers::pi_v / 4.0F) { + return this->curves_parameters[CurveType::REGULAR_45]; + } + + if (turn_angle == std::numbers::pi_v / 2.0F) { + return is_diagonal ? this->curves_parameters[CurveType::DIAGONAL_90] : + this->curves_parameters[CurveType::REGULAR_90]; + } + + if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { + return this->curves_parameters[CurveType::REGULAR_135]; + } + + return this->curves_parameters[CurveType::REGULAR_180]; +} + +void ActionQueuer::compute_curve_parameters(float angle, bool is_diagonal) { + const float curve_radius = calculate_curve_radius(angle, this->cell_size, this->curve_safety_margin, is_diagonal); + const float max_angular_speed = TurnAction::calculate_max_angular_speed( + angle, curve_radius, this->solving_params.max_angular_acceleration, + this->solving_params.max_centrifugal_acceleration + ); + const float curve_linear_speed = this->solving_params.max_centrifugal_acceleration / max_angular_speed; + + this->get_curve_parameters(angle, is_diagonal) = { + .curve_radius = curve_radius, + .max_angular_speed = max_angular_speed, + .linear_speed = curve_linear_speed, + .side_displacement = TurnAction::calculate_side_displacement( + angle, max_angular_speed, curve_linear_speed, this->solving_params.max_angular_acceleration + ), + .forward_displacement = TurnAction::calculate_forward_displacement( + angle, max_angular_speed, curve_linear_speed, this->solving_params.max_angular_acceleration + ), + }; +} + +constexpr float + ActionQueuer::calculate_curve_radius(float angle, float cell_size, float safety_margin, bool is_diagonal) { + const float turn_angle = std::abs(angle); + + if (turn_angle == std::numbers::pi_v / 4.0F) { + return (1.0F + std::numbers::sqrt2_v)*cell_size / 2.0F; + } + + if (turn_angle == std::numbers::pi_v / 2.0F) { + const float curve_radius = (cell_size - 2.0F * safety_margin) / (2.0F * std::numbers::sqrt2_v - 2.0F); + + if (is_diagonal) { + return curve_radius; + } + + return curve_radius + cell_size / 2.0F; + } + + if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { + const float constant = 10.0F * std::numbers::sqrt2_v - 14.0F; + const float root_term = + (std::sqrt(constant * (cell_size * cell_size - 2.0F * cell_size * safety_margin)) + cell_size) / 2.0F; + + return root_term - safety_margin * (std::numbers::sqrt2_v - 1.0F); + } + + return cell_size / 2.0F; +} } // namespace micras::nav diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index e75989a..da36ddd 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -89,6 +89,7 @@ Side GridPose::get_relative_side(const GridPoint& other) const { case Side::LEFT: return this->position.y > other.y ? Side::LEFT : Side::RIGHT; case Side::DOWN: + default: return this->position.x < other.x ? Side::LEFT : Side::RIGHT; } } diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index b2120d8..1e349fa 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -19,7 +19,7 @@ class Button { /** * @brief Enum for button status. */ - enum Status : uint8_t { + enum class Status : uint8_t { NO_PRESS = 0, SHORT_PRESS = 1, LONG_PRESS = 2, @@ -29,7 +29,7 @@ class Button { /** * @brief Enum for button pull resistor. */ - enum PullResistor : uint8_t { + enum class PullResistor : uint8_t { PULL_UP = 0, PULL_DOWN = 1, }; @@ -145,7 +145,7 @@ class Button { /** * @brief Current status of the button. */ - Status current_status{NO_PRESS}; + Status current_status{Status::NO_PRESS}; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index c452468..d2b327f 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -65,7 +65,7 @@ class Fan { /** * @brief Enum for rotation direction. */ - enum RotationDirection : uint8_t { + enum class RotationDirection : uint8_t { FORWARD = 0, BACKWARDS = 1 }; diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 7c296b0..4c6038c 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -36,7 +36,7 @@ class Imu { /** * @brief Enum to select the axis of the IMU. */ - enum Axis : uint8_t { + enum class Axis : uint8_t { X = 0, Y = 1, Z = 2 diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp index 86b9e45..5e9473b 100644 --- a/micras_proxy/src/button.cpp +++ b/micras_proxy/src/button.cpp @@ -28,20 +28,20 @@ void Button::update() { this->status_stopwatch.reset_ms(); } else if (this->is_falling_edge()) { if (this->status_stopwatch.elapsed_time_ms() > extra_long_press_delay) { - this->current_status = EXTRA_LONG_PRESS; + this->current_status = Status::EXTRA_LONG_PRESS; return; } if (this->status_stopwatch.elapsed_time_ms() > long_press_delay) { - this->current_status = LONG_PRESS; + this->current_status = Status::LONG_PRESS; return; } - this->current_status = SHORT_PRESS; + this->current_status = Status::SHORT_PRESS; return; } - this->current_status = NO_PRESS; + this->current_status = Status::NO_PRESS; } bool Button::get_raw_reading() const { From 95d7abe243a8db43e9f55c8aabaef7fa5802b106 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 24 May 2025 04:38:08 -0300 Subject: [PATCH 435/561] =?UTF-8?q?=E2=99=BF=EF=B8=8F=20Improve=20action?= =?UTF-8?q?=20queuer=20constructor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 15 ---- micras_nav/src/action_queuer.cpp | 74 +++++++++++-------- 2 files changed, 43 insertions(+), 46 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 3893a1d..7501ea9 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -176,21 +176,6 @@ class ActionQueuer { */ float curve_safety_margin; - /** - * @brief Radius of the exploration curve. - */ - float exploration_curve_radius; - - /** - * @brief Configuration for the exploring move action during exploration. - */ - MoveAction::Config exploring_move_config; - - /** - * @brief Configuration for the turn action during exploration. - */ - TurnAction::Config exploring_turn_config; - /** * @brief Dynamic solving parameters. */ diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 566d40d..53967c6 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -13,40 +13,52 @@ ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, start_offset{config.start_offset}, curve_safety_margin{config.curve_safety_margin}, - exploration_curve_radius{ - config.exploring.max_linear_speed * config.exploring.max_linear_speed / - config.exploring.max_centrifugal_acceleration - }, - exploring_move_config{ - config.exploring.max_linear_speed, config.exploring.max_linear_speed, config.exploring.max_linear_speed, - config.exploring.max_linear_acceleration, config.exploring.max_linear_deceleration - }, - exploring_turn_config{ - TurnAction::calculate_max_angular_speed( + solving_params{config.solving} { + const float exploration_curve_radius = config.exploring.max_linear_speed * config.exploring.max_linear_speed / + config.exploring.max_centrifugal_acceleration; + + MoveAction::Config move_config = { + .start_speed = 0.0F, + .end_speed = config.exploring.max_linear_speed, + .max_speed = config.exploring.max_linear_speed, + .max_acceleration = config.exploring.max_linear_acceleration, + .max_deceleration = config.exploring.max_linear_deceleration + }; + + this->start = std::make_shared(ActionType::START, this->cell_size - config.start_offset, move_config); + this->move_half = std::make_shared(ActionType::MOVE_FORWARD, cell_size / 2.0F, move_config); + + move_config.start_speed = config.exploring.max_linear_speed; + + this->move_forward = std::make_shared(ActionType::MOVE_FORWARD, cell_size, move_config); + this->move_to_turn = std::make_shared( + ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config + ); + + move_config.end_speed = 0.0F; + + this->stop = std::make_shared(ActionType::STOP, cell_size / 2.0F, move_config); + + const TurnAction::Config turn_config = { + .max_angular_speed = TurnAction::calculate_max_angular_speed( std::numbers::pi_v / 2.0F, exploration_curve_radius, config.exploring.max_angular_acceleration, config.exploring.max_centrifugal_acceleration ), - config.exploring.max_linear_speed, config.exploring.max_angular_acceleration - }, - solving_params{config.solving}, - stop{std::make_shared(ActionType::STOP, cell_size / 2.0F, exploring_move_config, false)}, - start{std::make_shared(ActionType::START, cell_size - config.start_offset, exploring_move_config)}, - move_forward{std::make_shared(ActionType::MOVE_FORWARD, cell_size, exploring_move_config)}, - move_half{std::make_shared(ActionType::MOVE_FORWARD, cell_size / 2.0F, exploring_move_config, false)}, - move_to_turn{std::make_shared( - ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, exploring_move_config - )}, - turn_left{std::make_shared(ActionType::TURN, std::numbers::pi_v / 2.0F, exploring_turn_config)}, - turn_right{std::make_shared(ActionType::TURN, -std::numbers::pi_v / 2.0F, exploring_turn_config) - }, - turn_back{std::make_shared( - ActionType::SPIN, std::numbers::pi_v, - TurnAction::Config{ - .max_angular_speed = config.exploring.max_angular_acceleration * 0.01F, - .linear_speed = 0.0F, - .max_angular_acceleration = config.exploring.max_angular_acceleration, - } - )} { + .linear_speed = config.exploring.max_linear_speed, + .max_angular_acceleration = config.exploring.max_angular_acceleration + }; + + this->turn_left = std::make_shared(ActionType::TURN, std::numbers::pi_v / 2.0F, turn_config); + this->turn_right = std::make_shared(ActionType::TURN, -std::numbers::pi_v / 2.0F, turn_config); + + const TurnAction::Config turn_back_config = { + .max_angular_speed = config.exploring.max_angular_acceleration * 0.01F, + .linear_speed = 0.0F, + .max_angular_acceleration = config.exploring.max_angular_acceleration + }; + + this->turn_back = std::make_shared(ActionType::SPIN, std::numbers::pi_v, turn_back_config); + this->compute_curve_parameters(std::numbers::pi_v / 4.0F, false); this->compute_curve_parameters(std::numbers::pi_v / 2.0F, false); this->compute_curve_parameters(std::numbers::pi_v / 2.0F, true); From 342612dd2d99d9cc1b55a663333fe5e059347e78 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 24 May 2025 04:53:35 -0300 Subject: [PATCH 436/561] =?UTF-8?q?=F0=9F=90=9B=20Disable=20follow=20wall?= =?UTF-8?q?=20on=20actions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 53967c6..fcb6eab 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -25,14 +25,15 @@ ActionQueuer::ActionQueuer(Config config) : .max_deceleration = config.exploring.max_linear_deceleration }; - this->start = std::make_shared(ActionType::START, this->cell_size - config.start_offset, move_config); - this->move_half = std::make_shared(ActionType::MOVE_FORWARD, cell_size / 2.0F, move_config); + this->start = + std::make_shared(ActionType::START, this->cell_size - config.start_offset, move_config, false); + this->move_half = std::make_shared(ActionType::MOVE_FORWARD, cell_size / 2.0F, move_config, false); move_config.start_speed = config.exploring.max_linear_speed; this->move_forward = std::make_shared(ActionType::MOVE_FORWARD, cell_size, move_config); this->move_to_turn = std::make_shared( - ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config + ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config, false ); move_config.end_speed = 0.0F; From 56b0439c06096e1f9eb0dfe16ff496a648727f65 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 24 May 2025 05:02:24 -0300 Subject: [PATCH 437/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Add=20move=5Ffrom?= =?UTF-8?q?=5Fturn=20action?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 1 + micras_nav/src/action_queuer.cpp | 23 +++++++++++-------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 7501ea9..84850ce 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -195,6 +195,7 @@ class ActionQueuer { std::shared_ptr move_forward; std::shared_ptr move_half; std::shared_ptr move_to_turn; + std::shared_ptr move_from_turn; std::shared_ptr turn_left; std::shared_ptr turn_right; std::shared_ptr turn_back; diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index fcb6eab..de94444 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -33,6 +33,9 @@ ActionQueuer::ActionQueuer(Config config) : this->move_forward = std::make_shared(ActionType::MOVE_FORWARD, cell_size, move_config); this->move_to_turn = std::make_shared( + ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config + ); + this->move_from_turn = std::make_shared( ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config, false ); @@ -69,28 +72,28 @@ ActionQueuer::ActionQueuer(Config config) : void ActionQueuer::push_exploring(const GridPose& origin_pose, const GridPoint& target_position) { if (origin_pose.front().position == target_position) { - this->action_queue.emplace_back(move_forward); + this->action_queue.emplace_back(this->move_forward); return; } if (origin_pose.turned_left().front().position == target_position) { - this->action_queue.emplace_back(move_to_turn); - this->action_queue.emplace_back(turn_left); - this->action_queue.emplace_back(move_to_turn); + this->action_queue.emplace_back(this->move_to_turn); + this->action_queue.emplace_back(this->turn_left); + this->action_queue.emplace_back(this->move_to_turn); return; } if (origin_pose.turned_right().front().position == target_position) { - this->action_queue.emplace_back(move_to_turn); - this->action_queue.emplace_back(turn_right); - this->action_queue.emplace_back(move_to_turn); + this->action_queue.emplace_back(this->move_to_turn); + this->action_queue.emplace_back(this->turn_right); + this->action_queue.emplace_back(this->move_from_turn); return; } if (origin_pose.turned_back().front().position == target_position) { - this->action_queue.emplace_back(stop); - this->action_queue.emplace_back(turn_back); - this->action_queue.emplace_back(move_half); + this->action_queue.emplace_back(this->stop); + this->action_queue.emplace_back(this->turn_back); + this->action_queue.emplace_back(this->move_half); return; } } From dfd4f080e7bf4527acdfd048038c527fc9076ba8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 24 May 2025 05:15:52 -0300 Subject: [PATCH 438/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20curve=20parameters?= =?UTF-8?q?=20access?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/action_queuer.hpp | 2 +- micras_nav/src/action_queuer.cpp | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 84850ce..a822a32 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -131,7 +131,7 @@ class ActionQueuer { * @param turn_action Turn action to get the trim distances for. * @return Pair of trim distances before and after the turn. */ - std::pair get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) const; + std::pair get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action); /** * @brief Get the curve parameters for a given angle and diagonal status. diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index de94444..a567c71 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -150,7 +150,8 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta continue; } - const auto& curve_parameters = this->curves_parameters.at(std::abs(action_it->value)); + const CurveParameters& curve_parameters = + this->get_curve_parameters(action_it->value, std::prev(action_it)->type == ActionType::DIAGONAL); const MoveAction::Config move_config = { .start_speed = start_speed, @@ -275,9 +276,10 @@ std::list } std::pair - ActionQueuer::get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) const { - const float turn_angle = std::abs(turn_action.value); - const auto& curve_parameters = this->curves_parameters.at(turn_angle); + ActionQueuer::get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) { + const float turn_angle = std::abs(turn_action.value); + const CurveParameters& curve_parameters = + this->get_curve_parameters(turn_angle, action_before.type == ActionType::DIAGONAL); if (turn_angle == std::numbers::pi_v / 4.0F) { const float trim_before_distance = curve_parameters.forward_displacement - curve_parameters.side_displacement; From 06aed305a4802a397f148ed4551fdaed18fe9c3b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 24 May 2025 05:46:55 -0300 Subject: [PATCH 439/561] =?UTF-8?q?=F0=9F=90=9B=20Can't=20skip=20using=201?= =?UTF-8?q?80=20turn?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/maze_graph.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp index 8ec4eb6..036e027 100644 --- a/micras_nav/src/maze_graph.cpp +++ b/micras_nav/src/maze_graph.cpp @@ -166,7 +166,7 @@ void MazeGraph::reset() { } bool MazeGraph::can_skip(const GridPose& first, const GridPose& second, const GridPose& third) const { - auto skip_action = this->get_main_action_type(first, third); + ActionQueuer::ActionType skip_action = this->get_main_action_type(first, third); if (skip_action == ActionQueuer::ActionType::STOP) { return false; @@ -212,14 +212,15 @@ float MazeGraph::get_edge_cost(const GridPose& from, const GridPose& to) const { ActionQueuer::ActionType MazeGraph::get_main_action_type(const GridPose& from, const GridPose& to) const { auto actions = ActionQueuer::get_actions(from, to, this->cell_size); - if (actions.empty()) { - return ActionQueuer::ActionType::STOP; - } - if (actions.size() == 3) { return ActionQueuer::ActionType::DIAGONAL; } + if (actions.empty() or (actions.front().type == ActionQueuer::ActionType::TURN and + std::abs(actions.front().value) == std::numbers::pi_v)) { + return ActionQueuer::ActionType::STOP; + } + return static_cast(actions.front().type); } } // namespace micras::nav From fc5f054fccb66ba0efe1cf0dc4b14ee22b5859ff Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 24 May 2025 19:49:35 -0300 Subject: [PATCH 440/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Add=20first=20run?= =?UTF-8?q?=20flag=20to=20pid?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/pid_controller.hpp | 5 +++++ micras_core/src/pid_controller.cpp | 6 ++++++ 2 files changed, 11 insertions(+) diff --git a/micras_core/include/micras/core/pid_controller.hpp b/micras_core/include/micras/core/pid_controller.hpp index 5a62612..6452847 100644 --- a/micras_core/include/micras/core/pid_controller.hpp +++ b/micras_core/include/micras/core/pid_controller.hpp @@ -94,6 +94,11 @@ class PidController { */ float max_integral; + /** + * @brief Flag indicating whether this is the first run of the controller. + */ + bool first_run = true; + /** * @brief Accumulated error for integrative term. */ diff --git a/micras_core/src/pid_controller.cpp b/micras_core/src/pid_controller.cpp index baa44c6..ac20ac7 100644 --- a/micras_core/src/pid_controller.cpp +++ b/micras_core/src/pid_controller.cpp @@ -33,9 +33,15 @@ void PidController::reset() { this->error_acc = 0; this->prev_state = 0; this->last_response = 0; + this->first_run = true; } float PidController::compute_response(float state, float elapsed_time, bool save) { + if (first_run) { + this->prev_state = state; + this->first_run = false; + } + const float state_change = (state - this->prev_state) / elapsed_time; return this->compute_response(state, elapsed_time, state_change, save); } From b7911d6d658a70e62e3327bade1c4813c38202ba Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 24 May 2025 20:46:08 -0300 Subject: [PATCH 441/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Clamp=20follow=20w?= =?UTF-8?q?all=20response?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_core/include/micras/core/utils.hpp | 2 +- micras_nav/include/micras/nav/follow_wall.hpp | 11 +++++++++++ micras_nav/src/follow_wall.cpp | 9 +++++++-- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index 0553c1f..ca788ac 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -38,7 +38,7 @@ constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) { */ template constexpr T move_towards(T value, T target, T step) { - return std::max(std::min(value + step, target), value - step); + return std::clamp(target, value - step, value + step); } /** diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 715c493..7f7a9e3 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -25,6 +25,7 @@ class FollowWall { core::PidController::Config pid; core::WallSensorsIndex wall_sensor_index{}; float max_linear_speed{}; + float max_angular_acceleration{}; float post_threshold{}; float cell_size{}; float post_clearance{}; @@ -114,6 +115,11 @@ class FollowWall { */ float max_linear_speed; + /** + * @brief Maximum angular acceleration of the robot. + */ + float max_angular_acceleration; + /** * @brief Derivative threshold for detecting posts. */ @@ -159,6 +165,11 @@ class FollowWall { */ float last_right_error{}; + /** + * @brief Last response returned by the Follow Wall. + */ + float last_response{}; + /** * @brief Flag to indicate if relative pose was reset by a post. */ diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index a5d13eb..f9703d3 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -12,6 +12,7 @@ FollowWall::FollowWall( pid{config.pid}, sensor_index{config.wall_sensor_index}, max_linear_speed{config.max_linear_speed}, + max_angular_acceleration{config.max_angular_acceleration}, post_threshold{config.post_threshold}, blind_pose{absolute_pose}, cell_size{config.cell_size}, @@ -44,9 +45,12 @@ float FollowWall::compute_angular_correction(float elapsed_time, float linear_sp return 0.0F; } - const float response = this->pid.compute_response(error, elapsed_time); + const float response = linear_speed * this->pid.compute_response(error, elapsed_time) / this->max_linear_speed; + const float clamped_response = + core::move_towards(this->last_response, response, this->max_angular_acceleration * elapsed_time); - return response * linear_speed / this->max_linear_speed; + this->last_response = clamped_response; + return clamped_response; } bool FollowWall::check_posts() { @@ -104,6 +108,7 @@ core::Observation FollowWall::get_observation() const { void FollowWall::reset() { this->pid.reset(); + this->last_response = 0.0F; this->reset_displacement(); this->following_left = this->wall_sensors->get_wall(this->sensor_index.left); this->following_right = this->wall_sensors->get_wall(this->sensor_index.right); From 255712034b7ca7fb08515b8b087890af51dc8239 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 25 May 2025 11:41:48 -0300 Subject: [PATCH 442/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Fix=20exploration?= =?UTF-8?q?=20linear=20speed=20on=20action=20queuer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 32 ++++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index a567c71..316b626 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -17,38 +17,42 @@ ActionQueuer::ActionQueuer(Config config) : const float exploration_curve_radius = config.exploring.max_linear_speed * config.exploring.max_linear_speed / config.exploring.max_centrifugal_acceleration; + const float exploration_max_angular_speed = TurnAction::calculate_max_angular_speed( + std::numbers::pi_v / 2.0F, exploration_curve_radius, config.exploring.max_angular_acceleration, + config.exploring.max_centrifugal_acceleration + ); + + const float exploration_linear_speed = + config.exploring.max_centrifugal_acceleration / exploration_max_angular_speed; + MoveAction::Config move_config = { .start_speed = 0.0F, - .end_speed = config.exploring.max_linear_speed, - .max_speed = config.exploring.max_linear_speed, + .end_speed = exploration_linear_speed, + .max_speed = exploration_linear_speed, .max_acceleration = config.exploring.max_linear_acceleration, .max_deceleration = config.exploring.max_linear_deceleration }; - this->start = - std::make_shared(ActionType::START, this->cell_size - config.start_offset, move_config, false); - this->move_half = std::make_shared(ActionType::MOVE_FORWARD, cell_size / 2.0F, move_config, false); + this->start = std::make_shared(ActionType::START, this->cell_size - config.start_offset, move_config); + this->move_half = std::make_shared(ActionType::MOVE_FORWARD, cell_size / 2.0F, move_config); - move_config.start_speed = config.exploring.max_linear_speed; + move_config.start_speed = exploration_linear_speed; this->move_forward = std::make_shared(ActionType::MOVE_FORWARD, cell_size, move_config); this->move_to_turn = std::make_shared( - ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config + ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config, false ); this->move_from_turn = std::make_shared( - ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config, false + ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config ); move_config.end_speed = 0.0F; - this->stop = std::make_shared(ActionType::STOP, cell_size / 2.0F, move_config); + this->stop = std::make_shared(ActionType::STOP, cell_size / 2.0F, move_config, false); const TurnAction::Config turn_config = { - .max_angular_speed = TurnAction::calculate_max_angular_speed( - std::numbers::pi_v / 2.0F, exploration_curve_radius, config.exploring.max_angular_acceleration, - config.exploring.max_centrifugal_acceleration - ), - .linear_speed = config.exploring.max_linear_speed, + .max_angular_speed = exploration_max_angular_speed, + .linear_speed = exploration_linear_speed, .max_angular_acceleration = config.exploring.max_angular_acceleration }; From 6f8e63065f2ef1e54281d4514ab3226d0cfa8e2d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 25 May 2025 13:36:33 -0300 Subject: [PATCH 443/561] =?UTF-8?q?=E2=9C=A8=20Clear=20the=20odometry=20po?= =?UTF-8?q?sitional=20error=20from=20the=20follow=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 66 ++++++------- micras_nav/include/micras/nav/grid_pose.hpp | 2 +- micras_nav/include/micras/nav/odometry.hpp | 7 ++ micras_nav/src/follow_wall.cpp | 92 ++++++++++--------- micras_nav/src/odometry.cpp | 4 + 5 files changed, 95 insertions(+), 76 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 7f7a9e3..7894516 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -26,30 +26,28 @@ class FollowWall { core::WallSensorsIndex wall_sensor_index{}; float max_linear_speed{}; float max_angular_acceleration{}; - float post_threshold{}; float cell_size{}; + float post_threshold{}; float post_clearance{}; + float post_reference{}; }; /** * @brief Construct a new FollowWall object. * * @param wall_sensors The wall sensors of the robot. - * @param absolute_pose Reference to the absolute pose of the robot. * @param config The configuration for the FollowWall class. */ - FollowWall( - const std::shared_ptr>& wall_sensors, const Pose& absolute_pose, const Config& config - ); + FollowWall(const std::shared_ptr>& wall_sensors, const Config& config); /** * @brief Calculate the desired angular speed to follow wall. * * @param elapsed_time The time elapsed since the last update. - * @param linear_speed Current linear speed of the robot. + * @param state The current state of the robot. * @return The desired angular speed to follow wall. */ - float compute_angular_correction(float elapsed_time, float linear_speed); + float compute_angular_correction(float elapsed_time, State& state); /** * @brief Get the observation of the walls around the robot. @@ -72,28 +70,30 @@ class FollowWall { */ bool get_is_following_right() const; - /** - * @brief Reset the PID controller and the relative pose. - */ - void reset(); - private: - /** - * @brief Reset the displacement of the robot. - * - * @param reset_by_post Whether the reset was triggered by a post. - */ - void reset_displacement(bool reset_by_post = false); - /** * @brief Check if the robot saw a post. * + * @param cell_advance The distance of the robot from the start of the cell. * @return True if the robot saw a post, false otherwise. * * @details This function uses the derivative of the distance sensors readings with respect * to the robot's traveled distance, comparing it to a threshold to detect posts. */ - bool check_posts(); + bool check_posts(float cell_advance); + + /** + * @brief Reset the PID controller and the relative pose. + */ + void reset(); + + /** + * @brief Clear the positional error of the robot. + * + * @param state The state of the robot. + * @param error The error to be cleared. + */ + static void clear_position_error(State& state, float error); /** * @brief Wall sensors of the robot. @@ -120,30 +120,35 @@ class FollowWall { */ float max_angular_acceleration; + /** + * @brief Size of the cells in the map. + */ + float cell_size; + /** * @brief Derivative threshold for detecting posts. */ float post_threshold; /** - * @brief Current pose of the robot relative to the last time it was reset. + * @brief Margin in front of a post to stop seeing it. */ - RelativePose blind_pose; + float post_clearance; /** - * @brief Last distance of the robot to the start point, used to compute the derivative of the distance sensors. + * @brief Cell advance of the robot when it sees a post. */ - float last_blind_distance{}; + float post_reference; /** - * @brief Size of the cells in the map. + * @brief Last grid pose of the robot. */ - float cell_size; + GridPose last_grid_pose{}; /** - * @brief Margin in front of a post to stop seeing it. + * @brief Last cell advance of the robot. */ - float post_clearance; + float last_cell_advance{}; /** * @brief Flag to indicate if the robot is currently following the left wall. @@ -169,11 +174,6 @@ class FollowWall { * @brief Last response returned by the Follow Wall. */ float last_response{}; - - /** - * @brief Flag to indicate if relative pose was reset by a post. - */ - bool reset_by_post{}; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index 00fefe6..54c2779 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -44,6 +44,7 @@ struct GridPoint { /** * @brief Convert the point to a grid point. * + * @param point The point to convert. * @param cell_size The size of the grid cells. * @return The grid point corresponding to the point. */ @@ -52,7 +53,6 @@ struct GridPoint { /** * @brief Convert a grid point to a point. * - * @param grid_point The grid point to convert. * @param cell_size The size of the grid cells. * @return The point corresponding to the grid point. */ diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 6b3352a..ef06a0d 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -60,6 +60,13 @@ class Odometry { */ const State& get_state() const; + /** + * @brief Get the state of the robot. + * + * @return Current state of the robot in space. + */ + State& get_state(); + /** * @brief Set the state of the robot. * diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index f9703d3..2ab8833 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -5,31 +5,42 @@ #include "micras/nav/follow_wall.hpp" namespace micras::nav { -FollowWall::FollowWall( - const std::shared_ptr>& wall_sensors, const Pose& absolute_pose, const Config& config -) : +FollowWall::FollowWall(const std::shared_ptr>& wall_sensors, const Config& config) : wall_sensors{wall_sensors}, pid{config.pid}, sensor_index{config.wall_sensor_index}, max_linear_speed{config.max_linear_speed}, max_angular_acceleration{config.max_angular_acceleration}, - post_threshold{config.post_threshold}, - blind_pose{absolute_pose}, cell_size{config.cell_size}, - post_clearance{config.post_clearance} { } + post_threshold{config.post_threshold}, + post_clearance{config.post_clearance}, + post_reference{config.post_reference} { } + +float FollowWall::compute_angular_correction(float elapsed_time, State& state) { + float cell_advance = state.pose.to_cell(this->cell_size).y; + const GridPose grid_pose = state.pose.to_grid(this->cell_size); -float FollowWall::compute_angular_correction(float elapsed_time, float linear_speed) { if (this->wall_sensors.use_count() == 1) { this->wall_sensors->update(); } - if (this->check_posts()) { - return 0.0F; + if (grid_pose == this->last_grid_pose) { + if (this->check_posts(cell_advance)) { + clear_position_error(state, this->post_reference - cell_advance); + cell_advance = this->post_reference; + } + } else { + if (grid_pose != this->last_grid_pose.front()) { + this->reset(); + } } - if ((not this->following_left or not this->following_right) and - (this->last_blind_distance >= (this->cell_size + (this->reset_by_post ? this->post_clearance : 0.0F)))) { - this->reset(); + this->last_grid_pose = grid_pose; + this->last_cell_advance = cell_advance; + + if (std::abs(cell_advance - this->cell_size / 2.0F) < this->cell_size / 2.0F - this->post_clearance) { + this->following_left |= this->wall_sensors->get_wall(this->sensor_index.left); + this->following_right |= this->wall_sensors->get_wall(this->sensor_index.right); } float error{}; @@ -45,7 +56,8 @@ float FollowWall::compute_angular_correction(float elapsed_time, float linear_sp return 0.0F; } - const float response = linear_speed * this->pid.compute_response(error, elapsed_time) / this->max_linear_speed; + const float response = + state.velocity.linear * this->pid.compute_response(error, elapsed_time) / this->max_linear_speed; const float clamped_response = core::move_towards(this->last_response, response, this->max_angular_acceleration * elapsed_time); @@ -53,38 +65,24 @@ float FollowWall::compute_angular_correction(float elapsed_time, float linear_sp return clamped_response; } -bool FollowWall::check_posts() { - const float current_distance = this->blind_pose.get().position.magnitude(); - const float delta_distance = current_distance - this->last_blind_distance; +bool FollowWall::check_posts(float cell_advance) { + const float delta_distance = cell_advance - this->last_cell_advance; if (delta_distance <= 0.0F) { return false; } - this->last_blind_distance = current_distance; bool found_posts = false; - if (this->following_left and - -(this->wall_sensors->get_sensor_error(this->sensor_index.left) - this->last_left_error) / delta_distance >= - this->post_threshold) { + if (-(this->wall_sensors->get_sensor_error(this->sensor_index.left) - this->last_left_error) / delta_distance >= + this->post_threshold) { this->following_left = false; - - if (this->following_right) { - this->reset_displacement(true); - } - found_posts = true; } - if (this->following_right and - -(this->wall_sensors->get_sensor_error(this->sensor_index.right) - this->last_right_error) / delta_distance >= - this->post_threshold) { + if (-(this->wall_sensors->get_sensor_error(this->sensor_index.right) - this->last_right_error) / delta_distance >= + this->post_threshold) { this->following_right = false; - - if (this->following_left) { - this->reset_displacement(true); - } - found_posts = true; } @@ -106,18 +104,28 @@ core::Observation FollowWall::get_observation() const { }; } +void FollowWall::clear_position_error(State& state, float error) { + switch (angle_to_grid(state.pose.orientation)) { + case Side::RIGHT: + state.pose.position.x += error; + break; + case Side::UP: + state.pose.position.y += error; + break; + case Side::LEFT: + state.pose.position.x -= error; + break; + case Side::DOWN: + state.pose.position.y -= error; + break; + } +} + void FollowWall::reset() { this->pid.reset(); this->last_response = 0.0F; - this->reset_displacement(); - this->following_left = this->wall_sensors->get_wall(this->sensor_index.left); - this->following_right = this->wall_sensors->get_wall(this->sensor_index.right); -} - -void FollowWall::reset_displacement(bool reset_by_post) { - this->blind_pose.reset_reference(); - this->last_blind_distance = 0.0F; - this->reset_by_post = reset_by_post; + this->following_left = false; + this->following_right = false; } bool FollowWall::get_is_following_left() const { diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 55f60ea..77f22c0 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -62,6 +62,10 @@ const nav::State& Odometry::get_state() const { return this->state; } +nav::State& Odometry::get_state() { + return this->state; +} + void Odometry::set_state(const nav::State& new_state) { this->state = new_state; } From 7edcfd19cc71fda47745bcdd6671d6aae98b7d49 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 25 May 2025 15:03:45 -0300 Subject: [PATCH 444/561] =?UTF-8?q?=F0=9F=90=9B=20Add=20odometry=20initial?= =?UTF-8?q?=20pose?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/odometry.hpp | 5 +++++ micras_nav/src/odometry.cpp | 3 ++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index ef06a0d..3ed018b 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -95,6 +95,11 @@ class Odometry { */ float wheel_radius; + /** + * @brief Initial pose of the robot. + */ + Pose initial_pose; + /** * @brief Last left rotary sensor position. */ diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 77f22c0..5a53712 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -16,6 +16,7 @@ Odometry::Odometry( right_rotary_sensor{right_rotary_sensor}, imu{imu}, wheel_radius{config.wheel_radius}, + initial_pose(config.initial_pose), left_last_position{left_rotary_sensor->get_position()}, right_last_position{right_rotary_sensor->get_position()}, linear_filter{config.linear_cutoff_frequency}, @@ -55,7 +56,7 @@ void Odometry::update(float elapsed_time) { void Odometry::reset() { this->left_last_position = this->left_rotary_sensor->get_position(); this->right_last_position = this->right_rotary_sensor->get_position(); - this->state = {{{0.0F, 0.0F}, 0.0F}, {0.0F, 0.0F}}; + this->state = {this->initial_pose, {0.0F, 0.0F}}; } const nav::State& Odometry::get_state() const { From e3d56739580fe175afa7d1069df777baf0fcde21 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 25 May 2025 15:17:52 -0300 Subject: [PATCH 445/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Use=20reading=20in?= =?UTF-8?q?stead=20of=20error=20on=20follow=20wall=20check=20posts?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 8 ++++---- micras_nav/src/follow_wall.cpp | 10 ++++++---- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 7894516..6a0ca81 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -161,14 +161,14 @@ class FollowWall { bool following_right{true}; /** - * @brief Last error measured by the left wall sensor, used to compute the derivative of the distance sensors. + * @brief Last reading measured by the left wall sensor, used to compute the derivative of the distance sensors. */ - float last_left_error{}; + float last_left_reading{}; /** - * @brief Last error measured by the right wall sensor, used to compute the derivative of the distance sensors. + * @brief Last reading measured by the right wall sensor, used to compute the derivative of the distance sensors. */ - float last_right_error{}; + float last_right_reading{}; /** * @brief Last response returned by the Follow Wall. diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 2ab8833..2b8b8e5 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -74,20 +74,20 @@ bool FollowWall::check_posts(float cell_advance) { bool found_posts = false; - if (-(this->wall_sensors->get_sensor_error(this->sensor_index.left) - this->last_left_error) / delta_distance >= + if (-(this->wall_sensors->get_reading(this->sensor_index.left) - this->last_left_reading) / delta_distance >= this->post_threshold) { this->following_left = false; found_posts = true; } - if (-(this->wall_sensors->get_sensor_error(this->sensor_index.right) - this->last_right_error) / delta_distance >= + if (-(this->wall_sensors->get_reading(this->sensor_index.right) - this->last_right_reading) / delta_distance >= this->post_threshold) { this->following_right = false; found_posts = true; } - this->last_left_error = this->wall_sensors->get_sensor_error(this->sensor_index.left); - this->last_right_error = this->wall_sensors->get_sensor_error(this->sensor_index.right); + this->last_left_reading = this->wall_sensors->get_reading(this->sensor_index.left); + this->last_right_reading = this->wall_sensors->get_reading(this->sensor_index.right); return found_posts; } @@ -126,6 +126,8 @@ void FollowWall::reset() { this->last_response = 0.0F; this->following_left = false; this->following_right = false; + this->last_left_reading = 0.0F; + this->last_right_reading = 0.0F; } bool FollowWall::get_is_following_left() const { From f6672ebc4b28dfb207dd472f524e3907232d29bf Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 25 May 2025 16:05:56 -0300 Subject: [PATCH 446/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20post=20reference?= =?UTF-8?q?=20on=20follow=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 12 ++++++------ micras_nav/src/follow_wall.cpp | 10 +++++----- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 6a0ca81..39e123b 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -28,8 +28,8 @@ class FollowWall { float max_angular_acceleration{}; float cell_size{}; float post_threshold{}; - float post_clearance{}; - float post_reference{}; + float post_reference_start{}; + float post_reference_end{}; }; /** @@ -131,14 +131,14 @@ class FollowWall { float post_threshold; /** - * @brief Margin in front of a post to stop seeing it. + * @brief Cell advance of the robot when it starts to see a post. */ - float post_clearance; + float post_reference_start{}; /** - * @brief Cell advance of the robot when it sees a post. + * @brief Cell advance of the robot when it stops seeing a post. */ - float post_reference; + float post_reference_end{}; /** * @brief Last grid pose of the robot. diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 2b8b8e5..9e94662 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -13,8 +13,8 @@ FollowWall::FollowWall(const std::shared_ptr>& wall_senso max_angular_acceleration{config.max_angular_acceleration}, cell_size{config.cell_size}, post_threshold{config.post_threshold}, - post_clearance{config.post_clearance}, - post_reference{config.post_reference} { } + post_reference_start{config.post_reference_start}, + post_reference_end{config.post_reference_end} { } float FollowWall::compute_angular_correction(float elapsed_time, State& state) { float cell_advance = state.pose.to_cell(this->cell_size).y; @@ -26,8 +26,8 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { if (grid_pose == this->last_grid_pose) { if (this->check_posts(cell_advance)) { - clear_position_error(state, this->post_reference - cell_advance); - cell_advance = this->post_reference; + clear_position_error(state, this->post_reference_end - cell_advance); + cell_advance = this->post_reference_end; } } else { if (grid_pose != this->last_grid_pose.front()) { @@ -38,7 +38,7 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { this->last_grid_pose = grid_pose; this->last_cell_advance = cell_advance; - if (std::abs(cell_advance - this->cell_size / 2.0F) < this->cell_size / 2.0F - this->post_clearance) { + if (cell_advance < this->post_reference_start or cell_advance > this->post_reference_end) { this->following_left |= this->wall_sensors->get_wall(this->sensor_index.left); this->following_right |= this->wall_sensors->get_wall(this->sensor_index.right); } From 48c99132e3fcd7b61d25ee294b3081b811b07a33 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 25 May 2025 16:37:26 -0300 Subject: [PATCH 447/561] =?UTF-8?q?=E2=8F=AA=20Add=20post=20clearance=20ba?= =?UTF-8?q?ck?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 10 +++++----- micras_nav/src/follow_wall.cpp | 20 ++++++++++--------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 39e123b..f0b24d7 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -28,8 +28,8 @@ class FollowWall { float max_angular_acceleration{}; float cell_size{}; float post_threshold{}; - float post_reference_start{}; - float post_reference_end{}; + float post_reference{}; + float post_clearance{}; }; /** @@ -133,12 +133,12 @@ class FollowWall { /** * @brief Cell advance of the robot when it starts to see a post. */ - float post_reference_start{}; + float post_reference{}; /** - * @brief Cell advance of the robot when it stops seeing a post. + * @brief Distance the robot should travel after seeing a post to start making observations. */ - float post_reference_end{}; + float post_clearance{}; /** * @brief Last grid pose of the robot. diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 9e94662..40b266f 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -13,8 +13,8 @@ FollowWall::FollowWall(const std::shared_ptr>& wall_senso max_angular_acceleration{config.max_angular_acceleration}, cell_size{config.cell_size}, post_threshold{config.post_threshold}, - post_reference_start{config.post_reference_start}, - post_reference_end{config.post_reference_end} { } + post_reference{config.post_reference}, + post_clearance{config.post_clearance} { } float FollowWall::compute_angular_correction(float elapsed_time, State& state) { float cell_advance = state.pose.to_cell(this->cell_size).y; @@ -26,8 +26,8 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { if (grid_pose == this->last_grid_pose) { if (this->check_posts(cell_advance)) { - clear_position_error(state, this->post_reference_end - cell_advance); - cell_advance = this->post_reference_end; + clear_position_error(state, this->post_reference - cell_advance); + cell_advance = this->post_reference; } } else { if (grid_pose != this->last_grid_pose.front()) { @@ -38,7 +38,7 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { this->last_grid_pose = grid_pose; this->last_cell_advance = cell_advance; - if (cell_advance < this->post_reference_start or cell_advance > this->post_reference_end) { + if (std::abs(cell_advance - this->post_reference) > this->post_clearance) { this->following_left |= this->wall_sensors->get_wall(this->sensor_index.left); this->following_right |= this->wall_sensors->get_wall(this->sensor_index.right); } @@ -74,14 +74,16 @@ bool FollowWall::check_posts(float cell_advance) { bool found_posts = false; - if (-(this->wall_sensors->get_reading(this->sensor_index.left) - this->last_left_reading) / delta_distance >= - this->post_threshold) { + if (this->following_left and + -(this->wall_sensors->get_reading(this->sensor_index.left) - this->last_left_reading) / delta_distance >= + this->post_threshold) { this->following_left = false; found_posts = true; } - if (-(this->wall_sensors->get_reading(this->sensor_index.right) - this->last_right_reading) / delta_distance >= - this->post_threshold) { + if (this->following_right and + -(this->wall_sensors->get_reading(this->sensor_index.right) - this->last_right_reading) / delta_distance >= + this->post_threshold) { this->following_right = false; found_posts = true; } From b715e2694bf9e54aea962f8e9f1092dc14ec5d49 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 25 May 2025 19:20:15 -0300 Subject: [PATCH 448/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Remove=20feed=20fo?= =?UTF-8?q?rward=20limiter?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/speed_controller.hpp | 12 ------------ micras_nav/src/speed_controller.cpp | 8 ++------ 2 files changed, 2 insertions(+), 18 deletions(-) diff --git a/micras_nav/include/micras/nav/speed_controller.hpp b/micras_nav/include/micras/nav/speed_controller.hpp index 9bbacda..46f904a 100644 --- a/micras_nav/include/micras/nav/speed_controller.hpp +++ b/micras_nav/include/micras/nav/speed_controller.hpp @@ -27,8 +27,6 @@ class SpeedController { float angular_acceleration; }; - float max_linear_acceleration{}; - float max_angular_acceleration{}; core::PidController::Config linear_pid; core::PidController::Config angular_pid; FeedForward left_feed_forward{}; @@ -78,16 +76,6 @@ class SpeedController { */ static float feed_forward(const Twist& speed, const Twist& acceleration, const Config::FeedForward& config); - /** - * @brief The maximum linear acceleration of the robot. - */ - float max_linear_acceleration; - - /** - * @brief The maximum angular acceleration of the robot. - */ - float max_angular_acceleration; - /** * @brief The last linear speed of the robot. */ diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp index c27cd02..2e70cff 100644 --- a/micras_nav/src/speed_controller.cpp +++ b/micras_nav/src/speed_controller.cpp @@ -9,8 +9,6 @@ namespace micras::nav { SpeedController::SpeedController(const Config& config) : - max_linear_acceleration{config.max_linear_acceleration}, - max_angular_acceleration{config.max_angular_acceleration}, linear_pid(config.linear_pid), angular_pid(config.angular_pid), left_feed_forward{config.left_feed_forward}, @@ -29,11 +27,9 @@ std::pair SpeedController::compute_control_commands( } std::pair SpeedController::compute_feed_forward_commands(const Twist& desired_twist, float elapsed_time) { - const float linear_acceleration = (desired_twist.linear - this->last_linear_speed) / elapsed_time; - const float angular_acceleration = (desired_twist.angular - this->last_angular_speed) / elapsed_time; const Twist acceleration_twist{ - .linear = std::clamp(linear_acceleration, -this->max_linear_acceleration, this->max_linear_acceleration), - .angular = std::clamp(angular_acceleration, -this->max_angular_acceleration, this->max_angular_acceleration), + .linear = (desired_twist.linear - this->last_linear_speed) / elapsed_time, + .angular = (desired_twist.angular - this->last_angular_speed) / elapsed_time, }; this->last_linear_speed = desired_twist.linear; From dc2fd8e225704d3776a51407995f4594bc8ff779 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 25 May 2025 22:11:54 -0300 Subject: [PATCH 449/561] =?UTF-8?q?=F0=9F=94=A5=20Disable=20follow=20wall?= =?UTF-8?q?=20near=20post=20references?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/follow_wall.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 40b266f..559b8ee 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -38,11 +38,14 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { this->last_grid_pose = grid_pose; this->last_cell_advance = cell_advance; - if (std::abs(cell_advance - this->post_reference) > this->post_clearance) { - this->following_left |= this->wall_sensors->get_wall(this->sensor_index.left); - this->following_right |= this->wall_sensors->get_wall(this->sensor_index.right); + if (cell_advance <= this->post_reference + this->post_clearance and + cell_advance >= this->post_reference - this->post_clearance / 2.0F) { + return 0.0F; } + this->following_left |= this->wall_sensors->get_wall(this->sensor_index.left); + this->following_right |= this->wall_sensors->get_wall(this->sensor_index.right); + float error{}; if (this->following_left and this->following_right) { @@ -75,21 +78,21 @@ bool FollowWall::check_posts(float cell_advance) { bool found_posts = false; if (this->following_left and - -(this->wall_sensors->get_reading(this->sensor_index.left) - this->last_left_reading) / delta_distance >= + -(this->wall_sensors->get_adc_reading(this->sensor_index.left) - this->last_left_reading) / delta_distance >= this->post_threshold) { this->following_left = false; found_posts = true; } if (this->following_right and - -(this->wall_sensors->get_reading(this->sensor_index.right) - this->last_right_reading) / delta_distance >= + -(this->wall_sensors->get_adc_reading(this->sensor_index.right) - this->last_right_reading) / delta_distance >= this->post_threshold) { this->following_right = false; found_posts = true; } - this->last_left_reading = this->wall_sensors->get_reading(this->sensor_index.left); - this->last_right_reading = this->wall_sensors->get_reading(this->sensor_index.right); + this->last_left_reading = this->wall_sensors->get_adc_reading(this->sensor_index.left); + this->last_right_reading = this->wall_sensors->get_adc_reading(this->sensor_index.right); return found_posts; } From 7f9bc04cdb52eaff10d7e5be3fb729079e87c214 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 25 May 2025 22:40:16 -0300 Subject: [PATCH 450/561] =?UTF-8?q?=F0=9F=94=A5=20Remove=20max=5Flinear=5F?= =?UTF-8?q?speed=20from=20follow=20wall?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 6 ------ micras_nav/src/follow_wall.cpp | 4 +--- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index f0b24d7..df835f9 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -24,7 +24,6 @@ class FollowWall { struct Config { core::PidController::Config pid; core::WallSensorsIndex wall_sensor_index{}; - float max_linear_speed{}; float max_angular_acceleration{}; float cell_size{}; float post_threshold{}; @@ -110,11 +109,6 @@ class FollowWall { */ core::WallSensorsIndex sensor_index; - /** - * @brief Maximum linear speed of the robot. - */ - float max_linear_speed; - /** * @brief Maximum angular acceleration of the robot. */ diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 559b8ee..d0976a1 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -9,7 +9,6 @@ FollowWall::FollowWall(const std::shared_ptr>& wall_senso wall_sensors{wall_sensors}, pid{config.pid}, sensor_index{config.wall_sensor_index}, - max_linear_speed{config.max_linear_speed}, max_angular_acceleration{config.max_angular_acceleration}, cell_size{config.cell_size}, post_threshold{config.post_threshold}, @@ -59,8 +58,7 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { return 0.0F; } - const float response = - state.velocity.linear * this->pid.compute_response(error, elapsed_time) / this->max_linear_speed; + const float response = state.velocity.linear * this->pid.compute_response(error, elapsed_time); const float clamped_response = core::move_towards(this->last_response, response, this->max_angular_acceleration * elapsed_time); From 8881af69ab9787d84f4ce3f6ac57a0b75f96cf45 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 26 May 2025 09:38:08 -0300 Subject: [PATCH 451/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20angle=20to=20grid?= =?UTF-8?q?=20conversion?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 2 +- micras_nav/src/grid_pose.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 316b626..825e54a 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -373,7 +373,7 @@ constexpr float const float turn_angle = std::abs(angle); if (turn_angle == std::numbers::pi_v / 4.0F) { - return (1.0F + std::numbers::sqrt2_v)*cell_size / 2.0F; + return cell_size * (1.0F + std::numbers::sqrt2_v) / 2.0F; } if (turn_angle == std::numbers::pi_v / 2.0F) { diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index da36ddd..21786f2 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -9,7 +9,7 @@ namespace micras::nav { Side angle_to_grid(float angle) { const int32_t grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); - return static_cast(grid_angle < 0 ? 4 + (grid_angle % 4) : grid_angle % 4); + return static_cast(grid_angle < 0 ? (4 + (grid_angle % 4)) % 4 : grid_angle % 4); } Side GridPoint::direction(const GridPoint& next) const { From d99d748fd54f6db2877ae034dfe78c888ffdac43 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 26 May 2025 12:58:59 -0300 Subject: [PATCH 452/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Reset=20pid=20afte?= =?UTF-8?q?r=20follow=20wall=20change?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/follow_wall.hpp | 8 ++++---- micras_nav/src/follow_wall.cpp | 20 +++++++++++++------ 2 files changed, 18 insertions(+), 10 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index df835f9..e57545a 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -73,13 +73,13 @@ class FollowWall { /** * @brief Check if the robot saw a post. * - * @param cell_advance The distance of the robot from the start of the cell. + * @param pose The current pose of the robot. * @return True if the robot saw a post, false otherwise. * * @details This function uses the derivative of the distance sensors readings with respect * to the robot's traveled distance, comparing it to a threshold to detect posts. */ - bool check_posts(float cell_advance); + bool check_posts(const Pose& pose); /** * @brief Reset the PID controller and the relative pose. @@ -140,9 +140,9 @@ class FollowWall { GridPose last_grid_pose{}; /** - * @brief Last cell advance of the robot. + * @brief Last pose of the robot. */ - float last_cell_advance{}; + Pose last_pose{}; /** * @brief Flag to indicate if the robot is currently following the left wall. diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index d0976a1..fc24483 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -24,9 +24,10 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { } if (grid_pose == this->last_grid_pose) { - if (this->check_posts(cell_advance)) { + if (this->check_posts(state.pose)) { clear_position_error(state, this->post_reference - cell_advance); cell_advance = this->post_reference; + this->pid.reset(); } } else { if (grid_pose != this->last_grid_pose.front()) { @@ -35,15 +36,22 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { } this->last_grid_pose = grid_pose; - this->last_cell_advance = cell_advance; + this->last_pose = state.pose; if (cell_advance <= this->post_reference + this->post_clearance and cell_advance >= this->post_reference - this->post_clearance / 2.0F) { return 0.0F; } - this->following_left |= this->wall_sensors->get_wall(this->sensor_index.left); - this->following_right |= this->wall_sensors->get_wall(this->sensor_index.right); + if (not this->following_left and this->wall_sensors->get_wall(this->sensor_index.left)) { + this->following_left = true; + this->pid.reset(); + } + + if (not this->following_right and this->wall_sensors->get_wall(this->sensor_index.right)) { + this->following_right = true; + this->pid.reset(); + } float error{}; @@ -66,8 +74,8 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { return clamped_response; } -bool FollowWall::check_posts(float cell_advance) { - const float delta_distance = cell_advance - this->last_cell_advance; +bool FollowWall::check_posts(const Pose& pose) { + const float delta_distance = (pose.position - this->last_pose.position).magnitude(); if (delta_distance <= 0.0F) { return false; From 96889e852ba54cb08981cf1b2ea2781b4964ad9b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 26 May 2025 15:48:07 -0300 Subject: [PATCH 453/561] =?UTF-8?q?=F0=9F=9A=A7=20Use=20dfs=20instead=20of?= =?UTF-8?q?=20graph=20to=20compute=20best=20route?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 4 +- micras_nav/include/micras/nav/maze.hpp | 20 +- micras_nav/include/micras/nav/maze_graph.hpp | 170 ------------- micras_nav/src/action_queuer.cpp | 77 ++---- micras_nav/src/maze.cpp | 96 ++++---- micras_nav/src/maze_graph.cpp | 226 ------------------ 6 files changed, 75 insertions(+), 518 deletions(-) delete mode 100644 micras_nav/include/micras/nav/maze_graph.hpp delete mode 100644 micras_nav/src/maze_graph.cpp diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index a822a32..46ab22f 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -81,7 +81,7 @@ class ActionQueuer { /** * @brief Fill the action queue with a sequence of actions to the end. */ - void recompute(const std::list& best_route, bool add_start = true); + void recompute(const std::list& best_route, bool add_start = true); /** * @brief Get the total time to complete the actions in the queue. @@ -98,7 +98,7 @@ class ActionQueuer { * @param cell_size Size of the cells in the grid. * @return List of actions to perform. */ - static std::list get_actions(const GridPose& origin_pose, const GridPose& target_pose, float cell_size); + static Action::Id get_action(const GridPose& origin_pose, const GridPoint& target_point, float cell_size); private: /** diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index f6c8d5f..b643622 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -11,9 +11,9 @@ #include "micras/core/serializable.hpp" #include "micras/core/types.hpp" +#include "micras/nav/action_queuer.hpp" #include "micras/nav/costmap.hpp" #include "micras/nav/grid_pose.hpp" -#include "micras/nav/maze_graph.hpp" namespace micras::nav { /** @@ -32,7 +32,7 @@ class TMaze : public core::ISerializable { GridPose start{}; std::unordered_set goal; float cost_margin{}; - MazeGraph::Config graph_config{}; + ActionQueuer::Config action_queuer_config{}; }; /** @@ -78,12 +78,7 @@ class TMaze : public core::ISerializable { * * @return The best route to the goal. */ - const std::list& get_best_route() const; - - /** - * @brief Compute the maze graph. - */ - void compute_graph(); + const std::list& get_best_route() const; /** * @brief Serialize the best route to the goal. @@ -131,6 +126,8 @@ class TMaze : public core::ISerializable { */ std::pair get_next_bfs_goal(const GridPose& pose, bool discover) const; + float get_route_time(const std::list& route); + /** * @brief Check if the cell is a dead end. * @@ -161,10 +158,7 @@ class TMaze : public core::ISerializable { */ Costmap costmap; - /** - * @brief Graph of the maze for computing the best route. - */ - MazeGraph graph; + ActionQueuer action_queuer; /** * @brief Start pose of the robot in the maze. @@ -194,7 +188,7 @@ class TMaze : public core::ISerializable { /** * @brief Current best found route to the goal. */ - std::list best_route; + std::list best_route; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/maze_graph.hpp b/micras_nav/include/micras/nav/maze_graph.hpp deleted file mode 100644 index a1307de..0000000 --- a/micras_nav/include/micras/nav/maze_graph.hpp +++ /dev/null @@ -1,170 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_MAZE_GRAPH_HPP -#define MICRAS_NAV_MAZE_GRAPH_HPP - -#include -#include -#include - -#include "micras/nav/action_queuer.hpp" -#include "micras/nav/grid_pose.hpp" - -namespace micras::nav { -/** - * @brief - */ -class MazeGraph { -public: - /** - * @brief Node structure representing a node in the maze graph. - */ - struct Node { - explicit Node(GridPose pose) : pose(pose){}; - - GridPose pose; - std::unordered_set next; - std::unordered_set prev; - }; - - /** - * @brief Configuration structure for the maze graph. - */ - struct Config { - float cell_size; - ActionQueuer::Config::Dynamic cost_params; - }; - - /** - * @brief Construct a new MazeGraph object. - * - * @param config The configuration for the maze graph. - */ - explicit MazeGraph(Config config); - - /** - * @brief Check if the graph has a node for a given pose. - * - * @param pose The pose to check. - * @return true if the graph has a node for the given pose, false otherwise. - */ - bool has_node(const GridPose& pose) const; - - /** - * @brief Add a new node to the graph. - * - * @param pose The grid pose that represents the node. - */ - void add_node(const GridPose& pose); - - /** - * @brief Remove a node from the graph. - * - * @param pose The grid pose that represents the node. - */ - void remove_node(const GridPose& pose); - - /** - * @brief Add a new edge connecting two poses. - * - * @param from The starting node of the edge. - * @param to The ending node of the edge. - * @param cost The cost of the edge. - */ - void add_edge(const GridPose& from, const GridPose& to); - - /** - * @brief Remove an edge connecting two poses. - * - * @param from The starting node of the edge. - * @param to The ending node of the edge. - */ - void remove_edge(const GridPose& from, const GridPose& to); - - /** - * @brief Process the nodes in the graph. - * - * @param start The starting node. - */ - void process_nodes(const GridPose& start); - - /** - * @brief Compute the best route on the graph using the dijkstra algorithm. - * - * @param start The starting node. - * @param end A set of ending nodes. - * @return A list of poses representing the best route from start to end. - */ - std::list get_best_route(const GridPose& start, const std::unordered_set& end); - - /** - * @brief Clear the contents of the graph. - */ - void reset(); - -private: - /** - * @brief Add extra edges representing diagonals, long curves and long straights to the graph. - * - * @param start The starting node. - */ - void add_extra_edges(const GridPose& start); - - /** - * @brief Check if a skip is possible between three poses. - * - * @param first The first pose. - * @param second The second pose. - * @param third The third pose. - * @return true if a skip is possible, false otherwise. - */ - bool can_skip(const GridPose& first, const GridPose& second, const GridPose& third) const; - - /** - * @brief Get the cost of an edge between two poses. - * - * @param from The starting pose. - * @param to The ending pose. - * @return The cost of the edge. - */ - float get_edge_cost(const GridPose& from, const GridPose& to) const; - - /** - * @brief Get the main action type that connects two grid poses. - * - * @param from The starting pose. - * @param to The ending pose. - * @return The main action type. - */ - ActionQueuer::ActionType get_main_action_type(const GridPose& from, const GridPose& to) const; - - /** - * @brief The size of the cells in the maze. - */ - float cell_size; - - /** - * @brief The configuration for calculating the edge costs. - */ - ActionQueuer::Config::Dynamic cost_params; - - /** - * @brief The radius of the curve when performing a turn. - */ - float curve_radius; - - /** - * @brief The linear speed of the robot while turning. - */ - float curve_linear_speed; - - /** - * @brief - */ - std::unordered_map graph; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_MAZE_GRAPH_HPP diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 825e54a..b10cd5d 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -112,7 +112,7 @@ bool ActionQueuer::empty() const { return this->action_queue.empty(); } -void ActionQueuer::recompute(const std::list& best_route, bool add_start) { +void ActionQueuer::recompute(const std::list& best_route, bool add_start) { this->action_queue = {}; if (add_start) { @@ -125,8 +125,11 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_sta std::list actions; + Side direction = best_route.front().direction(*std::next(best_route.begin())); + for (auto route_it = best_route.begin(); std::next(route_it) != best_route.end(); route_it++) { - actions.splice(actions.end(), get_actions(*route_it, *std::next(route_it), this->cell_size)); + actions.push_back(get_action({*route_it, direction}, *std::next(route_it), this->cell_size)); + direction = (*route_it).direction(*std::next(route_it)); } for (auto action_it = actions.begin(); action_it != actions.end(); action_it++) { @@ -210,73 +213,21 @@ float ActionQueuer::get_total_time() const { return total_time; } -std::list - ActionQueuer::get_actions(const GridPose& origin_pose, const GridPose& target_pose, float cell_size) { - const Side relative_side = origin_pose.get_relative_side(target_pose.position); - - if (origin_pose.position == target_pose.position or relative_side == Side::DOWN) { - return {}; - } - - if (relative_side == Side::UP) { - if (origin_pose.orientation != target_pose.orientation) { - return {}; - } - - const float distance = - origin_pose.position.to_vector(cell_size).distance(target_pose.position.to_vector(cell_size)); - return {{ActionType::MOVE_FORWARD, distance}}; - } - - if (origin_pose.turned_left().front() == target_pose) { - return {{ActionType::TURN, std::numbers::pi_v / 2.0F}}; - } - - if (origin_pose.turned_right().front() == target_pose) { - return {{ActionType::TURN, -std::numbers::pi_v / 2.0F}}; - } - - if (origin_pose.turned_left().front().turned_left().front() == target_pose) { - return {{ActionType::TURN, std::numbers::pi_v}}; +Action::Id ActionQueuer::get_action(const GridPose& origin_pose, const GridPoint& target_point, float cell_size) { + if (origin_pose.front().position == target_point) { + const float distance = origin_pose.position.to_vector(cell_size).distance(target_point.to_vector(cell_size)); + return {ActionType::MOVE_FORWARD, distance}; } - if (origin_pose.turned_right().front().turned_right().front() == target_pose) { - return {{ActionType::TURN, -std::numbers::pi_v}}; + if (origin_pose.turned_left().front().position == target_point) { + return {ActionType::TURN, std::numbers::pi_v / 2.0F}; } - const bool keep_direction = (origin_pose.orientation == target_pose.orientation); - const GridPoint target_position = (keep_direction ? target_pose : target_pose.turned_back().front()).position; - - if (std::abs(target_position.x - origin_pose.position.x) != std::abs(target_position.y - origin_pose.position.y) or - origin_pose.turned_right().get_relative_side(target_position) != Side::LEFT) { - return {}; - } - - if (not keep_direction and - target_pose.orientation != (relative_side == Side::LEFT ? origin_pose.turned_left().orientation : - origin_pose.turned_right().orientation)) { - return {}; - } - - std::list actions; - - float turn_angle = - relative_side == Side::LEFT ? std::numbers::pi_v / 4.0F : -std::numbers::pi_v / 4.0F; - - actions.emplace_back(Action::Id{ActionType::TURN, turn_angle}); - - float distance = origin_pose.position.to_vector(cell_size).distance(target_position.to_vector(cell_size)); - - if (keep_direction) { - turn_angle *= -1.0F; - } else { - distance += cell_size * std::numbers::sqrt2_v / 2; + if (origin_pose.turned_right().front().position == target_point) { + return {ActionType::TURN, -std::numbers::pi_v / 2.0F}; } - actions.emplace_back(Action::Id{ActionType::DIAGONAL, distance}); - actions.emplace_back(Action::Id{ActionType::TURN, turn_angle}); - - return actions; + return {ActionType::STOP, cell_size / 2.0F}; } std::pair diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 3181f22..5703ee1 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -6,15 +6,18 @@ #define MICRAS_NAV_MAZE_CPP #include +#include #include -#include "micras/nav/grid_pose.hpp" #include "micras/nav/maze.hpp" namespace micras::nav { template TMaze::TMaze(Config config) : - graph(config.graph_config), start{config.start}, goal{config.goal}, cost_margin{config.cost_margin} { + action_queuer(config.action_queuer_config), + start{config.start}, + goal{config.goal}, + cost_margin{config.cost_margin} { this->costmap.update_wall(this->start, false); this->costmap.update_wall(start.turned_right(), true); @@ -125,69 +128,68 @@ void TMaze::compute_minimum_cost() { template void TMaze::compute_best_route() { - this->compute_graph(); - this->best_route = this->graph.get_best_route(this->start, this->goal); -} + std::list current_route; + std::stack stack; -template -const std::list& TMaze::get_best_route() const { - return this->best_route; -} + float best_route_time = std::numeric_limits::max(); + stack.push(this->start); -template -void TMaze::compute_graph() { - std::queue queue; - this->graph.reset(); + while (not stack.empty()) { + GridPose prev_pose = stack.top(); + stack.pop(); + current_route.push_back(prev_pose.position); + GridPoint current_position = prev_pose.front().position; - this->graph.add_node(this->start); - queue.push(this->start); + bool dead_end = true; - while (not queue.empty()) { - GridPose current_pose = queue.front(); - queue.pop(); - - if (this->goal.contains(current_pose.position)) { - continue; - } + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { + if (this->goal.contains(current_position)) { + current_route.push_back(current_position); + float current_route_time = get_route_time(current_route); - auto sides_order = { - current_pose.turned_right().orientation, - current_pose.turned_left().orientation, - current_pose.orientation, - }; + if (current_route_time < best_route_time) { + best_route_time = current_route_time; + best_route = current_route; + } - for (Side side : sides_order) { - if (this->costmap.has_wall({current_pose.position, side}, true)) { - continue; + break; } - GridPose next_pose = {current_pose.position + side, side}; + Side side = static_cast(i); + GridPose next_pose = {current_position, side}; - if (not this->was_visited(this->costmap.get_cell(next_pose.position))) { + if (this->costmap.has_wall(next_pose, true) or + not this->was_visited(this->costmap.get_cell(next_pose.front().position))) { continue; } - if (not this->graph.has_node(next_pose)) { - this->graph.add_node(next_pose); - queue.push(next_pose); + if (std::find(current_route.begin(), current_route.end(), next_pose.position) == current_route.end()) { + stack.push(next_pose); + dead_end = false; } + } - this->graph.add_edge(current_pose, next_pose); + if (dead_end and not stack.empty()) { + const GridPoint& intersection_point = stack.top().position; + auto intersection_it = std::find(current_route.begin(), current_route.end(), intersection_point); + current_route.erase(intersection_it, current_route.end()); } } +} - this->graph.process_nodes(this->start); +template +const std::list& TMaze::get_best_route() const { + return this->best_route; } template std::vector TMaze::serialize() const { std::vector buffer; - buffer.reserve(3 * this->best_route.size()); + buffer.reserve(2 * this->best_route.size()); - for (const auto& grid_pose : this->best_route) { - buffer.emplace_back(grid_pose.position.x); - buffer.emplace_back(grid_pose.position.y); - buffer.emplace_back(grid_pose.orientation); + for (const auto& grid_point : this->best_route) { + buffer.emplace_back(grid_point.x); + buffer.emplace_back(grid_point.y); } return buffer; @@ -197,10 +199,10 @@ template void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { this->best_route.clear(); - for (uint32_t i = 0; i < size; i += 3) { + for (uint32_t i = 0; i < size; i += 2) { this->best_route.emplace_back( // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) - GridPoint{buffer[i], buffer[i + 1]}, static_cast(buffer[i + 2]) + GridPoint{buffer[i], buffer[i + 1]} ); } } @@ -288,6 +290,12 @@ std::pair TMaze::get_next_bfs_goal(const Grid return result_pair; } +template +float TMaze::get_route_time(const std::list& route) { + this->action_queuer.recompute(route); + return this->action_queuer.get_total_time(); +} + template bool TMaze::is_dead_end(const Costmap::Cell& cell) { uint8_t wall_count = 0; diff --git a/micras_nav/src/maze_graph.cpp b/micras_nav/src/maze_graph.cpp deleted file mode 100644 index 036e027..0000000 --- a/micras_nav/src/maze_graph.cpp +++ /dev/null @@ -1,226 +0,0 @@ -/** - * @file - */ - -#include -#include -#include - -#include "micras/nav/maze_graph.hpp" - -namespace micras::nav { -MazeGraph::MazeGraph(Config config) : - cell_size{config.cell_size}, - cost_params{config.cost_params}, - curve_radius{this->cell_size / 2.0F}, - curve_linear_speed{std::sqrt(cost_params.max_centrifugal_acceleration * curve_radius)} { } - -bool MazeGraph::has_node(const GridPose& pose) const { - return this->graph.contains(pose); -} - -void MazeGraph::add_node(const GridPose& pose) { - this->graph.emplace(pose, Node{pose}); -} - -void MazeGraph::remove_node(const GridPose& pose) { - for (const GridPose& next_pose : this->graph.at(pose).next) { - if (this->graph.at(next_pose).prev.size() == 1) { - this->remove_node(next_pose); - } else { - this->graph.at(next_pose).prev.erase(pose); - } - } - - this->graph.erase(pose); -} - -void MazeGraph::add_edge(const GridPose& from, const GridPose& to) { - if (this->graph.at(from).next.contains(to)) { - return; - } - - this->graph.at(from).next.emplace(to); - this->graph.at(to).prev.emplace(from); -} - -void MazeGraph::remove_edge(const GridPose& from, const GridPose& to) { - this->graph.at(from).next.erase(to); - this->graph.at(to).prev.erase(from); - - if (this->graph.at(to).prev.empty()) { - this->remove_node(to); - } -} - -void MazeGraph::process_nodes(const GridPose& start) { - this->add_extra_edges(start); -} - -void MazeGraph::add_extra_edges(const GridPose& start) { - std::unordered_set visited; - std::queue queue; - queue.push(start); - - while (not queue.empty()) { - const GridPose current_pose = queue.front(); - Node& current_node = this->graph.at(current_pose); - queue.pop(); - - visited.insert(current_pose); - bool skipped = false; - - for (auto next_it = current_node.next.begin(); next_it != current_node.next.end();) { - const GridPose next_pose = *next_it; - - for (const GridPose& next_next_pose : this->graph.at(next_pose).next) { - if (current_node.next.contains(next_next_pose)) { - continue; - } - - if (this->can_skip(current_pose, next_pose, next_next_pose)) { - skipped = true; - this->add_edge(current_pose, next_next_pose); - next_it = current_node.next.begin(); - - if (this->graph.at(next_pose).next.size() == 1) { - this->remove_edge(current_pose, next_pose); - next_it = current_node.next.begin(); - break; - } - } - } - - if (skipped) { - skipped = false; - continue; - } - - if (not visited.contains(next_pose)) { - queue.push(next_pose); - } - - next_it++; - } - } -} - -std::list MazeGraph::get_best_route(const GridPose& start, const std::unordered_set& end) { - std::unordered_map distance; - std::unordered_map previous; - std::unordered_set visited; - - auto comp_function = [&distance](const GridPose& first, const GridPose& second) { - return distance.at(first) > distance.at(second); - }; - - std::priority_queue, decltype(comp_function)> queue(comp_function); - - for (const auto& [pose, node] : this->graph) { - distance[pose] = std::numeric_limits::max(); - } - - distance[start] = 0; - queue.push(start); - GridPose current_pose = start; - - while (!queue.empty()) { - current_pose = queue.top(); - queue.pop(); - - if (visited.contains(current_pose)) { - continue; - } - - if (end.contains(current_pose.position)) { - break; - } - - visited.insert(current_pose); - - for (const GridPose& next_pose : this->graph.at(current_pose).next) { - const float edge_cost = this->get_edge_cost(current_pose, next_pose); - - if (distance.at(next_pose) > distance.at(current_pose) + edge_cost) { - previous[next_pose] = current_pose; - distance[next_pose] = distance.at(current_pose) + edge_cost; - queue.push(next_pose); - } - } - } - - std::list route; - - while (current_pose != start) { - route.push_front(current_pose); - current_pose = previous.at(current_pose); - } - - route.push_front(start); - - return route; -} - -void MazeGraph::reset() { - this->graph.clear(); -} - -bool MazeGraph::can_skip(const GridPose& first, const GridPose& second, const GridPose& third) const { - ActionQueuer::ActionType skip_action = this->get_main_action_type(first, third); - - if (skip_action == ActionQueuer::ActionType::STOP) { - return false; - } - - if (skip_action != ActionQueuer::ActionType::MOVE_FORWARD) { - return true; - } - - return this->get_main_action_type(first, second) == ActionQueuer::ActionType::MOVE_FORWARD; -} - -float MazeGraph::get_edge_cost(const GridPose& from, const GridPose& to) const { - auto actions = ActionQueuer::get_actions(from, to, this->cell_size); - float total_time = 0.0F; - - for (const auto& action : actions) { - if (action.type == ActionQueuer::ActionType::TURN) { - const float max_angular_speed = TurnAction::calculate_max_angular_speed( - action.value, this->curve_radius, this->cost_params.max_angular_acceleration, - this->cost_params.max_centrifugal_acceleration - ); - - total_time += TurnAction::calculate_total_time( - action.value, max_angular_speed, this->cost_params.max_angular_acceleration - ); - - if (std::abs(action.value) == std::numbers::pi_v / 4.0F) { - total_time -= - this->cell_size * (std::numbers::sqrt2_v - 1.0F) / this->cost_params.max_linear_speed; - } - } else { - total_time += MoveAction::calculate_total_time( - action.value, this->curve_linear_speed, this->curve_linear_speed, this->cost_params.max_linear_speed, - this->cost_params.max_linear_acceleration, this->cost_params.max_linear_deceleration - ); - } - } - - return total_time; -} - -ActionQueuer::ActionType MazeGraph::get_main_action_type(const GridPose& from, const GridPose& to) const { - auto actions = ActionQueuer::get_actions(from, to, this->cell_size); - - if (actions.size() == 3) { - return ActionQueuer::ActionType::DIAGONAL; - } - - if (actions.empty() or (actions.front().type == ActionQueuer::ActionType::TURN and - std::abs(actions.front().value) == std::numbers::pi_v)) { - return ActionQueuer::ActionType::STOP; - } - - return static_cast(actions.front().type); -} -} // namespace micras::nav From fcb9a393620c5a088a533417d879f1cdaaa56d91 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 27 May 2025 14:07:03 -0300 Subject: [PATCH 454/561] =?UTF-8?q?=E2=9C=A8=20Add=20diagonals=20post=20pr?= =?UTF-8?q?ocessing=20to=20action=20queuer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/micras/nav/action_queuer.hpp | 14 +++++ micras_nav/src/action_queuer.cpp | 61 ++++++++++++++----- 2 files changed, 59 insertions(+), 16 deletions(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 46ab22f..c7bbd31 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -124,6 +124,20 @@ class ActionQueuer { NUMBER_OF_CURVES = 5, }; + /** + * @brief Add diagonals to the list of actions. + * + * @param actions List of actions to add diagonals to. + */ + void add_diagonals(std::list& actions) const; + + /** + * @brief Join consecutive actions of the same type. + * + * @param actions List of actions to join. + */ + void join_actions(std::list& actions); + /** * @brief Get the trim distances for a turn action. * diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index b10cd5d..f209c39 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -132,22 +132,8 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_st direction = (*route_it).direction(*std::next(route_it)); } - for (auto action_it = actions.begin(); action_it != actions.end(); action_it++) { - if (action_it->type != ActionType::TURN) { - continue; - } - - if (std::next(action_it)->type == ActionType::TURN) { - const float turn_angle = action_it->value; - action_it = actions.erase(action_it); - action_it->value += turn_angle; - } - - auto [trim_before_distance, trim_after_distance] = this->get_trim_distances(*std::prev(action_it), *action_it); - - std::prev(action_it)->value -= trim_before_distance; - std::next(action_it)->value -= trim_after_distance; - } + this->add_diagonals(actions); + this->join_actions(actions); actions.front().value -= this->start_offset; float start_speed = 0.0F; @@ -230,6 +216,49 @@ Action::Id ActionQueuer::get_action(const GridPose& origin_pose, const GridPoint return {ActionType::STOP, cell_size / 2.0F}; } +void ActionQueuer::add_diagonals(std::list& actions) const { + for (auto action_it = actions.begin(); std::next(action_it) != actions.end(); action_it++) { + uint8_t diagonal_count = 1; + + while (action_it->type == ActionType::TURN and std::next(action_it)->type == ActionType::TURN and + std::abs(action_it->value) == std::numbers::pi_v / 2.0F and + action_it->value == -std::next(action_it)->value) { + diagonal_count++; + action_it = actions.erase(action_it); + } + + if (diagonal_count == 1) { + continue; + } + + action_it->value /= 2.0F; + const float first_angle = diagonal_count % 2 == 0 ? -action_it->value : action_it->value; + const float diagonal_distance = diagonal_count * this->cell_size * std::numbers::sqrt2_v / 2.0F; + actions.insert(action_it, {{ActionType::TURN, first_angle}, {ActionType::DIAGONAL, diagonal_distance}}); + } +} + +void ActionQueuer::join_actions(std::list& actions) { + for (auto action_it = actions.begin(); std::next(action_it) != actions.end(); action_it++) { + if (action_it->type != std::next(action_it)->type) { + continue; + } + + const float value = action_it->value; + action_it = actions.erase(action_it); + action_it->value += value; + + if (action_it->type != ActionType::TURN) { + continue; + } + + auto [trim_before_distance, trim_after_distance] = this->get_trim_distances(*std::prev(action_it), *action_it); + + std::prev(action_it)->value -= trim_before_distance; + std::next(action_it)->value -= trim_after_distance; + } +} + std::pair ActionQueuer::get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) { const float turn_angle = std::abs(turn_action.value); From 83d96b992eb0e4c29706865e4f3e79e5ca1f93ed Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 27 May 2025 17:00:20 -0300 Subject: [PATCH 455/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Change=20best=5Fro?= =?UTF-8?q?ute=20computation=20to=20recursive=20approach?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 6 +++ micras_nav/src/maze.cpp | 66 ++++++++++++-------------- 2 files changed, 37 insertions(+), 35 deletions(-) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index b643622..1eb0ac6 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -126,6 +126,10 @@ class TMaze : public core::ISerializable { */ std::pair get_next_bfs_goal(const GridPose& pose, bool discover) const; + void recursive_backtracking( + const GridPoint& position, std::list& route, std::unordered_set& visited + ); + float get_route_time(const std::list& route); /** @@ -189,6 +193,8 @@ class TMaze : public core::ISerializable { * @brief Current best found route to the goal. */ std::list best_route; + + float best_route_time{std::numeric_limits::max()}; }; } // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index 5703ee1..fea54ff 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -128,52 +128,48 @@ void TMaze::compute_minimum_cost() { template void TMaze::compute_best_route() { - std::list current_route; - std::stack stack; + std::unordered_set visited; + std::list current_route; - float best_route_time = std::numeric_limits::max(); - stack.push(this->start); + visited.insert(this->start.position); + current_route.push_back(this->start.position); - while (not stack.empty()) { - GridPose prev_pose = stack.top(); - stack.pop(); - current_route.push_back(prev_pose.position); - GridPoint current_position = prev_pose.front().position; + this->recursive_backtracking(this->start.position, current_route, visited); +} - bool dead_end = true; +template +void TMaze::recursive_backtracking( + const GridPoint& position, std::list& route, std::unordered_set& visited +) { + if (this->goal.contains(position)) { + float current_route_time = get_route_time(route); - for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - if (this->goal.contains(current_position)) { - current_route.push_back(current_position); - float current_route_time = get_route_time(current_route); + if (current_route_time < this->best_route_time) { + this->best_route_time = current_route_time; + this->best_route = route; + } - if (current_route_time < best_route_time) { - best_route_time = current_route_time; - best_route = current_route; - } + return; + } - break; - } + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { + Side side = static_cast(i); - Side side = static_cast(i); - GridPose next_pose = {current_position, side}; + if (this->costmap.has_wall({position, side}, true)) { + continue; + } - if (this->costmap.has_wall(next_pose, true) or - not this->was_visited(this->costmap.get_cell(next_pose.front().position))) { - continue; - } + GridPoint next_position = position + side; - if (std::find(current_route.begin(), current_route.end(), next_pose.position) == current_route.end()) { - stack.push(next_pose); - dead_end = false; - } + if (not this->was_visited(this->costmap.get_cell(next_position)) or visited.contains(next_position)) { + continue; } - if (dead_end and not stack.empty()) { - const GridPoint& intersection_point = stack.top().position; - auto intersection_it = std::find(current_route.begin(), current_route.end(), intersection_point); - current_route.erase(intersection_it, current_route.end()); - } + visited.insert(next_position); + route.push_back(next_position); + this->recursive_backtracking(next_position, route, visited); + visited.erase(next_position); + route.pop_back(); } } From 38c5fb2039b6bc554ac8522977d56a6549571b4b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 27 May 2025 18:53:35 -0300 Subject: [PATCH 456/561] =?UTF-8?q?=E2=9A=97=EF=B8=8F=20Add=20heuristic=20?= =?UTF-8?q?function=20to=20compute=20best=20route?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/maze.hpp | 2 ++ micras_nav/src/maze.cpp | 20 ++++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 1eb0ac6..5e0a138 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -130,6 +130,8 @@ class TMaze : public core::ISerializable { const GridPoint& position, std::list& route, std::unordered_set& visited ); + uint16_t heuristic(const GridPoint& position) const; + float get_route_time(const std::list& route); /** diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index fea54ff..da18469 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -152,6 +152,11 @@ void TMaze::recursive_backtracking( return; } + if (not this->best_route.empty() and + route.size() + this->heuristic(position) >= this->cost_margin * this->best_route.size()) { + return; + } + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { Side side = static_cast(i); @@ -173,6 +178,21 @@ void TMaze::recursive_backtracking( } } +template +uint16_t TMaze::heuristic(const GridPoint& position) const { + uint16_t minimum_distance = std::numeric_limits::max(); + + for (const auto& goal_position : this->goal) { + uint16_t distance = std::abs(position.x - goal_position.x) + std::abs(position.y - goal_position.y); + + if (distance < minimum_distance) { + minimum_distance = distance; + } + } + + return minimum_distance; +} + template const std::list& TMaze::get_best_route() const { return this->best_route; From 43d6d5aa41baa93b36a00ee148e87859ba30fe00 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 27 May 2025 22:57:48 -0300 Subject: [PATCH 457/561] =?UTF-8?q?=F0=9F=90=9B=20Fix=20action=20queuer=20?= =?UTF-8?q?join=20actions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/src/action_queuer.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index f209c39..e726cfb 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -239,16 +239,16 @@ void ActionQueuer::add_diagonals(std::list& actions) const { } void ActionQueuer::join_actions(std::list& actions) { - for (auto action_it = actions.begin(); std::next(action_it) != actions.end(); action_it++) { - if (action_it->type != std::next(action_it)->type) { + for (auto action_it = actions.begin(); std::next(action_it) != actions.end();) { + if (action_it->type == std::next(action_it)->type) { + const float value = action_it->value; + action_it = actions.erase(action_it); + action_it->value += value; continue; } - const float value = action_it->value; - action_it = actions.erase(action_it); - action_it->value += value; - if (action_it->type != ActionType::TURN) { + action_it++; continue; } @@ -256,6 +256,7 @@ void ActionQueuer::join_actions(std::list& actions) { std::prev(action_it)->value -= trim_before_distance; std::next(action_it)->value -= trim_after_distance; + action_it++; } } From 89cbc97e39755ca18d5a3a407929dbcc9e5813ba Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 30 May 2025 15:12:05 -0300 Subject: [PATCH 458/561] =?UTF-8?q?=E2=9C=A8=20Add=20signal=20handler?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/actions/turn.hpp | 9 ++++++++- micras_nav/src/action_queuer.cpp | 14 ++++++++------ 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index 0cba72c..a5586b7 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -7,6 +7,7 @@ #include #include +#include #include "micras/nav/actions/base.hpp" @@ -112,10 +113,16 @@ class TurnAction : public Action { const float transformed_acceleration = std::pow(1.0F - std::cos(angle), 2.0F) * correction_factor * max_angular_acceleration; - return std::sqrt( + const float max_angular_speed = std::sqrt( (max_centripetal_acceleration * transformed_acceleration) / (curve_radius * transformed_acceleration - max_centripetal_acceleration) ); + + if (std::isnan(max_angular_speed)) { + std::terminate(); + } + + return max_angular_speed; } /** diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index e726cfb..30e1427 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -143,6 +143,10 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_st continue; } + if (std::prev(action_it)->value < 0.0F) { + std::terminate(); + } + const CurveParameters& curve_parameters = this->get_curve_parameters(action_it->value, std::prev(action_it)->type == ActionType::DIAGONAL); @@ -153,18 +157,16 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_st .max_acceleration = this->solving_params.max_linear_acceleration, .max_deceleration = this->solving_params.max_linear_deceleration }; + this->action_queue.emplace_back(std::make_shared( + std::prev(action_it)->type, std::prev(action_it)->value, move_config, + std::prev(action_it)->type != ActionType::DIAGONAL + )); const TurnAction::Config turn_config = { .max_angular_speed = curve_parameters.max_angular_speed, .linear_speed = curve_parameters.linear_speed, .max_angular_acceleration = this->solving_params.max_angular_acceleration }; - - this->action_queue.emplace_back(std::make_shared( - std::prev(action_it)->type, std::prev(action_it)->value, move_config, - std::prev(action_it)->type != ActionType::DIAGONAL - )); - this->action_queue.emplace_back(std::make_shared(action_it->type, action_it->value, turn_config)); start_speed = curve_parameters.linear_speed; From 66969e19b9204c81a0351e0caff8ff5322af6a42 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 7 Jun 2025 21:23:56 -0300 Subject: [PATCH 459/561] =?UTF-8?q?=F0=9F=9A=B8=20Change=20imu=20submodule?= =?UTF-8?q?=20to=20CMake=20fetch?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/CMakeLists.txt | 17 +++++++++++++---- micras_proxy/libs/lsm6dsv-pid | 1 - 2 files changed, 13 insertions(+), 5 deletions(-) delete mode 160000 micras_proxy/libs/lsm6dsv-pid diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt index dc523d5..e1bc81b 100644 --- a/micras_proxy/CMakeLists.txt +++ b/micras_proxy/CMakeLists.txt @@ -1,25 +1,34 @@ project(micras_proxy) file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") -file(GLOB_RECURSE PROJECT_LIBS_SOURCES CONFIGURE_DEPENDS "libs/*.c*") + +include(FetchContent) +FetchContent_Declare( + lsm6dsv-pid + GIT_REPOSITORY https://github.com/STMicroelectronics/lsm6dsv-pid.git + GIT_TAG v3.3.0 +) + +FetchContent_Populate(lsm6dsv-pid) +file(GLOB_RECURSE LIB_SOURCES CONFIGURE_DEPENDS "${lsm6dsv-pid_SOURCE_DIR}/*.c") # Remove warnings from libraries sources set_source_files_properties( - ${PROJECT_LIBS_SOURCES} + ${LIB_SOURCES} PROPERTIES COMPILE_FLAGS "-w" ) add_library(${PROJECT_NAME} STATIC ${PROJECT_SOURCES} - ${PROJECT_LIBS_SOURCES} + ${LIB_SOURCES} ) add_library(micras::proxy ALIAS ${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} PUBLIC include - libs/lsm6dsv-pid + ${lsm6dsv-pid_SOURCE_DIR} ) target_link_libraries(${PROJECT_NAME} PUBLIC diff --git a/micras_proxy/libs/lsm6dsv-pid b/micras_proxy/libs/lsm6dsv-pid deleted file mode 160000 index ba840e9..0000000 --- a/micras_proxy/libs/lsm6dsv-pid +++ /dev/null @@ -1 +0,0 @@ -Subproject commit ba840e93355bbce604ceb2ee8d3eee8105588dc0 From 2565be339baf7b0dc083ee06cbb59c8ac415c07a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 7 Jun 2025 22:24:56 -0300 Subject: [PATCH 460/561] =?UTF-8?q?=F0=9F=9A=A8=20Fix=20linter=20warnings?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/action_queuer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index c7bbd31..4849858 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -59,7 +59,7 @@ class ActionQueuer { /** * @brief Push an exploring action to the queue. * - * @param current_pose Current pose of the robot. + * @param origin_pose Origin pose where the action will be performed. * @param target_position Target position to move to. */ void push_exploring(const GridPose& origin_pose, const GridPoint& target_position); From 724e4efd11fd09a4c40ac598ef250398c3770d5a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 15 Dec 2025 01:52:38 -0300 Subject: [PATCH 461/561] =?UTF-8?q?=E2=AC=86=EF=B8=8F=20Update=20imu=20lib?= =?UTF-8?q?rary=20to=205.1.1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_proxy/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt index e1bc81b..2b35f10 100644 --- a/micras_proxy/CMakeLists.txt +++ b/micras_proxy/CMakeLists.txt @@ -6,7 +6,7 @@ include(FetchContent) FetchContent_Declare( lsm6dsv-pid GIT_REPOSITORY https://github.com/STMicroelectronics/lsm6dsv-pid.git - GIT_TAG v3.3.0 + GIT_TAG v5.1.1 ) FetchContent_Populate(lsm6dsv-pid) From da777b1c92efc2a6cf7f40e9535846c0b281906e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 18 Sep 2026 05:59:24 -0300 Subject: [PATCH 462/561] =?UTF-8?q?=E2=AC=86=EF=B8=8F=20Move=20the=20toolc?= =?UTF-8?q?hain=20to=20Ubuntu=2026.04,=20clang=2022=20and=20C++23?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The base image brings arm-none-eabi-gcc 14.2, CMake 4.2 and doxygen 1.15. The clang tooling is pinned to a major version through a build argument, so that a distribution update cannot move the formatting rules or the enabled check set under the project. These changes are not separable: CMake 4.2 turns the deprecated FetchContent_Populate into an error, GCC 14 stopped providing transitively, the new warning set with -Werror needs its findings fixed in the same breath, and clang-tidy 22 refuses the tree until its new checks are either satisfied or suspended. C++23 costs 8 bytes of flash and no source changes. It also makes the enum work that clang-tidy 22 asks for affordable, through std::to_underlying. Warnings: - The flags cannot live in cube/cmake/gcc-arm-none-eabi.cmake, which STM32CubeMX regenerates, so they move to a micras::warnings target linked privately into the packages and the executables, never into the Cube sources - proxy::Storage reinterpreted an arbitrary byte offset as a T&, which is an unaligned access on Cortex-M; it now copies the bytes out - Two shifts of a uint8_t promoted to int overflowed a signed int, which is undefined - Default to Release and keep debug information in every configuration, so that local and CI builds match and a crash from a competition run can be symbolised Linting: - Replace the hand rolled parallel lint script with run-clang-tidy, which bounds the number of concurrent processes and serialises the fixes it applies to shared headers - Validate the configuration explicitly, since unknown check names are otherwise ignored - Disable checks for constructs this project cannot contain, worth 12% of lint time - Suspend the checks that ask for an API change or whose fixes do not compile here - Drop clang-analyzer-core.FixedAddressDereference, which every memory mapped peripheral access looks like by construction Includes: - Enable misc-include-cleaner, with the vendor headers out of scope because the CubeMX module headers are not standalone and need stm32h7xx_hal.h to have been seen first - Sort includes without promoting a main header, and disable llvm-include-order, so that clang-format is the single authority on include order --- .clang-format | 10 +++- .clang-tidy | 41 +++++++++++++- cmake/micras_warnings.cmake | 41 ++++++++++++++ cmake/templates/run_clang_tidy.sh.in | 55 +++++++++++-------- micras_core/CMakeLists.txt | 4 ++ micras_core/include/micras/core/utils.hpp | 2 +- micras_core/src/butterworth_filter.cpp | 1 + micras_core/src/fsm.cpp | 3 + micras_hal/CMakeLists.txt | 1 + micras_hal/include/micras/hal/flash.hpp | 2 +- micras_hal/stm32/src/adc_dma.cpp | 6 +- micras_hal/stm32/src/crc.cpp | 1 + micras_hal/stm32/src/encoder.cpp | 1 + micras_hal/stm32/src/flash.cpp | 5 +- micras_hal/stm32/src/pwm.cpp | 4 +- micras_hal/stm32/src/pwm_dma.cpp | 4 +- micras_hal/stm32/src/spi.cpp | 2 + micras_hal/stm32/src/timer.cpp | 1 + micras_nav/CMakeLists.txt | 4 ++ .../include/micras/nav/actions/move.hpp | 1 + micras_nav/src/action_queuer.cpp | 19 +++++-- micras_nav/src/costmap.cpp | 4 +- micras_nav/src/follow_wall.cpp | 6 ++ micras_nav/src/grid_pose.cpp | 3 + micras_nav/src/maze.cpp | 35 +++++++----- micras_nav/src/odometry.cpp | 3 + micras_nav/src/speed_controller.cpp | 3 +- micras_nav/src/state.cpp | 2 + micras_proxy/CMakeLists.txt | 6 +- micras_proxy/include/micras/proxy/argb.hpp | 2 +- micras_proxy/include/micras/proxy/storage.hpp | 4 +- micras_proxy/src/argb.cpp | 4 +- micras_proxy/src/buzzer.cpp | 1 + micras_proxy/src/dip_switch.cpp | 2 + micras_proxy/src/fan.cpp | 2 +- micras_proxy/src/imu.cpp | 3 + micras_proxy/src/rotary_sensor.cpp | 1 + micras_proxy/src/stopwatch.cpp | 1 + micras_proxy/src/storage.cpp | 5 ++ micras_proxy/src/torque_sensors.cpp | 3 + micras_proxy/src/wall_sensors.cpp | 6 +- 41 files changed, 243 insertions(+), 61 deletions(-) create mode 100644 cmake/micras_warnings.cmake diff --git a/.clang-format b/.clang-format index 5ab089a..358e344 100644 --- a/.clang-format +++ b/.clang-format @@ -1,5 +1,6 @@ --- Language: Cpp +BasedOnStyle: LLVM AccessModifierOffset: -4 AlignAfterOpenBracket: BlockIndent AlignConsecutiveBitFields: @@ -26,20 +27,25 @@ IncludeCategories: SortPriority: 1 - Regex: '^".*"' SortPriority: 2 -IncludeIsMainRegex: '$' +# No include is treated as the main header, so that sorting preserves the existing +# grouping instead of promoting the header matching the source file name +IncludeIsMainRegex: 'NO_MAIN_INCLUDE' IndentCaseLabels: true IndentExternBlock: NoIndent IndentPPDirectives: BeforeHash IndentRequiresClause: false IndentWidth: 4 IndentWrappedFunctionNames: true +InsertNewlineAtEOF: true KeepEmptyLinesAtTheStartOfBlocks: false +LineEnding: LF PackConstructorInitializers: NextLine PointerAlignment: Left SeparateDefinitionBlocks: Always ShortNamespaceLines: 0 -SortIncludes: Never +SortIncludes: CaseSensitive SpaceInEmptyBlock: true SpacesBeforeTrailingComments: 2 +Standard: Latest TabWidth: 4 ... diff --git a/.clang-tidy b/.clang-tidy index 37382f4..1915ed8 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,4 +1,17 @@ --- +# llvm-include-order enforces LLVM's own ordering, which contradicts the project's +# .clang-format rules; clang-format is the single authority on include order here. +# +# The last six entries are suspended pending review/stage-1-decisions-round-3.md R3-02. +# cppcoreguidelines-use-enum-class (43 sites) is an API change; the other three produce +# fixes that do not compile on this codebase, so they need to be applied by hand. +# +# The group before them disables checks for constructs this project cannot contain. They match +# nothing, so they only cost time scanning the standard library and the Cube headers: +# string_view, coroutines, Objective-C, GoogleTest, the LLVM internal APIs, std::bind, +# iostreams, std::auto_ptr and std::random_shuffle (both removed in C++17), and printf. +# It also drops clang-analyzer-core.FixedAddressDereference, which every memory mapped +# peripheral access looks like by construction. Checks: -*, bugprone-*, @@ -14,8 +27,8 @@ Checks: google-*, llvm-*, -llvm-header-guard, + -llvm-include-order, misc-*, - -misc-include-cleaner, -misc-use-anonymous-namespace, modernize-*, -modernize-use-nodiscard, @@ -25,6 +38,25 @@ Checks: -readability-duplicate-include, -readability-magic-numbers, -readability-math-missing-parentheses, + -bugprone-stringview-nullptr, + -cppcoreguidelines-no-suspend-with-lock, + -google-objc-*, + -google-upgrade-googletest-case, + -llvm-prefer-isa-or-dyn-cast-in-conditionals, + -llvm-prefer-register-over-unsigned, + -llvm-twine-local, + -modernize-avoid-bind, + -modernize-deprecated-ios-base-aliases, + -modernize-replace-auto-ptr, + -modernize-replace-random-shuffle, + -modernize-use-std-print, + -clang-analyzer-core.FixedAddressDereference, + -cppcoreguidelines-use-enum-class, + -modernize-use-designated-initializers, + -cppcoreguidelines-use-default-member-init, + -modernize-use-default-member-init, + -performance-unnecessary-value-param, + -cppcoreguidelines-pro-bounds-avoid-unchecked-container-access, HeaderFileExtensions: - '' - h @@ -36,9 +68,12 @@ ImplementationFileExtensions: - cc - cpp - cxx -HeaderFilterRegex: '(include|config)/.*.hpp' -FormatStyle: none +HeaderFilterRegex: '(include|config)/.*\.hpp$' +FormatStyle: file CheckOptions: + # The CubeMX HAL module headers are not standalone; they require stm32h7xx_hal.h to have + # pulled in stm32h7xx_hal_def.h first, so include-cleaner must not suggest or remove them + misc-include-cleaner.IgnoreHeaders: 'stm32.*;core_cm.*;cmsis.*;system_stm32.*;lsm6dsv.*' readability-identifier-length.MinimumVariableNameLength: 2 readability-identifier-length.IgnoredVariableNames: ^[nxyz]$ readability-identifier-length.MinimumParameterNameLength: 2 diff --git a/cmake/micras_warnings.cmake b/cmake/micras_warnings.cmake new file mode 100644 index 0000000..5f5a70d --- /dev/null +++ b/cmake/micras_warnings.cmake @@ -0,0 +1,41 @@ +############################################################################### +## Project Warning Flags +############################################################################### + +# These flags deliberately live here and not in cube/cmake/gcc-arm-none-eabi.cmake, +# because that file is regenerated by STM32CubeMX and any edit to it is lost. +# The target is linked privately into the micras_* libraries and the executables, so that +# the Cube sources are never compiled with it. + +option(MICRAS_WERROR "Treat compiler warnings as errors" OFF) + +add_library(micras_warnings INTERFACE) +add_library(micras::warnings ALIAS micras_warnings) + +# The -Wduplicated-* and -Wuseless-cast flags are GCC only +target_compile_options(micras_warnings INTERFACE + -Wall + -Wextra + -Wpedantic + -Wcast-align + -Wcast-qual + -Wdouble-promotion + -Wduplicated-branches + -Wduplicated-cond + -Wfloat-conversion + -Wformat=2 + -Wimplicit-fallthrough + -Wmisleading-indentation + -Wnull-dereference + -Wsign-conversion + -Wundef + -Wunused-parameter + -fno-common + $<$:-Werror> + + # C++ only, they make the C compiler warn about the command line itself + $<$:-Wnon-virtual-dtor> + $<$:-Wold-style-cast> + $<$:-Woverloaded-virtual> + $<$:-Wuseless-cast> +) diff --git a/cmake/templates/run_clang_tidy.sh.in b/cmake/templates/run_clang_tidy.sh.in index 8456b9e..3c2e0d1 100755 --- a/cmake/templates/run_clang_tidy.sh.in +++ b/cmake/templates/run_clang_tidy.sh.in @@ -1,31 +1,40 @@ #!/bin/bash -FIX_FLAG="" +# Positional arguments are the source files to check; they are passed to run-clang-tidy as +# path regexes matched against the entries of the compilation database. -if [[ "$1" == "--fix" ]]; then - FIX_FLAG="--fix" +RUN_FLAGS=() + +if [[ "${1:-}" == "--fix" ]]; then + RUN_FLAGS+=(-fix -format) shift fi +if [[ -t 1 ]]; then + RUN_FLAGS+=(-use-color) +fi + +# Unknown check names are silently ignored during a normal run, so the configuration is +# validated explicitly to keep it from rotting across clang-tidy versions. +clang-tidy --verify-config || exit 1 + echo "Running clang-tidy on all source files..." -ERRORS=$( - { - for file in "$@"; do - { - clang-tidy $FIX_FLAG --use-color --quiet --warnings-as-errors="*" \ - --extra-arg-before="--sysroot=@SYSROOT@/" \ - --extra-arg-before="-I@CXX_INCLUDE_DIR@/" \ - --extra-arg-before="-I@CXX_TRIPLE_INCLUDE_DIR@/" \ - -p @CMAKE_CURRENT_BINARY_DIR@ "$file" 2>&1 - } & - done - - wait - } | grep -Ev '[0-9]+ warnings? generated\.$' | awk '!seen[$0]++' -) - -if [[ -n "$ERRORS" ]]; then - echo -e "\n$ERRORS\033[0m" - exit 1 -fi +# run-clang-tidy bounds the number of concurrent clang-tidy processes, unlike a plain +# background job per file, and serializes the fixes it applies to shared headers. +# The filtered lines are the echoed clang-tidy invocations and the per-file diagnostic +# counters, which include the warnings suppressed in the Cube headers and are always noise. +# Real diagnostics name their own file, so nothing useful is hidden. +run-clang-tidy \ + "${RUN_FLAGS[@]}" \ + -quiet \ + -j "$(nproc)" \ + -warnings-as-errors="*" \ + -p @CMAKE_CURRENT_BINARY_DIR@ \ + -extra-arg-before=-Wno-unknown-warning-option \ + -extra-arg-before=--sysroot=@SYSROOT@/ \ + -extra-arg-before=-I@CXX_INCLUDE_DIR@/ \ + -extra-arg-before=-I@CXX_TRIPLE_INCLUDE_DIR@/ \ + "$@" 2>&1 | grep -Ev '^(\[[0-9]+/[0-9]+\]|[0-9]+ (warning|error)s? generated\.|clang-tidy(-[0-9.]+)? )' + +exit "${PIPESTATUS[0]}" diff --git a/micras_core/CMakeLists.txt b/micras_core/CMakeLists.txt index e4c6680..dbf768e 100644 --- a/micras_core/CMakeLists.txt +++ b/micras_core/CMakeLists.txt @@ -11,3 +11,7 @@ add_library(micras::core ALIAS ${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} PUBLIC include ) + +target_link_libraries(${PROJECT_NAME} PRIVATE + micras::warnings +) diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index ca788ac..5421e15 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -132,7 +132,7 @@ constexpr float assert_half_angle(float angle) { * @param tolerance Tolerance. * @return True if the numbers are near each other, false otherwise. */ -constexpr bool is_near(float x, float y, float tolerance = 0.001) { +constexpr bool is_near(float x, float y, float tolerance = 0.001F) { return std::abs(x - y) <= tolerance; } } // namespace micras::core diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index 3fe4650..b84b066 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -3,6 +3,7 @@ */ #include +#include #include #include "micras/core/butterworth_filter.hpp" diff --git a/micras_core/src/fsm.cpp b/micras_core/src/fsm.cpp index 9517969..c10bf97 100644 --- a/micras_core/src/fsm.cpp +++ b/micras_core/src/fsm.cpp @@ -2,6 +2,9 @@ * @file */ +#include +#include +#include #include "micras/core/fsm.hpp" namespace micras::core { diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt index c2da5ae..b38482b 100644 --- a/micras_hal/CMakeLists.txt +++ b/micras_hal/CMakeLists.txt @@ -20,6 +20,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PRIVATE cube_app STM32_Drivers + micras::warnings ) target_link_libraries(${PROJECT_NAME} PUBLIC diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp index fcdfa46..1d27635 100644 --- a/micras_hal/include/micras/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -22,7 +22,7 @@ class FlashWord { /** * @brief Number of 32 bit words inside a flash word. */ -#if defined(FLASH_NB_32BITWORD_IN_FLASHWORD) +#ifdef FLASH_NB_32BITWORD_IN_FLASHWORD static constexpr uint32_t words{FLASH_NB_32BITWORD_IN_FLASHWORD}; #else static constexpr uint32_t words{sizeof(uint64_t) / sizeof(uint32_t)}; diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 3485c6c..f04d0b8 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -3,6 +3,8 @@ */ #include +#include +#include #include "micras/hal/adc_dma.hpp" @@ -10,9 +12,9 @@ namespace micras::hal { AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, handle{config.handle} { config.init_function(); -#if defined(STM32H7) +#ifdef STM32H7 HAL_ADCEx_Calibration_Start(this->handle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED); -#elif defined(STM32G4) +#elifdef STM32G4 HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); #else #error "ADC calibration is only supported for STM32H7 and STM32G4 platforms." diff --git a/micras_hal/stm32/src/crc.cpp b/micras_hal/stm32/src/crc.cpp index 84c9980..e59a097 100644 --- a/micras_hal/stm32/src/crc.cpp +++ b/micras_hal/stm32/src/crc.cpp @@ -2,6 +2,7 @@ * @file */ +#include #include "micras/hal/crc.hpp" namespace micras::hal { diff --git a/micras_hal/stm32/src/encoder.cpp b/micras_hal/stm32/src/encoder.cpp index 419a1cb..68b1d85 100644 --- a/micras_hal/stm32/src/encoder.cpp +++ b/micras_hal/stm32/src/encoder.cpp @@ -3,6 +3,7 @@ */ #include +#include #include "micras/hal/encoder.hpp" diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index 6730066..7c393c7 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -4,7 +4,10 @@ #include #include -#include +#include +#include +#include +#include // NOLINT(misc-include-cleaner) #include "micras/hal/flash.hpp" diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index 60b3ee2..8aa271f 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -4,6 +4,7 @@ #include #include +#include #include "micras/hal/pwm.hpp" @@ -37,7 +38,8 @@ Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_cha } void Pwm::set_duty_cycle(float duty_cycle) { - const uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100.0F)); + const auto compare = + static_cast(std::lround(duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100.0F)); __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); } diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index fc9caac..78eb470 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -4,6 +4,8 @@ #include #include +#include +#include #include "micras/hal/pwm_dma.hpp" @@ -29,7 +31,7 @@ void PwmDma::stop_dma() { } uint32_t PwmDma::get_compare(float duty_cycle) const { - return std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100) - 1); + return static_cast(std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100) - 1)); } bool PwmDma::is_busy() { diff --git a/micras_hal/stm32/src/spi.cpp b/micras_hal/stm32/src/spi.cpp index 8c90cda..47517cc 100644 --- a/micras_hal/stm32/src/spi.cpp +++ b/micras_hal/stm32/src/spi.cpp @@ -2,6 +2,8 @@ * @file */ +#include +#include #include "micras/hal/spi.hpp" namespace micras::hal { diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index f5e532b..6b4a815 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -2,6 +2,7 @@ * @file */ +#include #include "micras/hal/timer.hpp" namespace micras::hal { diff --git a/micras_nav/CMakeLists.txt b/micras_nav/CMakeLists.txt index 0ddaacb..00e1670 100644 --- a/micras_nav/CMakeLists.txt +++ b/micras_nav/CMakeLists.txt @@ -15,3 +15,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC micras::proxy ) + +target_link_libraries(${PROJECT_NAME} PRIVATE + micras::warnings +) diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 0011493..3721389 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -5,6 +5,7 @@ #ifndef MICRAS_NAV_MOVE_ACTION_HPP #define MICRAS_NAV_MOVE_ACTION_HPP +#include #include #include "micras/nav/actions/base.hpp" diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 30e1427..86ca0d5 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -3,10 +3,19 @@ */ #include +#include +#include #include +#include +#include #include +#include #include "micras/nav/action_queuer.hpp" +#include "micras/nav/actions/base.hpp" +#include "micras/nav/actions/move.hpp" +#include "micras/nav/actions/turn.hpp" +#include "micras/nav/grid_pose.hpp" namespace micras::nav { ActionQueuer::ActionQueuer(Config config) : @@ -157,10 +166,12 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_st .max_acceleration = this->solving_params.max_linear_acceleration, .max_deceleration = this->solving_params.max_linear_deceleration }; - this->action_queue.emplace_back(std::make_shared( - std::prev(action_it)->type, std::prev(action_it)->value, move_config, - std::prev(action_it)->type != ActionType::DIAGONAL - )); + this->action_queue.emplace_back( + std::make_shared( + std::prev(action_it)->type, std::prev(action_it)->value, move_config, + std::prev(action_it)->type != ActionType::DIAGONAL + ) + ); const TurnAction::Config turn_config = { .max_angular_speed = curve_parameters.max_angular_speed, diff --git a/micras_nav/src/costmap.cpp b/micras_nav/src/costmap.cpp index bff20b8..5842514 100644 --- a/micras_nav/src/costmap.cpp +++ b/micras_nav/src/costmap.cpp @@ -6,9 +6,11 @@ #define MICRAS_NAV_COSTMAP_CPP #include +#include #include #include "micras/nav/costmap.hpp" +#include "micras/nav/grid_pose.hpp" namespace micras::nav { template @@ -71,7 +73,7 @@ void Costmap::recompute(const GridPoint& reference, uint8 continue; } - uint16_t old_cost = this->get_cost(current_position, layer); + int16_t old_cost = this->get_cost(current_position, layer); this->update_cost(current_position, layer, lowest_cost + 1); for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index fc24483..be5b121 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -2,7 +2,13 @@ * @file */ +#include +#include "micras/core/types.hpp" +#include "micras/core/utils.hpp" #include "micras/nav/follow_wall.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/state.hpp" +#include "micras/proxy/wall_sensors.hpp" namespace micras::nav { FollowWall::FollowWall(const std::shared_ptr>& wall_sensors, const Config& config) : diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index 21786f2..5af84cc 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -3,7 +3,10 @@ */ #include +#include +#include +#include "micras/core/vector.hpp" #include "micras/nav/grid_pose.hpp" namespace micras::nav { diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index da18469..f1559a9 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -5,10 +5,20 @@ #ifndef MICRAS_NAV_MAZE_CPP #define MICRAS_NAV_MAZE_CPP +#include +#include #include -#include +#include +#include +#include +#include +#include #include +#include +#include "micras/core/types.hpp" +#include "micras/nav/costmap.hpp" +#include "micras/nav/grid_pose.hpp" #include "micras/nav/maze.hpp" namespace micras::nav { @@ -123,7 +133,7 @@ bool TMaze::finished(const GridPoint& position, bool returning) c template void TMaze::compute_minimum_cost() { - this->minimum_cost = this->get_next_bfs_goal(this->start, false).second; + this->minimum_cost = static_cast(this->get_next_bfs_goal(this->start, false).second); } template @@ -166,7 +176,8 @@ void TMaze::recursive_backtracking( GridPoint next_position = position + side; - if (not this->was_visited(this->costmap.get_cell(next_position)) or visited.contains(next_position)) { + if (not TMaze::was_visited(this->costmap.get_cell(next_position)) or + visited.contains(next_position)) { continue; } @@ -183,11 +194,9 @@ uint16_t TMaze::heuristic(const GridPoint& position) const { uint16_t minimum_distance = std::numeric_limits::max(); for (const auto& goal_position : this->goal) { - uint16_t distance = std::abs(position.x - goal_position.x) + std::abs(position.y - goal_position.y); + const uint16_t distance = std::abs(position.x - goal_position.x) + std::abs(position.y - goal_position.y); - if (distance < minimum_distance) { - minimum_distance = distance; - } + minimum_distance = std::min(distance, minimum_distance); } return minimum_distance; @@ -235,7 +244,7 @@ void TMaze::update_cell(const GridPoint& position) { GridPoint dead_end_position = position; - while (this->is_dead_end(this->costmap.get_cell(dead_end_position))) { + while (TMaze::is_dead_end(this->costmap.get_cell(dead_end_position))) { for (Side side : {Side::UP, Side::DOWN, Side::LEFT, Side::RIGHT}) { if (not this->costmap.has_wall({dead_end_position, side}, true)) { this->costmap.add_virtual_wall({dead_end_position, side}); @@ -273,10 +282,10 @@ std::pair TMaze::get_next_bfs_goal(const Grid GridPose current_pose = queue.front(); queue.pop(); - if ((discover and - this->must_visit( - this->costmap.get_cell(current_pose.position), std::round(this->minimum_cost * this->cost_margin) - )) or + if ((discover and TMaze::must_visit( + this->costmap.get_cell(current_pose.position), + static_cast(std::round(this->minimum_cost * this->cost_margin)) + )) or (not discover and this->goal.contains(current_pose.position))) { result_pair = { {pose.position + current_pose.orientation, current_pose.orientation}, @@ -294,7 +303,7 @@ std::pair TMaze::get_next_bfs_goal(const Grid const GridPoint front_position = current_pose.position + side; - if ((discover or this->was_visited(this->costmap.get_cell(front_position))) and + if ((discover or TMaze::was_visited(this->costmap.get_cell(front_position))) and distance.at(front_position.y).at(front_position.x) == 0) { queue.emplace(front_position, current_pose.orientation); distance.at(front_position.y).at(front_position.x) = diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 5a53712..2741594 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -3,8 +3,11 @@ */ #include +#include #include "micras/nav/odometry.hpp" +#include "micras/proxy/imu.hpp" +#include "micras/proxy/rotary_sensor.hpp" namespace micras::nav { Odometry::Odometry( diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp index 2e70cff..2428e40 100644 --- a/micras_nav/src/speed_controller.cpp +++ b/micras_nav/src/speed_controller.cpp @@ -2,10 +2,11 @@ * @file */ -#include #include +#include #include "micras/nav/speed_controller.hpp" +#include "micras/nav/state.hpp" namespace micras::nav { SpeedController::SpeedController(const Config& config) : diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index ad206e4..0e73923 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -4,6 +4,8 @@ #include +#include "micras/core/vector.hpp" +#include "micras/nav/grid_pose.hpp" #include "micras/nav/state.hpp" namespace micras::nav { diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt index 2b35f10..2f3d1ce 100644 --- a/micras_proxy/CMakeLists.txt +++ b/micras_proxy/CMakeLists.txt @@ -9,7 +9,7 @@ FetchContent_Declare( GIT_TAG v5.1.1 ) -FetchContent_Populate(lsm6dsv-pid) +FetchContent_MakeAvailable(lsm6dsv-pid) file(GLOB_RECURSE LIB_SOURCES CONFIGURE_DEPENDS "${lsm6dsv-pid_SOURCE_DIR}/*.c") # Remove warnings from libraries sources @@ -34,3 +34,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC micras::hal ) + +target_link_libraries(${PROJECT_NAME} PRIVATE + micras::warnings +) diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index ba31624..eda38af 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -154,7 +154,7 @@ class TArgb { /** * @brief Data buffer to send to the addressable RGB LED. */ - std::array buffer{}; + std::array(num_of_leds) * bits_per_led + reset_length> buffer{}; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index b9c8d69..05cd064 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -6,6 +6,7 @@ #define MICRAS_PROXY_STORAGE_HPP #include +#include #include #include #include @@ -69,8 +70,7 @@ class Storage { void sync(const std::string& name, T& data) { if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr and this->primitives.at(name).size == sizeof(T)) { - // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) - data = reinterpret_cast(this->buffer.at(this->primitives.at(name).buffer_address)); + std::memcpy(&data, &this->buffer.at(this->primitives.at(name).buffer_address), sizeof(T)); } this->create(name, data); diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index bad7e37..41a8fc1 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -5,6 +5,7 @@ #ifndef MICRAS_PROXY_ARGB_CPP #define MICRAS_PROXY_ARGB_CPP +#include #include "micras/proxy/argb.hpp" namespace micras::proxy { @@ -63,7 +64,8 @@ void TArgb::update() { template void TArgb::encode_color(const Color& color, uint8_t index) { - uint32_t data = (color.green << (2 * bits_per_color)) | (color.red << bits_per_color) | color.blue; + uint32_t data = (static_cast(color.green) << (2 * bits_per_color)) | + (static_cast(color.red) << bits_per_color) | color.blue; for (uint32_t i = bits_per_led * index, j = bits_per_led - 1; i < bits_per_led * (index + 1U); i++, j--) { this->buffer.at(i) = ((data >> j) & 1) == 1 ? this->high_bit : this->low_bit; diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp index 0a2f035..700b114 100644 --- a/micras_proxy/src/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -2,6 +2,7 @@ * @file */ +#include #include "micras/proxy/buzzer.hpp" namespace micras::proxy { diff --git a/micras_proxy/src/dip_switch.cpp b/micras_proxy/src/dip_switch.cpp index 44f6f30..ea808e2 100644 --- a/micras_proxy/src/dip_switch.cpp +++ b/micras_proxy/src/dip_switch.cpp @@ -5,7 +5,9 @@ #ifndef MICRAS_PROXY_DIP_SWITCH_CPP #define MICRAS_PROXY_DIP_SWITCH_CPP +#include #include "micras/core/utils.hpp" +#include "micras/hal/gpio.hpp" #include "micras/proxy/dip_switch.hpp" namespace micras::proxy { diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp index 87cc932..9cf7067 100644 --- a/micras_proxy/src/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -52,7 +52,7 @@ void Fan::stop() { this->pwm.set_duty_cycle(0.0F); } -void Fan::set_direction(RotationDirection direction) { +void Fan::set_direction([[maybe_unused]] RotationDirection direction) { // this->direction_gpio.write(static_cast(direction)); } } // namespace micras::proxy diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 13b2171..a4ffc20 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -2,8 +2,11 @@ * @file */ +#include #include +#include +#include "lsm6dsv_reg.h" #include "micras/proxy/imu.hpp" #include "micras/proxy/stopwatch.hpp" diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index 0660c31..8eab7f1 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -3,6 +3,7 @@ */ #include +#include #include #include "micras/proxy/rotary_sensor.hpp" diff --git a/micras_proxy/src/stopwatch.cpp b/micras_proxy/src/stopwatch.cpp index f1c27ae..52a9b40 100644 --- a/micras_proxy/src/stopwatch.cpp +++ b/micras_proxy/src/stopwatch.cpp @@ -2,6 +2,7 @@ * @file */ +#include #include #include "micras/proxy/stopwatch.hpp" diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 6c528ca..3bad666 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -4,9 +4,14 @@ #include #include +#include +#include #include +#include +#include #include +#include "micras/core/serializable.hpp" #include "micras/hal/flash.hpp" #include "micras/proxy/storage.hpp" diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index d362f8a..f87bed1 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -5,7 +5,10 @@ #ifndef MICRAS_PROXY_TORQUE_SENSORS_CPP #define MICRAS_PROXY_TORQUE_SENSORS_CPP +#include +#include "micras/core/butterworth_filter.hpp" #include "micras/core/utils.hpp" +#include "micras/hal/adc_dma.hpp" #include "micras/proxy/torque_sensors.hpp" namespace micras::proxy { diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index 54d2aed..cdf80ff 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -5,7 +5,10 @@ #ifndef MICRAS_PROXY_WALL_SENSORS_CPP #define MICRAS_PROXY_WALL_SENSORS_CPP +#include +#include "micras/core/butterworth_filter.hpp" #include "micras/core/utils.hpp" +#include "micras/hal/pwm.hpp" #include "micras/proxy/wall_sensors.hpp" namespace micras::proxy { @@ -54,7 +57,8 @@ float TWallSensors::get_reading(uint8_t sensor_index) const { template float TWallSensors::get_adc_reading(uint8_t sensor_index) const { - return static_cast(std::abs(this->buffer.at(sensor_index) - this->buffer.at(sensor_index + num_of_sensors)) + return static_cast( + std::abs(this->buffer.at(sensor_index) - this->buffer.at(sensor_index + num_of_sensors)) ) / this->adc.get_max_reading(); } From 0797c37464ddb4e19c3168a225e26231b7772a02 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 18 Sep 2026 06:59:31 -0300 Subject: [PATCH 463/561] =?UTF-8?q?=F0=9F=9A=A8=20Scope=20every=20enum=20a?= =?UTF-8?q?nd=20make=20the=20integer=20conversions=20explicit?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Event, DipSwitchPins, State, Status, ActionType, CurveType, Side and Layer were all unscoped, which let them convert to their underlying type silently. That is what made `walls[Side::LEFT]` and `std::array` work, and equally what let an enumerator of one type be compared against another. They are now `enum class`, and every place that genuinely wants the number says so with std::to_underlying. Two things fell out of it: - Iterating the sides no longer counts integers. `for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++)` with a cast inside the body becomes `for (const Side side : all_sides)`, against a new constant in grid_pose.hpp. - Flash::Status hid a scoping subtlety: unqualified ERROR inside Flash resolved to the enumerator injected by the unscoped enum. With the enum scoped it would have resolved to the CMSIS ErrorStatus instead, so every use is now qualified. Also takes Maze::Config by const reference rather than by value, and routes the enum indexed container accesses through std::to_underlying. --- micras_hal/include/micras/hal/flash.hpp | 2 +- micras_hal/stm32/src/flash.cpp | 20 ++--- .../include/micras/nav/action_queuer.hpp | 7 +- micras_nav/include/micras/nav/grid_pose.hpp | 11 ++- micras_nav/include/micras/nav/maze.hpp | 15 ++-- micras_nav/src/action_queuer.cpp | 84 ++++++++++++------- micras_nav/src/costmap.cpp | 34 ++++---- micras_nav/src/grid_pose.cpp | 28 ++++--- micras_nav/src/maze.cpp | 59 +++++++------ micras_proxy/src/storage.cpp | 7 +- 10 files changed, 158 insertions(+), 109 deletions(-) diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp index 1d27635..2158ff0 100644 --- a/micras_hal/include/micras/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -82,7 +82,7 @@ class Flash { /** * @brief Enum for the status of a flash memory operation. */ - enum Status : uint8_t { + enum class Status : uint8_t { OK = 0, MISALIGNED = 1, OUT_OF_BOUNDS = 2, diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index 7c393c7..ed12d41 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -76,18 +76,18 @@ std::span Flash::read(uint16_t sector, uint32_t sector_address, u Flash::Status Flash::write(uint32_t address, std::span data) { if (address % FlashWord::size != 0) { - return MISALIGNED; + return Status::MISALIGNED; } if (address > total_size or align_size(data.size()) > total_size - address) { - return OUT_OF_BOUNDS; + return Status::OUT_OF_BOUNDS; } if (HAL_FLASH_Unlock() != HAL_OK) { - return ERROR; + return Status::ERROR; } - Status status = OK; + Status status = Status::OK; for (uint32_t offset = 0; offset < data.size(); offset += FlashWord::size) { const FlashWord word{data.subspan(offset)}; @@ -95,7 +95,7 @@ Flash::Status Flash::write(uint32_t address, std::span data) { if (HAL_FLASH_Program( FLASH_TYPEPROGRAM_FLASHWORD, base_address + address + offset, std::bit_cast(word.data()) ) != HAL_OK) { - status = ERROR; + status = Status::ERROR; break; } } @@ -107,7 +107,7 @@ Flash::Status Flash::write(uint32_t address, std::span data) { Flash::Status Flash::write(uint16_t sector, uint32_t sector_address, std::span data) { if (sector >= total_sectors or sector_address > sector_size) { - return OUT_OF_BOUNDS; + return Status::OUT_OF_BOUNDS; } return write(sector * sector_size + sector_address, data); @@ -115,7 +115,7 @@ Flash::Status Flash::write(uint16_t sector, uint32_t sector_address, std::span= total_sectors or number_of_sectors > total_sectors - start_sector) { - return OUT_OF_BOUNDS; + return Status::OUT_OF_BOUNDS; } FLASH_EraseInitTypeDef erase_struct = { @@ -127,7 +127,7 @@ Flash::Status Flash::erase_sectors(uint16_t start_sector, uint16_t number_of_sec }; if (HAL_FLASH_Unlock() != HAL_OK) { - return ERROR; + return Status::ERROR; } pFlash.ErrorCode = HAL_FLASH_ERROR_NONE; @@ -139,9 +139,9 @@ Flash::Status Flash::erase_sectors(uint16_t start_sector, uint16_t number_of_sec HAL_FLASH_Lock(); if (hal_status != HAL_OK or sector_error != 0xFFFFFFFFU) { - return ERROR; + return Status::ERROR; } - return OK; + return Status::OK; } } // namespace micras::hal diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 4849858..b63ef82 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include "micras/nav/actions/move.hpp" #include "micras/nav/actions/turn.hpp" @@ -21,7 +22,7 @@ class ActionQueuer { /** * @brief Enum for the exploration action types. */ - enum ActionType : uint8_t { + enum class ActionType : uint8_t { STOP = 0, START = 1, MOVE_FORWARD = 2, @@ -115,7 +116,7 @@ class ActionQueuer { /** * @brief Enum for the types of curves. */ - enum CurveType : uint8_t { + enum class CurveType : uint8_t { REGULAR_45 = 0, REGULAR_90 = 1, DIAGONAL_90 = 2, @@ -198,7 +199,7 @@ class ActionQueuer { /** * @brief Pre-computed curve parameters for different angles. */ - std::array curves_parameters{}; + std::array curves_parameters{}; /** * @brief Pre-built actions to use in the exploration. diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index 54c2779..d13f15a 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -5,8 +5,10 @@ #ifndef MICRAS_NAV_GRID_POSE_HPP #define MICRAS_NAV_GRID_POSE_HPP +#include #include #include +#include #include "micras/core/vector.hpp" @@ -14,13 +16,18 @@ namespace micras::nav { /** * @brief Possible sides in the grid. */ -enum Side : uint8_t { +enum class Side : uint8_t { RIGHT = 0, UP = 1, LEFT = 2, DOWN = 3 }; +/** + * @brief All the sides in the grid, in order, for iteration. + */ +constexpr std::array all_sides{Side::RIGHT, Side::UP, Side::LEFT, Side::DOWN}; + /** * @brief Convert an angle in radians to a Grid side. * @@ -166,7 +173,7 @@ template <> struct hash { size_t operator()(const micras::nav::GridPose& pose) const noexcept { const size_t h1 = hash{}(pose.position); - const size_t h2 = hash{}(pose.orientation); + const size_t h2 = hash{}(std::to_underlying(pose.orientation)); return h1 ^ (h2 << 1); } }; diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 5e0a138..435c8c0 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include "micras/core/serializable.hpp" #include "micras/core/types.hpp" @@ -40,7 +41,7 @@ class TMaze : public core::ISerializable { * * @param config The configuration for the maze. */ - explicit TMaze(Config config); + explicit TMaze(const Config& config); /** * @brief Update the maze walls with the current pose and new information. @@ -99,7 +100,7 @@ class TMaze : public core::ISerializable { /** * @brief The layers of the costmap. */ - enum Layer : uint8_t { + enum class Layer : uint8_t { EXPLORE = 0, RETURN = 1, NUM_OF_LAYERS = 2, @@ -140,7 +141,7 @@ class TMaze : public core::ISerializable { * @param cell The cell to check. * @return True if the cell is a dead end, false otherwise. */ - static bool is_dead_end(const Costmap::Cell& cell); + static bool is_dead_end(const Costmap::Cell& cell); /** * @brief Check if the cell was visited. @@ -148,7 +149,7 @@ class TMaze : public core::ISerializable { * @param cell The cell to check. * @return True if the cell was visited, false otherwise. */ - static bool was_visited(const Costmap::Cell& cell); + static bool was_visited(const Costmap::Cell& cell); /** * @brief Check if the cell must be visited. @@ -157,12 +158,14 @@ class TMaze : public core::ISerializable { * @param cost_threshold The cost threshold for the cell. * @return True if the cell must be visited, false otherwise. */ - static bool must_visit(const Costmap::Cell& cell, int16_t cost_threshold); + static bool must_visit( + const Costmap::Cell& cell, int16_t cost_threshold + ); /** * @brief Layered costmap for the maze. */ - Costmap costmap; + Costmap costmap; ActionQueuer action_queuer; diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 86ca0d5..1da9a77 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -42,22 +42,28 @@ ActionQueuer::ActionQueuer(Config config) : .max_deceleration = config.exploring.max_linear_deceleration }; - this->start = std::make_shared(ActionType::START, this->cell_size - config.start_offset, move_config); - this->move_half = std::make_shared(ActionType::MOVE_FORWARD, cell_size / 2.0F, move_config); + this->start = std::make_shared( + std::to_underlying(ActionType::START), this->cell_size - config.start_offset, move_config + ); + this->move_half = + std::make_shared(std::to_underlying(ActionType::MOVE_FORWARD), cell_size / 2.0F, move_config); move_config.start_speed = exploration_linear_speed; - this->move_forward = std::make_shared(ActionType::MOVE_FORWARD, cell_size, move_config); + this->move_forward = + std::make_shared(std::to_underlying(ActionType::MOVE_FORWARD), cell_size, move_config); this->move_to_turn = std::make_shared( - ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config, false + std::to_underlying(ActionType::MOVE_FORWARD), this->cell_size / 2.0F - exploration_curve_radius, move_config, + false ); this->move_from_turn = std::make_shared( - ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config + std::to_underlying(ActionType::MOVE_FORWARD), this->cell_size / 2.0F - exploration_curve_radius, move_config ); move_config.end_speed = 0.0F; - this->stop = std::make_shared(ActionType::STOP, cell_size / 2.0F, move_config, false); + this->stop = + std::make_shared(std::to_underlying(ActionType::STOP), cell_size / 2.0F, move_config, false); const TurnAction::Config turn_config = { .max_angular_speed = exploration_max_angular_speed, @@ -65,8 +71,12 @@ ActionQueuer::ActionQueuer(Config config) : .max_angular_acceleration = config.exploring.max_angular_acceleration }; - this->turn_left = std::make_shared(ActionType::TURN, std::numbers::pi_v / 2.0F, turn_config); - this->turn_right = std::make_shared(ActionType::TURN, -std::numbers::pi_v / 2.0F, turn_config); + this->turn_left = std::make_shared( + std::to_underlying(ActionType::TURN), std::numbers::pi_v / 2.0F, turn_config + ); + this->turn_right = std::make_shared( + std::to_underlying(ActionType::TURN), -std::numbers::pi_v / 2.0F, turn_config + ); const TurnAction::Config turn_back_config = { .max_angular_speed = config.exploring.max_angular_acceleration * 0.01F, @@ -74,7 +84,8 @@ ActionQueuer::ActionQueuer(Config config) : .max_angular_acceleration = config.exploring.max_angular_acceleration }; - this->turn_back = std::make_shared(ActionType::SPIN, std::numbers::pi_v, turn_back_config); + this->turn_back = + std::make_shared(std::to_underlying(ActionType::SPIN), std::numbers::pi_v, turn_back_config); this->compute_curve_parameters(std::numbers::pi_v / 4.0F, false); this->compute_curve_parameters(std::numbers::pi_v / 2.0F, false); @@ -137,7 +148,9 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_st Side direction = best_route.front().direction(*std::next(best_route.begin())); for (auto route_it = best_route.begin(); std::next(route_it) != best_route.end(); route_it++) { - actions.push_back(get_action({*route_it, direction}, *std::next(route_it), this->cell_size)); + actions.push_back( + get_action({.position = *route_it, .orientation = direction}, *std::next(route_it), this->cell_size) + ); direction = (*route_it).direction(*std::next(route_it)); } @@ -148,7 +161,7 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_st float start_speed = 0.0F; for (auto action_it = std::next(actions.begin()); action_it != actions.end(); action_it++) { - if (action_it->type != ActionType::TURN) { + if (action_it->type != std::to_underlying(ActionType::TURN)) { continue; } @@ -156,8 +169,9 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_st std::terminate(); } - const CurveParameters& curve_parameters = - this->get_curve_parameters(action_it->value, std::prev(action_it)->type == ActionType::DIAGONAL); + const CurveParameters& curve_parameters = this->get_curve_parameters( + action_it->value, std::prev(action_it)->type == std::to_underlying(ActionType::DIAGONAL) + ); const MoveAction::Config move_config = { .start_speed = start_speed, @@ -169,7 +183,7 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_st this->action_queue.emplace_back( std::make_shared( std::prev(action_it)->type, std::prev(action_it)->value, move_config, - std::prev(action_it)->type != ActionType::DIAGONAL + std::prev(action_it)->type != std::to_underlying(ActionType::DIAGONAL) ) ); @@ -191,13 +205,15 @@ void ActionQueuer::recompute(const std::list& best_route, bool add_st .max_deceleration = this->solving_params.max_linear_deceleration }; - if (actions.back().type == ActionType::MOVE_FORWARD) { + if (actions.back().type == std::to_underlying(ActionType::MOVE_FORWARD)) { this->action_queue.emplace_back( - std::make_shared(ActionType::STOP, actions.back().value + this->cell_size / 2.0F, stop_config) + std::make_shared( + std::to_underlying(ActionType::STOP), actions.back().value + this->cell_size / 2.0F, stop_config + ) ); } else { this->action_queue.emplace_back( - std::make_shared(ActionType::STOP, this->cell_size / 2.0F, stop_config) + std::make_shared(std::to_underlying(ActionType::STOP), this->cell_size / 2.0F, stop_config) ); } } @@ -215,25 +231,26 @@ float ActionQueuer::get_total_time() const { Action::Id ActionQueuer::get_action(const GridPose& origin_pose, const GridPoint& target_point, float cell_size) { if (origin_pose.front().position == target_point) { const float distance = origin_pose.position.to_vector(cell_size).distance(target_point.to_vector(cell_size)); - return {ActionType::MOVE_FORWARD, distance}; + return {.type = std::to_underlying(ActionType::MOVE_FORWARD), .value = distance}; } if (origin_pose.turned_left().front().position == target_point) { - return {ActionType::TURN, std::numbers::pi_v / 2.0F}; + return {.type = std::to_underlying(ActionType::TURN), .value = std::numbers::pi_v / 2.0F}; } if (origin_pose.turned_right().front().position == target_point) { - return {ActionType::TURN, -std::numbers::pi_v / 2.0F}; + return {.type = std::to_underlying(ActionType::TURN), .value = -std::numbers::pi_v / 2.0F}; } - return {ActionType::STOP, cell_size / 2.0F}; + return {.type = std::to_underlying(ActionType::STOP), .value = cell_size / 2.0F}; } void ActionQueuer::add_diagonals(std::list& actions) const { for (auto action_it = actions.begin(); std::next(action_it) != actions.end(); action_it++) { uint8_t diagonal_count = 1; - while (action_it->type == ActionType::TURN and std::next(action_it)->type == ActionType::TURN and + while (action_it->type == std::to_underlying(ActionType::TURN) and + std::next(action_it)->type == std::to_underlying(ActionType::TURN) and std::abs(action_it->value) == std::numbers::pi_v / 2.0F and action_it->value == -std::next(action_it)->value) { diagonal_count++; @@ -247,7 +264,10 @@ void ActionQueuer::add_diagonals(std::list& actions) const { action_it->value /= 2.0F; const float first_angle = diagonal_count % 2 == 0 ? -action_it->value : action_it->value; const float diagonal_distance = diagonal_count * this->cell_size * std::numbers::sqrt2_v / 2.0F; - actions.insert(action_it, {{ActionType::TURN, first_angle}, {ActionType::DIAGONAL, diagonal_distance}}); + actions.insert( + action_it, {{.type = std::to_underlying(ActionType::TURN), .value = first_angle}, + {.type = std::to_underlying(ActionType::DIAGONAL), .value = diagonal_distance}} + ); } } @@ -260,7 +280,7 @@ void ActionQueuer::join_actions(std::list& actions) { continue; } - if (action_it->type != ActionType::TURN) { + if (action_it->type != std::to_underlying(ActionType::TURN)) { action_it++; continue; } @@ -277,7 +297,7 @@ std::pair ActionQueuer::get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) { const float turn_angle = std::abs(turn_action.value); const CurveParameters& curve_parameters = - this->get_curve_parameters(turn_angle, action_before.type == ActionType::DIAGONAL); + this->get_curve_parameters(turn_angle, action_before.type == std::to_underlying(ActionType::DIAGONAL)); if (turn_angle == std::numbers::pi_v / 4.0F) { const float trim_before_distance = curve_parameters.forward_displacement - curve_parameters.side_displacement; @@ -290,7 +310,7 @@ std::pair float trim_before_distance = curve_parameters.forward_displacement; float trim_after_distance = curve_parameters.side_displacement; - if (action_before.type != ActionType::DIAGONAL) { + if (action_before.type != std::to_underlying(ActionType::DIAGONAL)) { trim_before_distance -= this->cell_size / 2.0F; trim_after_distance -= this->cell_size / 2.0F; } @@ -302,7 +322,7 @@ std::pair float trim_before_distance = curve_parameters.forward_displacement + curve_parameters.side_displacement; float trim_after_distance = std::numbers::sqrt2_v * curve_parameters.side_displacement; - if (action_before.type == ActionType::DIAGONAL) { + if (action_before.type == std::to_underlying(ActionType::DIAGONAL)) { trim_before_distance -= this->cell_size * std::numbers::sqrt2_v / 2.0F; trim_after_distance -= this->cell_size; } else { @@ -326,19 +346,19 @@ ActionQueuer::CurveParameters& ActionQueuer::get_curve_parameters(float angle, b const float turn_angle = std::abs(angle); if (turn_angle == std::numbers::pi_v / 4.0F) { - return this->curves_parameters[CurveType::REGULAR_45]; + return std::get(this->curves_parameters); } if (turn_angle == std::numbers::pi_v / 2.0F) { - return is_diagonal ? this->curves_parameters[CurveType::DIAGONAL_90] : - this->curves_parameters[CurveType::REGULAR_90]; + return is_diagonal ? std::get(this->curves_parameters) : + std::get(this->curves_parameters); } if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { - return this->curves_parameters[CurveType::REGULAR_135]; + return std::get(this->curves_parameters); } - return this->curves_parameters[CurveType::REGULAR_180]; + return std::get(this->curves_parameters); } void ActionQueuer::compute_curve_parameters(float angle, bool is_diagonal) { diff --git a/micras_nav/src/costmap.cpp b/micras_nav/src/costmap.cpp index 5842514..0fb9e9b 100644 --- a/micras_nav/src/costmap.cpp +++ b/micras_nav/src/costmap.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include "micras/nav/costmap.hpp" #include "micras/nav/grid_pose.hpp" @@ -16,13 +17,13 @@ namespace micras::nav { template Costmap::Costmap() { for (uint8_t row = 0; row < height; row++) { - this->cells[row][0].walls[Side::LEFT] = WallState::WALL; - this->cells[row][width - 1].walls[Side::RIGHT] = WallState::WALL; + this->cells[row][0].walls[std::to_underlying(Side::LEFT)] = WallState::WALL; + this->cells[row][width - 1].walls[std::to_underlying(Side::RIGHT)] = WallState::WALL; } for (uint8_t col = 0; col < width; col++) { - this->cells[0][col].walls[Side::DOWN] = WallState::WALL; - this->cells[height - 1][col].walls[Side::UP] = WallState::WALL; + this->cells[0][col].walls[std::to_underlying(Side::DOWN)] = WallState::WALL; + this->cells[height - 1][col].walls[std::to_underlying(Side::UP)] = WallState::WALL; } } @@ -35,8 +36,7 @@ void Costmap::compute(const GridPoint& reference, uint8_t GridPoint current_position = queue.front(); queue.pop(); - for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); + for (const Side side : all_sides) { GridPoint front_position = current_position + side; if (not this->has_wall({current_position, side})) { @@ -61,9 +61,7 @@ void Costmap::recompute(const GridPoint& reference, uint8 queue.pop(); int16_t lowest_cost = max_cost; - for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); - + for (const Side side : all_sides) { if (not this->has_wall({current_position, side})) { lowest_cost = std::min(lowest_cost, this->get_cost(current_position + side, layer)); } @@ -76,8 +74,7 @@ void Costmap::recompute(const GridPoint& reference, uint8 int16_t old_cost = this->get_cost(current_position, layer); this->update_cost(current_position, layer, lowest_cost + 1); - for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); + for (const Side side : all_sides) { GridPoint front_position = current_position + side; if (not this->has_wall({current_position, side}) and @@ -105,8 +102,9 @@ void Costmap::update_cost(const GridPoint& position, uint template bool Costmap::has_wall(const GridPose& pose, bool consider_virtual) const { - return this->get_cell(pose.position).walls[pose.orientation] == WallState::WALL or - (consider_virtual and this->get_cell(pose.position).walls[pose.orientation] == WallState::VIRTUAL); + return this->get_cell(pose.position).walls[std::to_underlying(pose.orientation)] == WallState::WALL or + (consider_virtual and + this->get_cell(pose.position).walls[std::to_underlying(pose.orientation)] == WallState::VIRTUAL); } template @@ -116,7 +114,8 @@ bool Costmap::update_wall(const GridPose& pose, bool wall } const bool updated = wall; - this->cell_on_position(pose.position).walls[pose.orientation] = wall ? WallState::WALL : WallState::NO_WALL; + this->cell_on_position(pose.position).walls[std::to_underlying(pose.orientation)] = + wall ? WallState::WALL : WallState::NO_WALL; GridPose front_pose = pose.front(); @@ -124,7 +123,7 @@ bool Costmap::update_wall(const GridPose& pose, bool wall return updated; } - this->cell_on_position(front_pose.position).walls[pose.turned_back().orientation] = + this->cell_on_position(front_pose.position).walls[std::to_underlying(pose.turned_back().orientation)] = wall ? WallState::WALL : WallState::NO_WALL; return updated; @@ -132,7 +131,7 @@ bool Costmap::update_wall(const GridPose& pose, bool wall template void Costmap::add_virtual_wall(const GridPose& pose) { - this->cell_on_position(pose.position).walls[pose.orientation] = WallState::VIRTUAL; + this->cell_on_position(pose.position).walls[std::to_underlying(pose.orientation)] = WallState::VIRTUAL; GridPose front_pose = pose.front(); @@ -140,7 +139,8 @@ void Costmap::add_virtual_wall(const GridPose& pose) { return; } - this->cell_on_position(front_pose.position).walls[pose.turned_back().orientation] = WallState::VIRTUAL; + this->cell_on_position(front_pose.position).walls[std::to_underlying(pose.turned_back().orientation)] = + WallState::VIRTUAL; } template diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index 5af84cc..cec2f23 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -2,6 +2,8 @@ * @file */ +#include + #include #include #include @@ -36,23 +38,23 @@ Side GridPoint::direction(const GridPoint& next) const { } GridPoint GridPoint::from_vector(const core::Vector& point, float cell_size) { - return {static_cast(point.x / cell_size), static_cast(point.y / cell_size)}; + return {.x = static_cast(point.x / cell_size), .y = static_cast(point.y / cell_size)}; } core::Vector GridPoint::to_vector(float cell_size) const { - return {cell_size * (this->x + 0.5F), cell_size * (this->y + 0.5F)}; + return {.x = cell_size * (this->x + 0.5F), .y = cell_size * (this->y + 0.5F)}; } GridPoint GridPoint::operator+(const Side& side) const { switch (side) { case Side::RIGHT: - return {static_cast(this->x + 1), this->y}; + return {.x = static_cast(this->x + 1), .y = this->y}; case Side::UP: - return {this->x, static_cast(this->y + 1)}; + return {.x = this->x, .y = static_cast(this->y + 1)}; case Side::LEFT: - return {static_cast(this->x - 1), this->y}; + return {.x = static_cast(this->x - 1), .y = this->y}; case Side::DOWN: - return {this->x, static_cast(this->y - 1)}; + return {.x = this->x, .y = static_cast(this->y - 1)}; } return *this; @@ -63,19 +65,25 @@ bool GridPoint::operator==(const GridPoint& other) const { } GridPose GridPose::front() const { - return {this->position + this->orientation, this->orientation}; + return {.position = this->position + this->orientation, .orientation = this->orientation}; } GridPose GridPose::turned_back() const { - return {this->position, static_cast((this->orientation + 2) % 4)}; + return { + .position = this->position, .orientation = static_cast((std::to_underlying(this->orientation) + 2) % 4) + }; } GridPose GridPose::turned_left() const { - return {this->position, static_cast((this->orientation + 1) % 4)}; + return { + .position = this->position, .orientation = static_cast((std::to_underlying(this->orientation) + 1) % 4) + }; } GridPose GridPose::turned_right() const { - return {this->position, static_cast((this->orientation + 3) % 4)}; + return { + .position = this->position, .orientation = static_cast((std::to_underlying(this->orientation) + 3) % 4) + }; } Side GridPose::get_relative_side(const GridPoint& other) const { diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index f1559a9..2271979 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -23,7 +23,7 @@ namespace micras::nav { template -TMaze::TMaze(Config config) : +TMaze::TMaze(const Config& config) : action_queuer(config.action_queuer_config), start{config.start}, goal{config.goal}, @@ -49,15 +49,15 @@ TMaze::TMaze(Config config) : } for (const auto& position : this->goal) { - this->costmap.update_cost(position, Layer::EXPLORE, 0); + this->costmap.update_cost(position, std::to_underlying(Layer::EXPLORE), 0); } for (const auto& position : this->goal) { - this->costmap.compute(position, Layer::EXPLORE); + this->costmap.compute(position, std::to_underlying(Layer::EXPLORE)); } - this->costmap.update_cost(this->start.position, Layer::RETURN, 0); - this->costmap.compute(this->start.position, Layer::RETURN); + this->costmap.update_cost(this->start.position, std::to_underlying(Layer::RETURN), 0); + this->costmap.compute(this->start.position, std::to_underlying(Layer::RETURN)); } template @@ -114,7 +114,10 @@ GridPose TMaze::get_next_goal(const GridPose& pose, bool returnin GridPoint front_position = pose.position + side; const int16_t flip_cost = pose.turned_back().orientation == side ? 1 : 0; const int16_t front_cost = - this->costmap.get_cost(front_position, returning ? Layer::RETURN : Layer::EXPLORE) + flip_cost; + this->costmap.get_cost( + front_position, returning ? std::to_underlying(Layer::RETURN) : std::to_underlying(Layer::EXPLORE) + ) + + flip_cost; if (front_cost < current_cost) { current_cost = front_cost; @@ -167,9 +170,7 @@ void TMaze::recursive_backtracking( return; } - for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); - + for (const Side side : all_sides) { if (this->costmap.has_wall({position, side}, true)) { continue; } @@ -227,7 +228,7 @@ void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { for (uint32_t i = 0; i < size; i += 2) { this->best_route.emplace_back( // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) - GridPoint{buffer[i], buffer[i + 1]} + GridPoint{.x = buffer[i], .y = buffer[i + 1]} ); } } @@ -235,11 +236,11 @@ void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { template void TMaze::update_cell(const GridPoint& position) { if (not this->goal.contains(position)) { - this->costmap.recompute(position, Layer::EXPLORE); + this->costmap.recompute(position, std::to_underlying(Layer::EXPLORE)); } if (position != this->start.position) { - this->costmap.recompute(position, Layer::RETURN); + this->costmap.recompute(position, std::to_underlying(Layer::RETURN)); } GridPoint dead_end_position = position; @@ -294,9 +295,7 @@ std::pair TMaze::get_next_bfs_goal(const Grid break; } - for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { - Side side = static_cast(i); - + for (const Side side : all_sides) { if (this->costmap.has_wall({current_pose.position, side})) { continue; } @@ -322,12 +321,14 @@ float TMaze::get_route_time(const std::list& route) { } template -bool TMaze::is_dead_end(const Costmap::Cell& cell) { +bool TMaze::is_dead_end( + const Costmap::Cell& cell +) { uint8_t wall_count = 0; for (const auto& wall : cell.walls) { - if (wall == Costmap::WallState::WALL or - wall == Costmap::WallState::VIRTUAL) { + if (wall == Costmap::WallState::WALL or + wall == Costmap::WallState::VIRTUAL) { wall_count++; } } @@ -336,20 +337,28 @@ bool TMaze::is_dead_end(const Costmap -bool TMaze::was_visited(const Costmap::Cell& cell) { +bool TMaze::was_visited( + const Costmap::Cell& cell +) { return not( - cell.walls[Side::UP] == Costmap::WallState::UNKNOWN or - cell.walls[Side::DOWN] == Costmap::WallState::UNKNOWN or - cell.walls[Side::LEFT] == Costmap::WallState::UNKNOWN or - cell.walls[Side::RIGHT] == Costmap::WallState::UNKNOWN + cell.walls[std::to_underlying(Side::UP)] == + Costmap::WallState::UNKNOWN or + cell.walls[std::to_underlying(Side::DOWN)] == + Costmap::WallState::UNKNOWN or + cell.walls[std::to_underlying(Side::LEFT)] == + Costmap::WallState::UNKNOWN or + cell.walls[std::to_underlying(Side::RIGHT)] == + Costmap::WallState::UNKNOWN ); } template bool TMaze::must_visit( - const Costmap::Cell& cell, int16_t cost_threshold + const Costmap::Cell& cell, int16_t cost_threshold ) { - return not was_visited(cell) and (cell.costs[Layer::EXPLORE] + cell.costs[Layer::RETURN] <= cost_threshold); + return not was_visited(cell) and + (cell.costs[std::to_underlying(Layer::EXPLORE)] + cell.costs[std::to_underlying(Layer::RETURN)] <= + cost_threshold); } } // namespace micras::nav diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 3bad666..538e82f 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -24,6 +24,7 @@ namespace micras::proxy { * @return Value read from the buffer. */ static uint16_t read_uint16(std::span buffer, uint16_t address) { + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) return static_cast(buffer[address] | buffer[address + 1U] << 8); } @@ -159,11 +160,11 @@ bool Storage::save() { return false; } - if (hal::Flash::erase_sectors(this->start_sector, this->number_of_sectors) != hal::Flash::OK) { + if (hal::Flash::erase_sectors(this->start_sector, this->number_of_sectors) != hal::Flash::Status::OK) { return false; } - if (hal::Flash::write(this->start_sector, 0, this->buffer) != hal::Flash::OK) { + if (hal::Flash::write(this->start_sector, 0, this->buffer) != hal::Flash::Status::OK) { return false; } @@ -200,7 +201,7 @@ bool Storage::deserialize_var_map( return false; } - const uint8_t var_name_len = buffer[current_addr]; + const uint8_t var_name_len = buffer.at(current_addr); if (current_addr + var_name_len + 5UL > buffer.size()) { return false; From 9b0bff4f028e97c61d27cfb9aaca1e935a227779 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 18 Sep 2026 06:59:31 -0300 Subject: [PATCH 464/561] =?UTF-8?q?=F0=9F=9A=A8=20Use=20std::get=20for=20t?= =?UTF-8?q?he=20constant=20container=20indices?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Every index here is a compile time constant, so std::get checks the bound at compile time and generates exactly the same code as operator[] did. An out of range index is now a build failure rather than a silent overrun. The two accesses into the storage buffer, which is deserialised from flash and therefore not trusted, use at() instead and pay for the check. The one in read_uint16 cannot, because std::span has no at() before C++26, and is annotated with the reason. --- micras_core/include/micras/core/utils.hpp | 2 +- micras_core/src/butterworth_filter.cpp | 23 ++++++++++---------- micras_proxy/src/imu.cpp | 26 +++++++++++------------ 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index 5421e15..60c3f60 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -68,7 +68,7 @@ constexpr T transition(T value, T start, T end, T resistance) { template constexpr std::array make_array(const std::array& parameters) { return [&](std::index_sequence) -> std::array { - return {T{parameters[I]}...}; + return {T{std::get(parameters)}...}; }(std::make_index_sequence()); } diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index b84b066..9de407b 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -2,6 +2,7 @@ * @file */ +#include #include #include #include @@ -22,18 +23,18 @@ ButterworthFilter::ButterworthFilter(float cutoff_frequency, float sampling_freq const float a1 = 2 - 8 / relative_frequency_2; const float a2 = 1 - 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; - this->a_array[0] = a2 / a0; - this->a_array[1] = a1 / a0; + std::get<0>(this->a_array) = a2 / a0; + std::get<1>(this->a_array) = a1 / a0; - this->b_array[0] = b2 / a0; - this->b_array[1] = b1 / a0; - this->b_array[2] = b0 / a0; + std::get<0>(this->b_array) = b2 / a0; + std::get<1>(this->b_array) = b1 / a0; + std::get<2>(this->b_array) = b0 / a0; } float ButterworthFilter::update(float x0) { - this->x_array[0] = this->x_array[1]; - this->x_array[1] = this->x_array[2]; - this->x_array[2] = x0; + std::get<0>(this->x_array) = std::get<1>(this->x_array); + std::get<1>(this->x_array) = std::get<2>(this->x_array); + std::get<2>(this->x_array) = x0; float x_b_dot = 0; @@ -49,13 +50,13 @@ float ButterworthFilter::update(float x0) { const float y0 = x_b_dot - y_a_dot; - this->y_array[0] = this->y_array[1]; - this->y_array[1] = y0; + std::get<0>(this->y_array) = std::get<1>(this->y_array); + std::get<1>(this->y_array) = y0; return y0; } float ButterworthFilter::get_last() const { - return this->y_array[1]; + return std::get<1>(this->y_array); } } // namespace micras::core diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index a4ffc20..f00a4fc 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -62,33 +62,33 @@ void Imu::update() { if (all_sources.drdy_xl) { lsm6dsv_acceleration_raw_get(&dev_ctx, raw_data.data()); - this->linear_acceleration[0] = raw_data[0] * xl_factor; - this->linear_acceleration[1] = raw_data[1] * xl_factor; - this->linear_acceleration[2] = raw_data[2] * xl_factor; + std::get<0>(this->linear_acceleration) = std::get<0>(raw_data) * xl_factor; + std::get<1>(this->linear_acceleration) = std::get<1>(raw_data) * xl_factor; + std::get<2>(this->linear_acceleration) = std::get<2>(raw_data) * xl_factor; } if (all_sources.drdy_gy) { lsm6dsv_angular_rate_raw_get(&dev_ctx, raw_data.data()); - this->angular_velocity[0] = raw_data[0] * gy_factor; - this->angular_velocity[1] = raw_data[1] * gy_factor; - this->angular_velocity[2] = raw_data[2] * gy_factor; + std::get<0>(this->angular_velocity) = std::get<0>(raw_data) * gy_factor; + std::get<1>(this->angular_velocity) = std::get<1>(raw_data) * gy_factor; + std::get<2>(this->angular_velocity) = std::get<2>(raw_data) * gy_factor; } if (not this->calibrated) { - this->calibration_filter.update(this->angular_velocity[2]); + this->calibration_filter.update(std::get<2>(this->angular_velocity)); } } float Imu::get_angular_velocity(Axis axis) const { switch (axis) { case Axis::X: - return this->angular_velocity[0]; + return std::get<0>(this->angular_velocity); case Axis::Y: - return this->angular_velocity[1]; + return std::get<1>(this->angular_velocity); case Axis::Z: - return this->angular_velocity[2] - this->calibration_filter.get_last(); + return std::get<2>(this->angular_velocity) - this->calibration_filter.get_last(); default: return 0.0F; @@ -98,13 +98,13 @@ float Imu::get_angular_velocity(Axis axis) const { float Imu::get_linear_acceleration(Axis axis) const { switch (axis) { case Axis::X: - return this->linear_acceleration[0]; + return std::get<0>(this->linear_acceleration); case Axis::Y: - return this->linear_acceleration[1]; + return std::get<1>(this->linear_acceleration); case Axis::Z: - return this->linear_acceleration[2]; + return std::get<2>(this->linear_acceleration); default: return 0.0F; From 676eb6d0db92ec9ed6a10c87bf149f564c5bb1bb Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 18 Sep 2026 06:59:31 -0300 Subject: [PATCH 465/561] =?UTF-8?q?=F0=9F=9A=A8=20Name=20the=20fields=20wh?= =?UTF-8?q?en=20returning=20aggregates?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit GridPoint, Vector, Pose and Action::Id all hold two same typed fields, so `return {a, b}` gives the compiler no way to catch a transposition. The maze code addresses cells as y then x in places and x then y in others, which is exactly the mistake this prevents. The nested points and poses in the maze and odometry configuration keep their positional form, where naming every field would only make the geometry harder to read. --- micras_core/src/vector.cpp | 8 ++++---- micras_nav/include/micras/nav/actions/move.hpp | 2 +- micras_nav/include/micras/nav/actions/turn.hpp | 2 +- micras_nav/src/odometry.cpp | 4 ++-- micras_nav/src/state.cpp | 10 ++++++---- 5 files changed, 14 insertions(+), 12 deletions(-) diff --git a/micras_core/src/vector.cpp b/micras_core/src/vector.cpp index e85f9ce..b1a551b 100644 --- a/micras_core/src/vector.cpp +++ b/micras_core/src/vector.cpp @@ -21,15 +21,15 @@ float Vector::angle_between(const Vector& other) const { Vector Vector::move_towards(const Vector& other, float distance) const { const float angle = this->angle_between(other); - return {this->x + distance * std::cos(angle), this->y + distance * std::sin(angle)}; + return {.x = this->x + distance * std::cos(angle), .y = this->y + distance * std::sin(angle)}; } Vector Vector::operator+(const Vector& other) const { - return {this->x + other.x, this->y + other.y}; + return {.x = this->x + other.x, .y = this->y + other.y}; } Vector Vector::operator-(const Vector& other) const { - return {this->x - other.x, this->y - other.y}; + return {.x = this->x - other.x, .y = this->y - other.y}; } bool Vector::operator==(const Vector& other) const { @@ -37,6 +37,6 @@ bool Vector::operator==(const Vector& other) const { } Vector Vector::operator%(float value) const { - return {std::fmod(this->x, value), std::fmod(this->y, value)}; + return {.x = std::fmod(this->x, value), .y = std::fmod(this->y, value)}; } } // namespace micras::core diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index 3721389..477caca 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -37,7 +37,7 @@ class MoveAction : public Action { */ MoveAction(uint8_t action_type, float distance, const Config& config, bool follow_wall = true) : Action{ - {action_type, distance}, + {.type = action_type, .value = distance}, follow_wall, calculate_total_time( distance, config.start_speed, config.end_speed, config.max_speed, config.max_acceleration, diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index a5586b7..7a37707 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -35,7 +35,7 @@ class TurnAction : public Action { */ TurnAction(uint8_t action_type, float angle, const Config& config) : Action{ - {action_type, angle}, + {.type = action_type, .value = angle}, false, calculate_total_time(angle, config.max_angular_speed, config.max_angular_acceleration) }, diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 2741594..f6e4a46 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -23,7 +23,7 @@ Odometry::Odometry( left_last_position{left_rotary_sensor->get_position()}, right_last_position{right_rotary_sensor->get_position()}, linear_filter{config.linear_cutoff_frequency}, - state{config.initial_pose, {0.0F, 0.0F}} { } + state{.pose = config.initial_pose, .velocity = {.linear = 0.0F, .angular = 0.0F}} { } void Odometry::update(float elapsed_time) { if (this->imu.use_count() == 1) { @@ -59,7 +59,7 @@ void Odometry::update(float elapsed_time) { void Odometry::reset() { this->left_last_position = this->left_rotary_sensor->get_position(); this->right_last_position = this->right_rotary_sensor->get_position(); - this->state = {this->initial_pose, {0.0F, 0.0F}}; + this->state = {.pose = this->initial_pose, .velocity = {.linear = 0.0F, .angular = 0.0F}}; } const nav::State& Odometry::get_state() const { diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 0e73923..4083eee 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -10,7 +10,9 @@ namespace micras::nav { GridPose Pose::to_grid(float cell_size) const { - return {GridPoint::from_vector(this->position, cell_size), angle_to_grid(this->orientation)}; + return { + .position = GridPoint::from_vector(this->position, cell_size), .orientation = angle_to_grid(this->orientation) + }; }; core::Vector Pose::to_cell(float cell_size) const { @@ -38,15 +40,15 @@ core::Vector Pose::to_cell(float cell_size) const { break; } - return {cell_alignment, cell_advance}; + return {.x = cell_alignment, .y = cell_advance}; } RelativePose::RelativePose(const Pose& absolute_pose) : absolute_pose{&absolute_pose} { } Pose RelativePose::get() const { return { - this->absolute_pose->position - this->reference_pose.position, - this->absolute_pose->orientation - this->reference_pose.orientation + .position = this->absolute_pose->position - this->reference_pose.position, + .orientation = this->absolute_pose->orientation - this->reference_pose.orientation }; } From f6cbb189ee73fed97ff3a0a73442f48824a7edda Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 18 Sep 2026 06:59:31 -0300 Subject: [PATCH 466/561] =?UTF-8?q?=F0=9F=9A=A8=20Re-enable=20the=20suspen?= =?UTF-8?q?ded=20clang-tidy=20checks?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit All six are now clean: cppcoreguidelines-use-enum-class, modernize-use-designated-initializers, performance-unnecessary-value-param, cppcoreguidelines-pro-bounds-avoid-unchecked-container-access and the two default member init checks, whose findings disappeared once the enums were scoped. --- .clang-tidy | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 1915ed8..86b8790 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -2,11 +2,7 @@ # llvm-include-order enforces LLVM's own ordering, which contradicts the project's # .clang-format rules; clang-format is the single authority on include order here. # -# The last six entries are suspended pending review/stage-1-decisions-round-3.md R3-02. -# cppcoreguidelines-use-enum-class (43 sites) is an API change; the other three produce -# fixes that do not compile on this codebase, so they need to be applied by hand. -# -# The group before them disables checks for constructs this project cannot contain. They match +# The trailing group disables checks for constructs this project cannot contain. They match # nothing, so they only cost time scanning the standard library and the Cube headers: # string_view, coroutines, Objective-C, GoogleTest, the LLVM internal APIs, std::bind, # iostreams, std::auto_ptr and std::random_shuffle (both removed in C++17), and printf. @@ -51,12 +47,6 @@ Checks: -modernize-replace-random-shuffle, -modernize-use-std-print, -clang-analyzer-core.FixedAddressDereference, - -cppcoreguidelines-use-enum-class, - -modernize-use-designated-initializers, - -cppcoreguidelines-use-default-member-init, - -modernize-use-default-member-init, - -performance-unnecessary-value-param, - -cppcoreguidelines-pro-bounds-avoid-unchecked-container-access, HeaderFileExtensions: - '' - h From 6df9f312cd7f71a3b2fd4003ed7875e09dbb1d10 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 18 Sep 2026 18:02:03 -0300 Subject: [PATCH 467/561] =?UTF-8?q?=F0=9F=90=9B=20Filter=20the=20padded=20?= =?UTF-8?q?progress=20counters=20from=20the=20lint=20output?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit run-clang-tidy right pads the file counter, so with more than nine files the echoed invocation reads [ 1/57] and not [1/57]. The noise filter demanded a digit right after the bracket, so the first nine invocations survived it and a clean run still printed nine command lines. --- cmake/templates/run_clang_tidy.sh.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/templates/run_clang_tidy.sh.in b/cmake/templates/run_clang_tidy.sh.in index 3c2e0d1..97a8b92 100755 --- a/cmake/templates/run_clang_tidy.sh.in +++ b/cmake/templates/run_clang_tidy.sh.in @@ -35,6 +35,6 @@ run-clang-tidy \ -extra-arg-before=--sysroot=@SYSROOT@/ \ -extra-arg-before=-I@CXX_INCLUDE_DIR@/ \ -extra-arg-before=-I@CXX_TRIPLE_INCLUDE_DIR@/ \ - "$@" 2>&1 | grep -Ev '^(\[[0-9]+/[0-9]+\]|[0-9]+ (warning|error)s? generated\.|clang-tidy(-[0-9.]+)? )' + "$@" 2>&1 | grep -Ev '^(\[ *[0-9]+/[0-9]+\]|[0-9]+ (warning|error)s? generated\.|clang-tidy(-[0-9.]+)? )' exit "${PIPESTATUS[0]}" From 9c17b555e808e55275fb5560ddbe751a0ee88037 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 18 Sep 2026 23:16:27 -0300 Subject: [PATCH 468/561] =?UTF-8?q?=F0=9F=9A=91=EF=B8=8F=20Fix=20the=20clo?= =?UTF-8?q?cks,=20the=20timebase=20and=20the=20SPI=20modes=20in=20micras?= =?UTF-8?q?=5Fhal?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The ADC kernel clock was never started: PeriphCommonClock_Config enables PLL2 and selects pll2_p_ck, nothing called it, and every ADC is configured for the asynchronous clock, so all three had no conversion clock at all. The failure was invisible because every HAL return value was discarded and the DMA buffers just stayed at zero. The microsecond timebase moves from TIM6 to the Cortex-M cycle counter. An APB1 timer runs at twice PCLK1 when the prescaler is not 1, so the 1 MHz check in the old constructor compared 500000 against 1000000, never started the timer and silently fell back to HAL_GetTick: the control loop ran at 500 Hz instead of 960 Hz and every elapsed time was quantised to a millisecond. The cycle counter resolves single cycles, costs no peripheral, and takes the wrap special case with it, since the subtraction now happens in cycles before the conversion. SPI3 carries three devices with two different modes. The clock polarity and phase become part of the device configuration, and select_device reprograms the bus when the device being selected disagrees with what is loaded. It also deasserts its own chip select before the bus exists, waits for the peripheral with a bound instead of leaving the caller to spin forever, and reports whether it came up. Also here: every wrapper records whether it started so that the application can refuse to drive on a peripheral that failed; the init function only runs when the handle is still in reset, so four channels of one timer no longer reinitialise it four times; GPIO reads IDR and writes BSRR directly, which is both faster and identical on every STM32; the PWM rounding stops calling libm; the CRC takes a byte span instead of a word array that was never words; the independent watchdog and an emergency stop that silences every PWM and driver enable pin from a fault path live in Mcu; and the filter math accelerator gets a wrapper. --- micras_hal/CMakeLists.txt | 21 +---- micras_hal/include/micras/hal/adc_dma.hpp | 36 +++++++- micras_hal/include/micras/hal/crc.hpp | 12 ++- micras_hal/include/micras/hal/encoder.hpp | 12 +++ micras_hal/include/micras/hal/fmac.hpp | 105 ++++++++++++++++++++++ micras_hal/include/micras/hal/mcu.hpp | 35 ++++++++ micras_hal/include/micras/hal/pwm.hpp | 12 +++ micras_hal/include/micras/hal/pwm_dma.hpp | 12 +++ micras_hal/include/micras/hal/spi.hpp | 50 ++++++++++- micras_hal/include/micras/hal/timer.hpp | 53 ++++++----- micras_hal/stm32/src/adc_dma.cpp | 34 +++++-- micras_hal/stm32/src/crc.cpp | 10 ++- micras_hal/stm32/src/encoder.cpp | 11 ++- micras_hal/stm32/src/fmac.cpp | 73 +++++++++++++++ micras_hal/stm32/src/gpio.cpp | 13 ++- micras_hal/stm32/src/mcu.cpp | 102 +++++++++++++++++++++ micras_hal/stm32/src/pwm.cpp | 24 +++-- micras_hal/stm32/src/pwm_dma.cpp | 17 +++- micras_hal/stm32/src/spi.cpp | 63 +++++++++++-- micras_hal/stm32/src/timer.cpp | 38 +++++--- 20 files changed, 640 insertions(+), 93 deletions(-) create mode 100644 micras_hal/include/micras/hal/fmac.hpp create mode 100644 micras_hal/stm32/src/fmac.cpp diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt index b38482b..3c0e599 100644 --- a/micras_hal/CMakeLists.txt +++ b/micras_hal/CMakeLists.txt @@ -1,12 +1,5 @@ project(micras_hal) -get_target_property(CUBE_INCLUDE_DIRECTORIES stm32cubemx INTERFACE_INCLUDE_DIRECTORIES) -get_target_property(CUBE_COMPILE_DEFINITIONS stm32cubemx INTERFACE_COMPILE_DEFINITIONS) - -# Remove warnings from Cube sources -target_compile_options(cube_app PRIVATE -w) -target_compile_options(STM32_Drivers PRIVATE -w) - file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") add_library(${PROJECT_NAME} STATIC @@ -17,22 +10,16 @@ target_include_directories(${PROJECT_NAME} PUBLIC include ) +# The object libraries have to be linked directly, since CMake only adds an object library's +# files to the targets that name it, never through an interface target target_link_libraries(${PROJECT_NAME} PRIVATE - cube_app - STM32_Drivers + ${MICRAS_CUBE_OBJECT_LIBRARIES} micras::warnings ) target_link_libraries(${PROJECT_NAME} PUBLIC micras::core -) - -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - ${CUBE_INCLUDE_DIRECTORIES} -) - -target_compile_definitions(${PROJECT_NAME} PUBLIC - ${CUBE_COMPILE_DEFINITIONS} + micras::cube ) add_library(micras_hal_whole_archive INTERFACE) diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 2a53da0..e838609 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -17,11 +17,15 @@ class AdcDma { public: /** * @brief Configuration struct for ADC DMA. + * + * @note The reference voltage is a property of the board, not of the silicon, so it is + * configured per instance next to the resolution instead of being fixed by this class. */ struct Config { void (*init_function)(); ADC_HandleTypeDef* handle; uint16_t max_reading; + float reference_voltage; }; /** @@ -35,15 +39,17 @@ class AdcDma { * @brief Enable ADC, start conversion of regular group and transfer result through DMA. * * @param buffer 32 bit destination buffer address. + * @return True if the conversion was started, false otherwise. */ - void start_dma(std::span buffer); + bool start_dma(std::span buffer); /** * @brief Enable ADC, start conversion of regular group and transfer result through DMA. * * @param buffer 16 bit destination buffer address. + * @return True if the conversion was started, false otherwise. */ - void start_dma(std::span buffer); + bool start_dma(std::span buffer); /** * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode). @@ -58,9 +64,21 @@ class AdcDma { uint16_t get_max_reading() const; /** - * @brief Reference voltage for the ADC measurement. + * @brief Get the reference voltage of the ADC measurement. + * + * @return Reference voltage in volts. + */ + float get_reference_voltage() const; + + /** + * @brief Check if the ADC was calibrated and started successfully. + * + * @note This is the flag that would have made a missing kernel clock visible instead of turning + * every analogue reading into a silent zero. + * + * @return True if the initialization was successful, false otherwise. */ - static constexpr float reference_voltage{3.3F}; + bool was_initialized() const; private: /** @@ -68,10 +86,20 @@ class AdcDma { */ uint16_t max_reading; + /** + * @brief Reference voltage for the ADC measurement. + */ + float reference_voltage; + /** * @brief ADC handle. */ ADC_HandleTypeDef* handle; + + /** + * @brief Flag to check if the ADC was calibrated and started. + */ + bool initialized{}; }; } // namespace micras::hal diff --git a/micras_hal/include/micras/hal/crc.hpp b/micras_hal/include/micras/hal/crc.hpp index e22dea8..649568d 100644 --- a/micras_hal/include/micras/hal/crc.hpp +++ b/micras_hal/include/micras/hal/crc.hpp @@ -6,6 +6,8 @@ #define MICRAS_HAL_CRC_HPP #include +#include +#include namespace micras::hal { /** @@ -28,13 +30,15 @@ class Crc { explicit Crc(const Config& config); /** - * @brief Calculate the CRC value. + * @brief Calculate the CRC value over a buffer of bytes. * - * @param data Data to calculate the CRC. - * @param size Size of the buffer. + * @note The polynomial, width and initial value come from the peripheral configuration, and the + * calculation unit is reset to that initial value on every call. + * + * @param data Data to calculate the CRC over. * @return CRC value. */ - uint32_t calculate(uint32_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) + uint32_t calculate(std::span data); private: /** diff --git a/micras_hal/include/micras/hal/encoder.hpp b/micras_hal/include/micras/hal/encoder.hpp index 39f4380..43705b4 100644 --- a/micras_hal/include/micras/hal/encoder.hpp +++ b/micras_hal/include/micras/hal/encoder.hpp @@ -36,6 +36,13 @@ class Encoder { */ int32_t get_counter() const; + /** + * @brief Check if the encoder was successfully started. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + private: /** * @brief Timer handle. @@ -46,6 +53,11 @@ class Encoder { * @brief Start value of the timer counter. */ uint32_t start_count; + + /** + * @brief Flag to check if the encoder was started. + */ + bool initialized{}; }; } // namespace micras::hal diff --git a/micras_hal/include/micras/hal/fmac.hpp b/micras_hal/include/micras/hal/fmac.hpp new file mode 100644 index 0000000..d25e525 --- /dev/null +++ b/micras_hal/include/micras/hal/fmac.hpp @@ -0,0 +1,105 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_FMAC_HPP +#define MICRAS_HAL_FMAC_HPP + +#include +#include +#include +#include + +namespace micras::hal { +/** + * @brief Class to handle the filter math accelerator on STM32 microcontrollers. + * + * @note The accelerator works in q1.15 fixed point and holds one filter configuration at a time, + * so a second filter cannot run without reprogramming the coefficient buffer. Both limits are + * properties of the peripheral, not of this wrapper. + */ +class Fmac { +public: + /** + * @brief Configuration struct for the filter math accelerator. + */ + struct Config { + void (*init_function)(); + FMAC_HandleTypeDef* handle; + }; + + /** + * @brief Construct a new Fmac object. + * + * @param config Configuration for the filter math accelerator. + */ + explicit Fmac(const Config& config); + + /** + * @brief Configure the accelerator as an infinite impulse response filter in direct form 1. + * + * @param feed_forward Coefficients applied to the input samples, most recent first, in q1.15. + * @param feedback Coefficients applied to the previous outputs, most recent first, in q1.15. + * @return True if the filter was configured and started, false otherwise. + */ + bool configure_iir(std::span feed_forward, std::span feedback); + + /** + * @brief Push one sample through the configured filter and read the matching output. + * + * @note The data registers are accessed directly rather than through the streaming API of the + * vendor HAL, whose per call bookkeeping costs more than the filter it is driving. + * + * @param sample Input sample in q1.15. + * @return Filtered value in q1.15. + */ + int16_t update(int16_t sample); + + /** + * @brief Check if the accelerator holds a running filter. + * + * @return True if a filter was configured and started, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Number of 16 bit words of the internal memory allocated to each buffer. + */ + static constexpr uint8_t buffer_size{8}; + + /** + * @brief Base addresses of the three buffers inside the internal memory. + */ + ///@{ + static constexpr uint8_t input_base_address{0}; + static constexpr uint8_t coefficient_base_address{buffer_size}; + static constexpr uint8_t output_base_address{2 * buffer_size}; + ///@} + + /** + * @brief Filter math accelerator handle. + */ + FMAC_HandleTypeDef* handle; + + /** + * @brief Output buffer registered with the vendor HAL when the filter is started. + * + * @note Never read, since update accesses the data register directly, but the HAL refuses to + * start a filter without one. + */ + std::array output_buffer{}; + + /** + * @brief Size of the output buffer, as the vendor HAL expects it by reference. + */ + uint16_t output_buffer_size{1}; + + /** + * @brief Flag to check if a filter is running. + */ + bool initialized{}; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_FMAC_HPP diff --git a/micras_hal/include/micras/hal/mcu.hpp b/micras_hal/include/micras/hal/mcu.hpp index 9414fad..17c49d4 100644 --- a/micras_hal/include/micras/hal/mcu.hpp +++ b/micras_hal/include/micras/hal/mcu.hpp @@ -5,6 +5,12 @@ #ifndef MICRAS_HAL_MCU_HPP #define MICRAS_HAL_MCU_HPP +#include +#include + +#include "micras/hal/gpio.hpp" +#include "micras/hal/pwm.hpp" + namespace micras::hal { /** * @brief Microcontroller unit class. @@ -20,6 +26,35 @@ class Mcu { * @brief Initialize MCU and some peripherals. */ static void init(); + + /** + * @brief Silence every actuator as directly as possible. + * + * @note Written to be callable from a fault or abort handler, so it touches only the timer and + * GPIO registers and relies on no object state. + * + * @param pwm_outputs Every PWM output to bring to a null duty cycle. + * @param enable_gpios Every driver enable pin to deassert. + */ + static void emergency_stop(std::span pwm_outputs, std::span enable_gpios); + + /** + * @brief Start the independent watchdog, or change the timeout of a running one. + * + * @note Once started the watchdog cannot be stopped by software, so anything that hangs for + * longer than the timeout resets the microcontroller, which brings every driver enable pin and + * every PWM output back to its reset state. The timeout can still be widened around an + * operation that legitimately stalls the core for longer than the control loop budget, such as + * erasing a flash sector. + * + * @param timeout_ms Time without a refresh that triggers a reset, in milliseconds. + */ + static void set_watchdog_timeout(uint32_t timeout_ms); + + /** + * @brief Refresh the independent watchdog. + */ + static void refresh_watchdog(); }; } // namespace micras::hal diff --git a/micras_hal/include/micras/hal/pwm.hpp b/micras_hal/include/micras/hal/pwm.hpp index 5bbd867..dc60e8b 100644 --- a/micras_hal/include/micras/hal/pwm.hpp +++ b/micras_hal/include/micras/hal/pwm.hpp @@ -50,6 +50,13 @@ class Pwm { */ void set_frequency(uint32_t frequency); + /** + * @brief Check if the PWM was successfully started. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + private: /** * @brief Timer handle. @@ -60,6 +67,11 @@ class Pwm { * @brief Channel number of the timer. */ uint32_t channel; + + /** + * @brief Flag to check if the PWM was started. + */ + bool initialized{}; }; } // namespace micras::hal diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp index 536d522..3b35902 100644 --- a/micras_hal/include/micras/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -65,6 +65,13 @@ class PwmDma { */ bool is_busy(); + /** + * @brief Check if the PWM was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + private: /** * @brief Timer handle. @@ -75,6 +82,11 @@ class PwmDma { * @brief Channel number of the timer. */ uint32_t channel; + + /** + * @brief Flag to check if the PWM was initialized. + */ + bool initialized{}; }; } // namespace micras::hal diff --git a/micras_hal/include/micras/hal/spi.hpp b/micras_hal/include/micras/hal/spi.hpp index 52c352c..15d2967 100644 --- a/micras_hal/include/micras/hal/spi.hpp +++ b/micras_hal/include/micras/hal/spi.hpp @@ -19,12 +19,18 @@ class Spi { public: /** * @brief SPI configuration struct. + * + * @note The clock polarity and phase belong to the device, not to the bus: several devices with + * different SPI modes can share one peripheral, and select_device reconfigures it when the mode + * of the device being selected differs from the one currently programmed. */ struct Config { void (*init_function)(); SPI_HandleTypeDef* handle; hal::Gpio::Config cs_gpio; uint32_t timeout; + uint32_t clock_polarity; + uint32_t clock_phase; }; /** @@ -35,7 +41,10 @@ class Spi { explicit Spi(const Config& config); /** - * @brief Activate the chip select. + * @brief Activate the chip select, reconfiguring the bus for this device if needed. + * + * @note Blocks for up to the configured timeout waiting for the peripheral to become ready, so + * that an SPI stuck in an error state is reported instead of hanging the caller. * * @return True if the device was successfully selected, false otherwise. */ @@ -50,15 +59,33 @@ class Spi { * @brief Transmit data over SPI. * * @param data Data to transmit. + * @return True if the transfer completed, false otherwise. */ - void transmit(std::span data); + bool transmit(std::span data); /** * @brief Receive data over SPI. * - * @param data Data to receive data. + * @param data Buffer to receive data into. + * @return True if the transfer completed, false otherwise. + */ + bool receive(std::span data); + + /** + * @brief Transmit and receive data over SPI in the same transfer. + * + * @param transmitted Data to transmit. + * @param received Buffer to receive data into, at least as large as the transmitted one. + * @return True if the transfer completed, false otherwise. + */ + bool transmit_receive(std::span transmitted, std::span received); + + /** + * @brief Check if the SPI was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. */ - void receive(std::span data); + bool was_initialized() const; private: /** @@ -75,6 +102,21 @@ class Spi { * @brief Timeout for the SPI operations in ms. */ uint32_t timeout; + + /** + * @brief Clock polarity required by the device behind this chip select. + */ + uint32_t clock_polarity; + + /** + * @brief Clock phase required by the device behind this chip select. + */ + uint32_t clock_phase; + + /** + * @brief Flag to check if the SPI was initialized. + */ + bool initialized{}; }; } // namespace micras::hal diff --git a/micras_hal/include/micras/hal/timer.hpp b/micras_hal/include/micras/hal/timer.hpp index 0ad0ada..434ec5a 100644 --- a/micras_hal/include/micras/hal/timer.hpp +++ b/micras_hal/include/micras/hal/timer.hpp @@ -6,33 +6,42 @@ #define MICRAS_HAL_TIMER_HPP #include -#include +#include namespace micras::hal { /** - * @brief Class to handle timer peripheral on STM32 microcontrollers. + * @brief Class providing the timebases of the microcontroller. + * + * @note The microsecond timebase comes from the Cortex-M cycle counter instead of a hardware timer: + * it costs no peripheral, resolves single cycles and is present on every Cortex-M3, M4 and M7, so + * the same implementation ports unchanged. Cortex-M0 and M0+ have no cycle counter and would need a + * timer here. */ class Timer { public: /** - * @brief Timer configuration struct. + * @brief Deleted constructor for static class. */ - struct Config { - void (*init_function)(); - TIM_HandleTypeDef* handle; - }; + Timer() = delete; /** - * @brief Construct a new Timer object. + * @brief Start the cycle counter and compute the conversion to microseconds. + * + * @note Must be called after the system clock is configured, since the conversion depends on + * the core clock frequency. */ - Timer() = default; + static void init(); /** - * @brief Construct a new Timer object. + * @brief Get the current value of the free running cycle counter. + * + * @note Wraps every 2^32 core clock cycles, which bounds the longest measurable interval to + * about 7.8 s at 550 MHz. Differences taken with unsigned arithmetic are correct across the + * wrap, so no special case is needed below that bound. * - * @param config Configuration for the timer. + * @return Current value of the cycle counter. */ - explicit Timer(const Config& config); + static uint32_t get_counter(); /** * @brief Get the current timer counter. @@ -42,22 +51,26 @@ class Timer { static uint32_t get_counter_ms(); /** - * @brief Get the current timer counter. + * @brief Convert a number of core clock cycles to microseconds. * - * @return Current timer counter in microseconds. + * @param cycles Number of core clock cycles. + * @return Equivalent time in microseconds. */ - uint32_t get_counter_us() const; + static uint32_t to_microseconds(uint32_t cycles); -private: /** - * @brief Timer handle. + * @brief Convert a time in microseconds to core clock cycles. + * + * @param microseconds Time in microseconds. + * @return Equivalent number of core clock cycles. */ - TIM_HandleTypeDef* handle{}; + static uint32_t to_cycles(uint32_t microseconds); +private: /** - * @brief Flag to enable microseconds. + * @brief Core clock cycles in one microsecond, resolved by init. */ - bool enable_microseconds{false}; + static uint32_t cycles_per_microsecond; }; } // namespace micras::hal diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index f04d0b8..2ac9269 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -9,24 +9,34 @@ #include "micras/hal/adc_dma.hpp" namespace micras::hal { -AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, handle{config.handle} { - config.init_function(); +AdcDma::AdcDma(const Config& config) : + max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { + if (this->handle->State == HAL_ADC_STATE_RESET) { + config.init_function(); + } #ifdef STM32H7 - HAL_ADCEx_Calibration_Start(this->handle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED); + const HAL_StatusTypeDef status = HAL_ADCEx_Calibration_Start(this->handle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED); #elifdef STM32G4 - HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); + const HAL_StatusTypeDef status = HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); #else #error "ADC calibration is only supported for STM32H7 and STM32G4 platforms." #endif + + this->initialized = status == HAL_OK; } -void AdcDma::start_dma(std::span buffer) { - HAL_ADC_Start_DMA(this->handle, buffer.data(), buffer.size()); +bool AdcDma::start_dma(std::span buffer) { + if (HAL_ADC_Start_DMA(this->handle, buffer.data(), buffer.size()) != HAL_OK) { + this->initialized = false; + return false; + } + + return true; } -void AdcDma::start_dma(std::span buffer) { - this->start_dma({std::bit_cast(buffer.data()), buffer.size()}); +bool AdcDma::start_dma(std::span buffer) { + return this->start_dma({std::bit_cast(buffer.data()), buffer.size()}); } void AdcDma::stop_dma() { @@ -36,4 +46,12 @@ void AdcDma::stop_dma() { uint16_t AdcDma::get_max_reading() const { return this->max_reading; } + +float AdcDma::get_reference_voltage() const { + return this->reference_voltage; +} + +bool AdcDma::was_initialized() const { + return this->initialized; +} } // namespace micras::hal diff --git a/micras_hal/stm32/src/crc.cpp b/micras_hal/stm32/src/crc.cpp index e59a097..149320c 100644 --- a/micras_hal/stm32/src/crc.cpp +++ b/micras_hal/stm32/src/crc.cpp @@ -2,14 +2,18 @@ * @file */ +#include #include +#include + #include "micras/hal/crc.hpp" namespace micras::hal { Crc::Crc(const Config& config) : handle{config.handle} { } -// NOLINTNEXTLINE(*-avoid-c-arrays) -uint32_t Crc::calculate(uint32_t data[], uint32_t size) { - return HAL_CRC_Calculate(this->handle, data, size); +uint32_t Crc::calculate(std::span data) { + // With CRC_INPUTDATA_FORMAT_BYTES the HAL reads the buffer byte by byte, so the word pointer in + // its signature describes the declaration rather than the access + return HAL_CRC_Calculate(this->handle, std::bit_cast(data.data()), data.size()); } } // namespace micras::hal diff --git a/micras_hal/stm32/src/encoder.cpp b/micras_hal/stm32/src/encoder.cpp index 68b1d85..b31da06 100644 --- a/micras_hal/stm32/src/encoder.cpp +++ b/micras_hal/stm32/src/encoder.cpp @@ -9,8 +9,11 @@ namespace micras::hal { Encoder::Encoder(const Config& config) : handle{config.handle} { - config.init_function(); - HAL_TIM_Encoder_Start(this->handle, config.timer_channel); + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + this->initialized = HAL_TIM_Encoder_Start(this->handle, config.timer_channel) == HAL_OK; // NOLINTNEXTLINE(cppcoreguidelines-prefer-member-initializer) this->start_count = __HAL_TIM_GET_AUTORELOAD(this->handle) / 2; @@ -20,4 +23,8 @@ Encoder::Encoder(const Config& config) : handle{config.handle} { int32_t Encoder::get_counter() const { return std::bit_cast(__HAL_TIM_GET_COUNTER(this->handle) - this->start_count); } + +bool Encoder::was_initialized() const { + return this->initialized; +} } // namespace micras::hal diff --git a/micras_hal/stm32/src/fmac.cpp b/micras_hal/stm32/src/fmac.cpp new file mode 100644 index 0000000..84888a5 --- /dev/null +++ b/micras_hal/stm32/src/fmac.cpp @@ -0,0 +1,73 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/fmac.hpp" + +namespace micras::hal { +Fmac::Fmac(const Config& config) : handle{config.handle} { + if (this->handle->State == HAL_FMAC_STATE_RESET) { + config.init_function(); + } +} + +bool Fmac::configure_iir(std::span feed_forward, std::span feedback) { + this->initialized = false; + + if (feed_forward.empty() or feedback.empty() or feed_forward.size() + feedback.size() > buffer_size) { + return false; + } + + FMAC_FilterConfigTypeDef filter_config{ + .InputBaseAddress = input_base_address, + .InputBufferSize = buffer_size, + .InputThreshold = FMAC_THRESHOLD_1, + .CoeffBaseAddress = coefficient_base_address, + .CoeffBufferSize = buffer_size, + .OutputBaseAddress = output_base_address, + .OutputBufferSize = buffer_size, + .OutputThreshold = FMAC_THRESHOLD_1, + // NOLINTBEGIN(cppcoreguidelines-pro-type-const-cast) the HAL only reads the coefficients + .pCoeffA = const_cast(feedback.data()), + .CoeffASize = static_cast(feedback.size()), + .pCoeffB = const_cast(feed_forward.data()), + .CoeffBSize = static_cast(feed_forward.size()), + // NOLINTEND(cppcoreguidelines-pro-type-const-cast) + .InputAccess = FMAC_BUFFER_ACCESS_POLLING, + .OutputAccess = FMAC_BUFFER_ACCESS_POLLING, + // Saturate instead of wrapping, so that a transient beyond the q1.15 range degrades the + // output rather than inverting it + .Clip = FMAC_CLIP_ENABLED, + .Filter = FMAC_FUNC_IIR_DIRECT_FORM_1, + .P = static_cast(feed_forward.size()), + .Q = static_cast(feedback.size()), + .R = 0, + }; + + if (HAL_FMAC_FilterConfig(this->handle, &filter_config) != HAL_OK) { + return false; + } + + if (HAL_FMAC_FilterStart(this->handle, this->output_buffer.data(), &this->output_buffer_size) != HAL_OK) { + return false; + } + + this->initialized = true; + return true; +} + +int16_t Fmac::update(int16_t sample) { + this->handle->Instance->WDATA = static_cast(sample); + + while ((this->handle->Instance->SR & FMAC_SR_YEMPTY) != 0) { } + + return static_cast(this->handle->Instance->RDATA & FMAC_WDATA_WDATA); +} + +bool Fmac::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/stm32/src/gpio.cpp b/micras_hal/stm32/src/gpio.cpp index 1d2b542..3486259 100644 --- a/micras_hal/stm32/src/gpio.cpp +++ b/micras_hal/stm32/src/gpio.cpp @@ -2,20 +2,27 @@ * @file */ +#include + #include "micras/hal/gpio.hpp" namespace micras::hal { +/** + * @brief Offset of the reset half of the bit set reset register. + */ +static constexpr uint32_t reset_offset{16}; + Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { } bool Gpio::read() const { - return HAL_GPIO_ReadPin(this->port, this->pin) == GPIO_PIN_SET; + return (this->port->IDR & this->pin) != 0; } void Gpio::write(bool state) { - HAL_GPIO_WritePin(this->port, this->pin, static_cast(state)); + this->port->BSRR = state ? this->pin : static_cast(this->pin) << reset_offset; } void Gpio::toggle() { - HAL_GPIO_TogglePin(this->port, this->pin); + this->write((this->port->ODR & this->pin) == 0); } } // namespace micras::hal diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp index c2abbdb..172c382 100644 --- a/micras_hal/stm32/src/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -2,11 +2,17 @@ * @file */ +#include #include +#include #include #include +#include +#include "micras/hal/gpio.hpp" #include "micras/hal/mcu.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/timer.hpp" extern "C" { /** @@ -15,18 +21,114 @@ extern "C" { * @note Defined by cube. */ void SystemClock_Config(); + +/** + * @brief Initialize the kernel clocks of the peripherals that do not run from an APB clock. + * + * @note Defined by cube. + */ +void PeriphCommonClock_Config(); } namespace micras::hal { +/** + * @brief Independent watchdog instance, spelled IWDG1 on the parts that have more than one. + */ +static IWDG_TypeDef* watchdog_instance() { +#ifdef IWDG1 + return IWDG1; +#else + return IWDG; +#endif +} + +/** + * @brief Key values of the independent watchdog key register. + */ +///@{ +static constexpr uint32_t watchdog_key_reload{0xAAAA}; +static constexpr uint32_t watchdog_key_write{0x5555}; +static constexpr uint32_t watchdog_key_start{0xCCCC}; +///@} + +/** + * @brief Smallest divider the watchdog prescaler can apply to the low speed oscillator. + */ +static constexpr uint32_t watchdog_min_divider{4}; + +/** + * @brief Largest value the watchdog prescaler register can hold. + */ +static constexpr uint32_t watchdog_max_prescaler{6}; + +/** + * @brief Largest value the 12 bit watchdog reload register can hold. + */ +static constexpr uint32_t watchdog_max_reload{0xFFF}; + +/** + * @brief Time to wait for the watchdog registers to take effect before giving up. + * + * @note Bounded so that a low speed oscillator that never starts cannot hang the boot. + */ +static constexpr uint32_t watchdog_timeout_us{1000}; + void Mcu::init() { SCB_EnableICache(); HAL_Init(); SystemClock_Config(); + PeriphCommonClock_Config(); + + Timer::init(); MX_GPIO_Init(); MX_DMA_Init(); MX_CRC_Init(); } + +void Mcu::emergency_stop(std::span pwm_outputs, std::span enable_gpios) { + for (const auto& pwm_output : pwm_outputs) { + __HAL_TIM_SET_COMPARE(pwm_output.handle, pwm_output.timer_channel, 0); + } + + for (const auto& enable_gpio : enable_gpios) { + HAL_GPIO_WritePin(enable_gpio.port, enable_gpio.pin, GPIO_PIN_RESET); + } +} + +void Mcu::set_watchdog_timeout(uint32_t timeout_ms) { + // The smallest prescaler that still fits the requested timeout in the 12 bit reload register + // gives the finest resolution + uint32_t prescaler = 0; + uint32_t ticks = timeout_ms * LSI_VALUE / (watchdog_min_divider * 1000); + + while (ticks > watchdog_max_reload + 1 and prescaler < watchdog_max_prescaler) { + prescaler++; + ticks /= 2; + } + + const uint32_t reload = std::clamp(ticks, 1, watchdog_max_reload + 1) - 1; + + watchdog_instance()->KR = watchdog_key_start; + watchdog_instance()->KR = watchdog_key_write; + watchdog_instance()->PR = prescaler; + watchdog_instance()->RLR = reload; + + const uint32_t start = Timer::get_counter(); + const uint32_t limit = Timer::to_cycles(watchdog_timeout_us); + + while ((watchdog_instance()->SR & (IWDG_SR_PVU | IWDG_SR_RVU)) != 0) { + if (Timer::get_counter() - start > limit) { + break; + } + } + + refresh_watchdog(); +} + +void Mcu::refresh_watchdog() { + watchdog_instance()->KR = watchdog_key_reload; +} } // namespace micras::hal diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index 8aa271f..3a659a1 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -3,7 +3,6 @@ */ #include -#include #include #include "micras/hal/pwm.hpp" @@ -32,14 +31,25 @@ static uint32_t get_timer_clock_frequency(const TIM_TypeDef* instance) { } Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel} { - config.init_function(); - HAL_TIM_PWM_Start(this->handle, this->channel); + // Several channels of the same timer are separate Pwm objects sharing one init function, and + // re-running it would regenerate an update event on a timer that is already counting + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + this->initialized = HAL_TIM_PWM_Start(this->handle, this->channel) == HAL_OK; __HAL_TIM_SET_COMPARE(this->handle, this->channel, 0); } void Pwm::set_duty_cycle(float duty_cycle) { - const auto compare = - static_cast(std::lround(duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100.0F)); + // Adding a half and truncating is exactly round half up for a duty cycle that is never + // negative, and it avoids the libm call std::lround would make in a path that runs five times + // per control loop + const float scaled = duty_cycle * static_cast(__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) * 0.01F; + + // NOLINTNEXTLINE(bugprone-incorrect-roundings) + const auto compare = static_cast(scaled + 0.5F); + __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); } @@ -51,4 +61,8 @@ void Pwm::set_frequency(uint32_t frequency) { __HAL_TIM_SET_AUTORELOAD(this->handle, autoreload); __HAL_TIM_SET_COUNTER(this->handle, 0); } + +bool Pwm::was_initialized() const { + return this->initialized; +} } // namespace micras::hal diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index 78eb470..09840fb 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -3,7 +3,6 @@ */ #include -#include #include #include @@ -11,7 +10,11 @@ namespace micras::hal { PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.timer_channel} { - config.init_function(); + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + this->initialized = this->handle->State == HAL_TIM_STATE_READY; } void PwmDma::start_dma(std::span buffer) { @@ -31,10 +34,18 @@ void PwmDma::stop_dma() { } uint32_t PwmDma::get_compare(float duty_cycle) const { - return static_cast(std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100) - 1)); + // Same expression as Pwm::set_duty_cycle, so that the two helpers cannot disagree + const float scaled = duty_cycle * static_cast(__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) * 0.01F; + + // NOLINTNEXTLINE(bugprone-incorrect-roundings) + return static_cast(scaled + 0.5F); } bool PwmDma::is_busy() { return TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_CHANNEL_STATE_BUSY; } + +bool PwmDma::was_initialized() const { + return this->initialized; +} } // namespace micras::hal diff --git a/micras_hal/stm32/src/spi.cpp b/micras_hal/stm32/src/spi.cpp index 47517cc..4756c8a 100644 --- a/micras_hal/stm32/src/spi.cpp +++ b/micras_hal/stm32/src/spi.cpp @@ -4,16 +4,45 @@ #include #include + #include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" namespace micras::hal { -Spi::Spi(const Config& config) : handle{config.handle}, cs_gpio{config.cs_gpio}, timeout{config.timeout} { - config.init_function(); +Spi::Spi(const Config& config) : + handle{config.handle}, + cs_gpio{config.cs_gpio}, + timeout{config.timeout}, + clock_polarity{config.clock_polarity}, + clock_phase{config.clock_phase} { + // Deassert before the bus exists, so that a device is never left selected by the reset state + // of its chip select pin + this->unselect_device(); + + if (this->handle->State == HAL_SPI_STATE_RESET) { + config.init_function(); + } + + this->initialized = this->handle->State == HAL_SPI_STATE_READY; } bool Spi::select_device() { - if (HAL_SPI_GetState(this->handle) != HAL_SPI_STATE_READY) { - return false; + const uint32_t start = Timer::get_counter(); + const uint32_t limit = Timer::to_cycles(1000 * this->timeout); + + while (HAL_SPI_GetState(this->handle) != HAL_SPI_STATE_READY) { + if (Timer::get_counter() - start > limit) { + return false; + } + } + + if (this->handle->Init.CLKPolarity != this->clock_polarity or this->handle->Init.CLKPhase != this->clock_phase) { + this->handle->Init.CLKPolarity = this->clock_polarity; + this->handle->Init.CLKPhase = this->clock_phase; + + if (HAL_SPI_Init(this->handle) != HAL_OK) { + return false; + } } this->cs_gpio.write(false); @@ -24,11 +53,29 @@ void Spi::unselect_device() { this->cs_gpio.write(true); } -void Spi::transmit(std::span data) { - HAL_SPI_Transmit(this->handle, data.data(), data.size(), this->timeout); +bool Spi::transmit(std::span data) { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-const-cast) the HAL takes a mutable pointer it only reads + auto* buffer = const_cast(data.data()); + + return HAL_SPI_Transmit(this->handle, buffer, data.size(), this->timeout) == HAL_OK; +} + +bool Spi::receive(std::span data) { + return HAL_SPI_Receive(this->handle, data.data(), data.size(), this->timeout) == HAL_OK; +} + +bool Spi::transmit_receive(std::span transmitted, std::span received) { + if (received.size() < transmitted.size()) { + return false; + } + + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-const-cast) the HAL takes a mutable pointer it only reads + auto* buffer = const_cast(transmitted.data()); + + return HAL_SPI_TransmitReceive(this->handle, buffer, received.data(), transmitted.size(), this->timeout) == HAL_OK; } -void Spi::receive(std::span data) { - HAL_SPI_Receive(this->handle, data.data(), data.size(), this->timeout); +bool Spi::was_initialized() const { + return this->initialized; } } // namespace micras::hal diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index 6b4a815..1500223 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -3,30 +3,44 @@ */ #include + #include "micras/hal/timer.hpp" namespace micras::hal { -Timer::Timer(const Config& config) : handle{config.handle} { - config.init_function(); +/** + * @brief Key that unlocks write access to the debug components on the Cortex-M7. + */ +static constexpr uint32_t software_lock_key{0xC5ACCE55}; - const uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); - const uint32_t prescaler = this->handle->Instance->PSC; +uint32_t Timer::cycles_per_microsecond{1}; - if (base_freq / (prescaler + 1) == 1000000) { - this->enable_microseconds = true; - HAL_TIM_Base_Start(this->handle); +void Timer::init() { + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + // Some Cortex-M7 implementations gate the debug registers behind a software lock + if ((DWT->LSR & ITM_LSR_Present_Msk) != 0 and (DWT->LSR & ITM_LSR_Access_Msk) != 0) { + DWT->LAR = software_lock_key; } + + DWT->CYCCNT = 0; + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; + + cycles_per_microsecond = SystemCoreClock / 1000000; +} + +uint32_t Timer::get_counter() { + return DWT->CYCCNT; } uint32_t Timer::get_counter_ms() { return HAL_GetTick(); } -uint32_t Timer::get_counter_us() const { - if (this->enable_microseconds) { - return __HAL_TIM_GET_COUNTER(this->handle); - } +uint32_t Timer::to_microseconds(uint32_t cycles) { + return cycles / cycles_per_microsecond; +} - return 1000 * HAL_GetTick(); +uint32_t Timer::to_cycles(uint32_t microseconds) { + return microseconds * cycles_per_microsecond; } } // namespace micras::hal From eb52e6286138bc3258d354bbf1c1093cf561f7d8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 18 Sep 2026 23:16:39 -0300 Subject: [PATCH 469/561] =?UTF-8?q?=F0=9F=9A=91=EF=B8=8F=20Correct=20the?= =?UTF-8?q?=20filter=20mathematics=20and=20index=20the=20FSM=20by=20state?= =?UTF-8?q?=20id?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The Butterworth filter had two independent errors in six lines. The sampling frequency defaulted to 100 Hz and every instantiation took the default while the control loop runs at 960 Hz, and the bilinear transform used 2 * sampling / cutoff where it needs cot(pi * cutoff / sampling), which is 2 pi too large. The first made the filters too narrow, the second made the parameter documented as hertz behave as radians per second, and together they nearly cancelled: a filter asking for 5 Hz was really running at 7.64 Hz. Both are fixed, so the cutoff is now the -3 dB point in hertz, exactly, at any ratio up to Nyquist rather than only when the cutoff is far below the sampling rate. The coefficients are also exposed, so anything else that wants to realise the same filter derives them from here instead of repeating the algebra. The state machine keyed its states by an unordered_map, so running it cost a hash lookup every control loop and two on a transition, for a dense enumeration from zero. It is now an array indexed by the state's own id. An id with no state behind it aborts, which runs the emergency stop, rather than dispatching through a null pointer with the motors turning. --- .../micras/core/butterworth_filter.hpp | 53 +++++-- micras_core/include/micras/core/fsm.hpp | 132 ++++++++++-------- micras_core/src/butterworth_filter.cpp | 37 +++-- micras_core/src/fsm.cpp | 35 +++-- 4 files changed, 166 insertions(+), 91 deletions(-) diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp index e68fd59..ed17392 100644 --- a/micras_core/include/micras/core/butterworth_filter.hpp +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -32,13 +32,51 @@ namespace micras::core { */ class ButterworthFilter { public: + /** + * @brief Order of the filter. + */ + static constexpr uint8_t filter_order{2}; + + /** + * @brief Configuration struct for the filter. + * + * @note Both frequencies are in hertz, and the cutoff is the -3 dB point of the resulting + * discrete filter, exactly, at any ratio up to Nyquist. + * + * @note The sampling frequency has no default on purpose: the discrete coefficients depend on + * the ratio of the cutoff to the sampling frequency, so a filter that assumes the wrong rate is + * silently a filter with the wrong cutoff. + */ + struct Config { + float cutoff_frequency; + float sampling_frequency; + }; + + /** + * @brief Coefficients of the discrete transfer function. + * + * @note Written in the natural order, most recent sample first, for the relation + * y[k] = sum(feed_forward[i] * x[k - i]) - sum(feedback[j] * y[k - 1 - j]). + */ + struct Coefficients { + std::array feed_forward; + std::array feedback; + }; + + /** + * @brief Compute the discrete coefficients of the filter. + * + * @param config Cutoff and sampling frequencies in Hz. + * @return Coefficients of the discrete transfer function. + */ + static Coefficients compute_coefficients(const Config& config); + /** * @brief Construct a new Butterworth Second Order filter object. * - * @param cutoff_frequency Low-pass cutoff frequency in Hz. - * @param sampling_frequency Sampling frequency in Hz. + * @param config Cutoff and sampling frequencies in Hz. */ - explicit ButterworthFilter(float cutoff_frequency, float sampling_frequency = 100.0F); + explicit ButterworthFilter(const Config& config); /** * @brief Produce a new value from measured data. @@ -56,11 +94,6 @@ class ButterworthFilter { float get_last() const; private: - /** - * @brief Order of the filter. - */ - static constexpr uint8_t filter_order{2}; - /** * @brief Last input values of the filter. */ @@ -72,12 +105,12 @@ class ButterworthFilter { std::array y_array{}; /** - * @brief Coefficients of the filter related to the output value. + * @brief Coefficients of the filter related to the output value, oldest first. */ std::array a_array{}; /** - * @brief Coefficients of the filter related to the input value. + * @brief Coefficients of the filter related to the input value, oldest first. */ std::array b_array{}; }; diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 9755c21..301c4df 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -5,76 +5,90 @@ #ifndef MICRAS_CORE_FSM_HPP #define MICRAS_CORE_FSM_HPP +#include #include #include -#include namespace micras::core { -class Fsm { +/** + * @brief A single state of a finite state machine. + */ +class FsmState { public: - class State { - public: - /** - * @brief Destroy the State object. - */ - virtual ~State() = default; - - /** - * @brief Execute the entry function of this state. - */ - virtual void on_entry() = 0; - - /** - * @brief Execute this state. - * - * @return The id of the next state. - */ - virtual uint8_t execute() = 0; - - /** - * @brief Get the id object of the state. - * - * @return The id of the state. - */ - uint8_t get_id() const; - - /** - * @brief The id of the state that is not valid. - */ - static constexpr uint8_t invalid_id{0xFF}; - - protected: - /** - * @brief Special member functions declared as default. - */ - ///@{ - explicit State(uint8_t id); - State(const State&) = default; - State(State&&) = default; - State& operator=(const State&) = default; - State& operator=(State&&) = default; - ///@} - - private: - /** - * @brief Fixed id of the state. - */ - uint8_t id; - }; + /** + * @brief Destroy the FsmState object. + */ + virtual ~FsmState() = default; + + /** + * @brief Execute the entry function of this state. + */ + virtual void on_entry() = 0; + + /** + * @brief Execute this state. + * + * @return The id of the next state. + */ + virtual uint8_t execute() = 0; /** - * @brief Construct a new FSM object. + * @brief Get the id object of the state. + * + * @return The id of the state. + */ + uint8_t get_id() const { return this->id; } + + /** + * @brief The id of the state that is not valid. + */ + static constexpr uint8_t invalid_id{0xFF}; + +protected: + /** + * @brief Special member functions declared as default. + */ + ///@{ + explicit FsmState(uint8_t id) : id{id} { } + + FsmState(const FsmState&) = default; + FsmState(FsmState&&) = default; + FsmState& operator=(const FsmState&) = default; + FsmState& operator=(FsmState&&) = default; + ///@} + +private: + /** + * @brief Fixed id of the state. + */ + uint8_t id; +}; + +/** + * @brief Finite state machine over a dense set of state ids. + * + * @note The states are stored in an array indexed by their own id, so running the machine costs an + * array access rather than a hash lookup. That only works because the ids are a dense enumeration + * from zero, which the number of states asserts. + * + * @tparam num_of_states Number of states, and therefore one past the largest valid id. + */ +template +class TFsm { +public: + /** + * @brief Construct a new TFsm object. * * @param initial_state_id The id of the initial state. */ - explicit Fsm(uint8_t initial_state_id); + explicit TFsm(uint8_t initial_state_id); /** - * @brief Add a state to the FSM. + * @brief Add a state to the FSM, taking ownership of it. * * @param state The state to be added. */ - void add_state(std::unique_ptr state); + void add_state(std::unique_ptr state); /** * @brief Run the FSM current state to compute the next state. @@ -83,9 +97,9 @@ class Fsm { private: /** - * @brief Map of ids to states. + * @brief States of the machine, indexed by their id. */ - std::unordered_map> states; + std::array, num_of_states> states{}; /** * @brief Id of the state currently running. @@ -95,8 +109,10 @@ class Fsm { /** * @brief Id of the last executed state. */ - uint8_t previous_state_id{State::invalid_id}; + uint8_t previous_state_id{FsmState::invalid_id}; }; } // namespace micras::core +#include "../src/fsm.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) + #endif // MICRAS_CORE_FSM_HPP diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index 9de407b..ced9614 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -10,25 +10,38 @@ #include "micras/core/butterworth_filter.hpp" namespace micras::core { -ButterworthFilter::ButterworthFilter(float cutoff_frequency, float sampling_frequency) { - const float relative_frequency = cutoff_frequency / sampling_frequency; - const float relative_frequency_2 = relative_frequency * relative_frequency; +ButterworthFilter::Coefficients ButterworthFilter::compute_coefficients(const Config& config) { + // The bilinear transform maps the analog prototype 1 / (s^2 + sqrt(2) s + 1) onto the unit + // circle by substituting s = warp (1 - z^-1) / (1 + z^-1), with warp chosen so the cutoff lands + // exactly where it was asked for, which is cot(pi * cutoff / sampling). The cotangent is the + // prewarping, and leaving it out is what would turn the cutoff into an angular frequency and + // put the real one a factor of 2 pi too low. + const float warp = 1.0F / std::tan(std::numbers::pi_v * config.cutoff_frequency / config.sampling_frequency); + const float warp_2 = warp * warp; const float b0 = 1; const float b1 = 2; const float b2 = 1; - // Butterworth filter coefficients - const float a0 = 1 + 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; - const float a1 = 2 - 8 / relative_frequency_2; - const float a2 = 1 - 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; + const float a0 = warp_2 + std::numbers::sqrt2_v * warp + 1; + const float a1 = 2 - 2 * warp_2; + const float a2 = warp_2 - std::numbers::sqrt2_v * warp + 1; - std::get<0>(this->a_array) = a2 / a0; - std::get<1>(this->a_array) = a1 / a0; + return { + .feed_forward = {b0 / a0, b1 / a0, b2 / a0}, + .feedback = {a1 / a0, a2 / a0}, + }; +} + +ButterworthFilter::ButterworthFilter(const Config& config) { + const Coefficients coefficients = compute_coefficients(config); + + std::get<0>(this->a_array) = std::get<1>(coefficients.feedback); + std::get<1>(this->a_array) = std::get<0>(coefficients.feedback); - std::get<0>(this->b_array) = b2 / a0; - std::get<1>(this->b_array) = b1 / a0; - std::get<2>(this->b_array) = b0 / a0; + std::get<0>(this->b_array) = std::get<2>(coefficients.feed_forward); + std::get<1>(this->b_array) = std::get<1>(coefficients.feed_forward); + std::get<2>(this->b_array) = std::get<0>(coefficients.feed_forward); } float ButterworthFilter::update(float x0) { diff --git a/micras_core/src/fsm.cpp b/micras_core/src/fsm.cpp index c10bf97..84ca9f5 100644 --- a/micras_core/src/fsm.cpp +++ b/micras_core/src/fsm.cpp @@ -2,32 +2,45 @@ * @file */ +#ifndef MICRAS_CORE_FSM_CPP +#define MICRAS_CORE_FSM_CPP + #include +#include #include #include + #include "micras/core/fsm.hpp" namespace micras::core { -Fsm::State::State(uint8_t id) : id{id} { } +template +TFsm::TFsm(uint8_t initial_state_id) : current_state_id{initial_state_id} { } -uint8_t Fsm::State::get_id() const { - return this->id; +template +void TFsm::add_state(std::unique_ptr state) { + this->states.at(state->get_id()) = std::move(state); } -Fsm::Fsm(uint8_t initial_state_id) : current_state_id{initial_state_id} { } +template +void TFsm::update() { + if (this->current_state_id >= num_of_states or this->states.at(this->current_state_id) == nullptr) { + // An id with no state behind it can only be a programming error. Aborting runs the + // emergency stop handler, which is a far better outcome than dispatching through a null + // pointer with the motors running. + std::abort(); + } -void Fsm::add_state(std::unique_ptr state) { - this->states.emplace(state->get_id(), std::move(state)); -} + FsmState& state = *this->states.at(this->current_state_id); -void Fsm::update() { if (this->current_state_id != this->previous_state_id) { - this->states.at(this->current_state_id)->on_entry(); + state.on_entry(); } - const uint8_t next_state = this->states.at(this->current_state_id)->execute(); + const uint8_t next_state_id = state.execute(); this->previous_state_id = this->current_state_id; - this->current_state_id = next_state; + this->current_state_id = next_state_id; } } // namespace micras::core + +#endif // MICRAS_CORE_FSM_CPP From 727d7b69f74f53de2b761d4f348b02dbd11d77dd Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 18 Sep 2026 23:16:55 -0300 Subject: [PATCH 470/561] =?UTF-8?q?=F0=9F=9A=91=EF=B8=8F=20Fix=20the=20sen?= =?UTF-8?q?sors=20and=20actuators=20in=20micras=5Fproxy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The magnetic encoders were talked to wrongly in four independent ways, so none of their configuration writes could ever be accepted and they ran on their factory defaults: the bus was in the wrong SPI mode, the 24-bit frame was transmitted least significant byte first where the sensor expects the opposite, the CRC was taken over the two bytes that happen to be first in memory rather than the two the sensor defines, and the CRC unit itself was computing CRC-32. The frame is now serialised explicitly, exchanged full duplex, and its answer is checked. The resolution follows: it is no longer a number in the configuration that has to agree with the ABIRES field, it is read back from the sensor and derived as four counts per pulse, so a configuration write that silently failed cannot turn into an odometry scaled by four. Locomotion scaled only one wheel on saturation, because the factor was computed from a variable the previous line had just overwritten, so the commanded ratio and with it the turn radius was lost exactly when it mattered. Fan::stop left the target speed alone and Micras::update ramped the fan straight back up, so stopping after a solve run did not stop the fan. Every DIP switch read inverted, since the package is wired to ground behind internal pull ups, which made the interface do the opposite of the legend. The torque calibration was not idempotent and destroyed itself on a second call. PwmDma underflowed to 0xFFFFFFFF at a null duty cycle and disagreed with Pwm about the same arithmetic. The IMU spent about a dozen SPI round trips per control loop, four of them pointless writes to the sensor, to read two data ready bits that STATUS_REG carries in one transaction. Also here: the wall sensor buffer documents the on/off cancellation scheme it depends on and the constructor checks the ADC scan length that has to match it; the stopwatch stops reaching past micras_hal into the vendor HAL and keeps its two timebases apart; the storage looks each variable up once; and a filter that runs on the math accelerator is available as an alternative to the software one. --- micras_proxy/include/micras/proxy/argb.hpp | 7 + micras_proxy/include/micras/proxy/battery.hpp | 18 +- micras_proxy/include/micras/proxy/buzzer.hpp | 7 + .../include/micras/proxy/dip_switch.hpp | 9 + micras_proxy/include/micras/proxy/fan.hpp | 45 ++--- .../include/micras/proxy/fmac_filter.hpp | 122 +++++++++++++ micras_proxy/include/micras/proxy/imu.hpp | 4 +- .../include/micras/proxy/locomotion.hpp | 7 + micras_proxy/include/micras/proxy/motor.hpp | 7 + .../include/micras/proxy/rotary_sensor.hpp | 91 +++++++++- .../micras/proxy/rotary_sensor_reg.hpp | 2 +- .../include/micras/proxy/stopwatch.hpp | 32 ++-- micras_proxy/include/micras/proxy/storage.hpp | 13 +- .../include/micras/proxy/torque_sensors.hpp | 24 ++- .../include/micras/proxy/wall_sensors.hpp | 28 ++- micras_proxy/src/argb.cpp | 5 + micras_proxy/src/battery.cpp | 11 +- micras_proxy/src/buzzer.cpp | 4 + micras_proxy/src/dip_switch.cpp | 6 +- micras_proxy/src/fan.cpp | 31 ++-- micras_proxy/src/fmac_filter.cpp | 71 ++++++++ micras_proxy/src/imu.cpp | 35 ++-- micras_proxy/src/locomotion.cpp | 19 +- micras_proxy/src/motor.cpp | 4 + micras_proxy/src/rotary_sensor.cpp | 168 ++++++++++++------ micras_proxy/src/stopwatch.cpp | 32 ++-- micras_proxy/src/storage.cpp | 7 +- micras_proxy/src/torque_sensors.cpp | 21 ++- micras_proxy/src/wall_sensors.cpp | 18 +- 29 files changed, 652 insertions(+), 196 deletions(-) create mode 100644 micras_proxy/include/micras/proxy/fmac_filter.hpp create mode 100644 micras_proxy/src/fmac_filter.cpp diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index eda38af..d7b62df 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -107,6 +107,13 @@ class TArgb { */ void update(); + /** + * @brief Check if the PWM was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + private: /** * @brief Encode a color into the data buffer. diff --git a/micras_proxy/include/micras/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp index e6d71d0..ce3a56f 100644 --- a/micras_proxy/include/micras/proxy/battery.hpp +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -20,9 +20,9 @@ class Battery { * @brief Configuration struct for the battery. */ struct Config { - hal::AdcDma::Config adc; - float voltage_divider; - float filter_cutoff; + hal::AdcDma::Config adc; + float voltage_divider; + core::ButterworthFilter::Config filter; }; /** @@ -58,6 +58,13 @@ class Battery { */ float get_adc_reading() const; + /** + * @brief Check if the ADC was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + private: /** * @brief Adc object. @@ -78,6 +85,11 @@ class Battery { * @brief Butterworth filter for the battery reading. */ core::ButterworthFilter filter; + + /** + * @brief Flag to check if the ADC was initialized. + */ + bool initialized{}; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/buzzer.hpp b/micras_proxy/include/micras/proxy/buzzer.hpp index 8aa18ca..38f05be 100644 --- a/micras_proxy/include/micras/proxy/buzzer.hpp +++ b/micras_proxy/include/micras/proxy/buzzer.hpp @@ -55,6 +55,13 @@ class Buzzer { */ void wait(uint32_t interval); + /** + * @brief Check if the PWM was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + private: /** * @brief PWM object. diff --git a/micras_proxy/include/micras/proxy/dip_switch.hpp b/micras_proxy/include/micras/proxy/dip_switch.hpp index 7bfa4ed..84e814f 100644 --- a/micras_proxy/include/micras/proxy/dip_switch.hpp +++ b/micras_proxy/include/micras/proxy/dip_switch.hpp @@ -19,9 +19,13 @@ class TDipSwitch { public: /** * @brief Configuration struct for the Dip Switch. + * + * @note A switch wired between the pin and ground with an internal pull up reads low when it is + * closed, so the sense of the reading is a property of the board and not of this class. */ struct Config { std::array gpio_array; + bool active_low; }; /** @@ -51,6 +55,11 @@ class TDipSwitch { * @brief Array of GPIOs for the switches. */ std::array gpio_array; + + /** + * @brief Whether a closed switch pulls its pin low. + */ + bool active_low; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index d2b327f..630b802 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -5,8 +5,6 @@ #ifndef MICRAS_PROXY_FAN_HPP #define MICRAS_PROXY_FAN_HPP -#include - #include "micras/hal/gpio.hpp" #include "micras/hal/pwm.hpp" #include "micras/proxy/stopwatch.hpp" @@ -14,6 +12,9 @@ namespace micras::proxy { /** * @brief Class for controlling the fan driver. + * + * @note The driver on this board has its two half bridges paralleled and its phase input strapped, + * so the drive is unidirectional by construction and there is no direction pin to control. */ class Fan { public: @@ -21,8 +22,7 @@ class Fan { * @brief Configuration struct for the fan. */ struct Config { - hal::Pwm::Config pwm; - // hal::Gpio::Config direction_gpio; + hal::Pwm::Config pwm; hal::Gpio::Config enable_gpio; float max_acceleration; }; @@ -53,42 +53,42 @@ class Fan { /** * @brief Update the speed of the fan. + * + * @return Current speed percentage of the fan. */ float update(); /** - * @brief Stop the fan. + * @brief Stop the fan and cancel any speed it was ramping towards. */ void stop(); -private: /** - * @brief Enum for rotation direction. + * @brief Check if the driver is reporting a fault. + * + * @note The enable pin is bidirectional: the driver pulls it low to signal an over current or a + * thermal shutdown, which is only observable while the fan is meant to be enabled and the pin + * is configured as an open drain output. + * + * @return True if the driver is pulling the enable pin low against the firmware, false otherwise. */ - enum class RotationDirection : uint8_t { - FORWARD = 0, - BACKWARDS = 1 - }; + bool check_fault() const; /** - * @brief Set the rotation direction of the fan. + * @brief Check if the fan PWM was successfully initialized. * - * @param direction Rotation direction. + * @return True if the initialization was successful, false otherwise. */ - void set_direction(RotationDirection direction); + bool was_initialized() const; +private: /** * @brief PWM object for controlling the fan speed. */ hal::Pwm pwm; /** - * @brief GPIO object for controlling the fan rotation direction. - */ - // hal::Gpio direction_gpio; - - /** - * @brief GPIO handle for the fan enable pin. + * @brief GPIO handle for the fan enable and fault pin. */ hal::Gpio enable_gpio; @@ -107,6 +107,11 @@ class Fan { */ float max_acceleration; + /** + * @brief Whether the firmware is currently asking the driver to be enabled. + */ + bool enabled{}; + /** * @brief Stopwatch for limiting the acceleration of the fan. */ diff --git a/micras_proxy/include/micras/proxy/fmac_filter.hpp b/micras_proxy/include/micras/proxy/fmac_filter.hpp new file mode 100644 index 0000000..257221d --- /dev/null +++ b/micras_proxy/include/micras/proxy/fmac_filter.hpp @@ -0,0 +1,122 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_FMAC_FILTER_HPP +#define MICRAS_PROXY_FMAC_FILTER_HPP + +#include +#include + +#include "micras/core/butterworth_filter.hpp" +#include "micras/hal/fmac.hpp" + +namespace micras::proxy { +/** + * @brief Second order Butterworth low-pass filter running on the filter math accelerator. + * + * @note Interface compatible with core::ButterworthFilter, so the two are interchangeable at the + * point of use. Three properties of the accelerator decide whether that substitution is a good + * idea, and none of them is a property of this class: + * + * - The accelerator works in q1.15, so every coefficient has to fall inside [-1, 1) and be large + * enough not to quantise to zero. A second order Butterworth has a feed forward coefficient of + * roughly (fc / fs)^2 / 4, so at a cutoff far below the sampling rate the coefficients underflow. + * At a 960 Hz sampling rate the useful range starts around a 100 Hz cutoff, well above the 5 Hz + * and 10 Hz cutoffs the sensors of this robot use. was_initialized reports that refusal instead + * of filtering with silently truncated coefficients. + * - Samples and results are q1.15 too, so the input has to be normalised to [-1, 1). That suits a + * ratiometric reading such as an ADC fraction, and not a physical quantity such as a rate in + * radians per second. + * - The accelerator holds one filter configuration at a time. Several instances cannot run + * concurrently without reprogramming the coefficient buffer between samples, which costs far + * more than the handful of multiply accumulates it would save. + */ +class FmacFilter { +public: + /** + * @brief Configuration struct for the filter. + */ + struct Config { + hal::Fmac::Config fmac; + core::ButterworthFilter::Config filter; + }; + + /** + * @brief Construct a new FmacFilter object. + * + * @param config Configuration for the filter. + */ + explicit FmacFilter(const Config& config); + + /** + * @brief Produce a new value from measured data. + * + * @param x0 Last measure, normalised to the range [-1, 1). + * @return Filtered value, or zero if the coefficients could not be represented. + */ + float update(float x0); + + /** + * @brief Get the last filtered value. + * + * @return Last filtered value. + */ + float get_last() const; + + /** + * @brief Check if the coefficients fit the fixed point format and the filter is running. + * + * @return True if the filter is usable, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Number of fractional bits of the q1.15 format of the accelerator. + */ + static constexpr uint8_t fractional_bits{15}; + + /** + * @brief Scale between a float in [-1, 1) and its q1.15 representation. + */ + static constexpr float fixed_point_scale{static_cast(1U << fractional_bits)}; + + /** + * @brief Convert a coefficient to q1.15, reporting whether it survives the conversion. + * + * @param value Coefficient to convert. + * @param result Converted coefficient. + * @return True if the coefficient is inside the representable range and does not quantise to + * zero, false otherwise. + */ + static bool to_fixed_point(float value, int16_t& result); + + /** + * @brief Filter math accelerator running the filter. + */ + hal::Fmac fmac; + + /** + * @brief Coefficients applied to the input samples, most recent first. + */ + std::array feed_forward{}; + + /** + * @brief Coefficients applied to the previous outputs, most recent first. + */ + std::array feedback{}; + + /** + * @brief Last filtered value. + */ + float last{}; + + /** + * @brief Flag to check if the filter is running. + */ + bool initialized{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_FMAC_FILTER_HPP diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 4c6038c..ca96093 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -26,11 +26,11 @@ class Imu { hal::Spi::Config spi; lsm6dsv_data_rate_t gyroscope_data_rate; lsm6dsv_data_rate_t accelerometer_data_rate; - lsm6dsv_sflp_data_rate_t orientation_data_rate; lsm6dsv_gy_full_scale_t gyroscope_scale; lsm6dsv_xl_full_scale_t accelerometer_scale; lsm6dsv_filt_gy_lp1_bandwidth_t gyroscope_filter; lsm6dsv_filt_xl_lp2_bandwidth_t accelerometer_filter; + core::ButterworthFilter::Config calibration_filter; }; /** @@ -151,7 +151,7 @@ class Imu { /** * @brief Gyroscope Butterworth filter for the calibration. */ - core::ButterworthFilter calibration_filter{5.0F}; + core::ButterworthFilter calibration_filter; /** * @brief Flag to check if the IMU was calibrated. diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index 9344c46..5aa831b 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -63,6 +63,13 @@ class Locomotion { */ void stop(); + /** + * @brief Check if both motors were successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + private: /** * @brief Left motor of the robot. diff --git a/micras_proxy/include/micras/proxy/motor.hpp b/micras_proxy/include/micras/proxy/motor.hpp index 8d1b2a7..bcb9768 100644 --- a/micras_proxy/include/micras/proxy/motor.hpp +++ b/micras_proxy/include/micras/proxy/motor.hpp @@ -37,6 +37,13 @@ class Motor { */ void set_command(float command); + /** + * @brief Check if both PWM channels were successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + private: /** * @brief PWM object for controlling the motor in the backwards direction. diff --git a/micras_proxy/include/micras/proxy/rotary_sensor.hpp b/micras_proxy/include/micras/proxy/rotary_sensor.hpp index 464daed..db783a1 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor.hpp @@ -5,7 +5,9 @@ #ifndef MICRAS_PROXY_ROTARY_SENSOR_HPP #define MICRAS_PROXY_ROTARY_SENSOR_HPP +#include #include +#include #include "micras/hal/crc.hpp" #include "micras/hal/encoder.hpp" @@ -14,6 +16,9 @@ namespace micras::proxy { /** * @brief Class for acquiring rotary sensor data. + * + * @note The angle is decoded from the quadrature output of the sensor by a hardware timer, while + * SPI is used only to configure the sensor at construction and to read back that configuration. */ class RotarySensor { public: @@ -21,15 +26,21 @@ class RotarySensor { /** * @brief Rotary sensor configuration struct. + * + * @note There is no resolution field: the pulses per revolution are whatever the ABIRES field + * of the sensor says after configuration, so the scale factor is read back from the hardware + * instead of being asserted here. Two numbers that have to agree are one number too many. */ struct Config { hal::Spi::Config spi; hal::Encoder::Config encoder; hal::Crc::Config crc; - uint32_t resolution; Registers registers; }; + /** + * @brief Frame sent to the sensor to address one of its registers. + */ union CommandFrame { struct __attribute__((__packed__)) Fields { uint8_t crc : 8; @@ -42,6 +53,9 @@ class RotarySensor { uint32_t raw; }; + /** + * @brief Frame carrying a register value in either direction. + */ union DataFrame { struct __attribute__((__packed__)) Fields { uint8_t crc : 8; @@ -69,21 +83,75 @@ class RotarySensor { float get_position() const; /** - * @brief Read a register to the rotary sensor. + * @brief Get the number of counts the quadrature decoder produces per revolution. * - * @param command_frame Command frame to send trough SPI. + * @return Counts per revolution, as read back from the sensor. */ - uint16_t read_register(uint16_t address); + uint32_t get_resolution() const; + + /** + * @brief Check if the sensor was configured and its configuration read back successfully. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + + /** + * @brief Read a register from the rotary sensor. + * + * @param address Address of the register. + * @return The register value, or nothing if a transfer failed or a frame was rejected. + */ + std::optional read_register(uint16_t address); /** * @brief Write a register to the rotary sensor. * - * @param command_frame Command frame to send trough SPI. - * @param data_frame Data frame to send trough SPI. + * @param address Address of the register. + * @param data Value to write. + * @return True if both frames were transferred, false otherwise. */ - void write_register(CommandFrame& command_frame, DataFrame& data_frame); + bool write_register(uint16_t address, uint16_t data); private: + /** + * @brief Number of bytes of a sensor SPI frame. + */ + static constexpr uint8_t frame_size{3}; + + /** + * @brief Number of edges the quadrature decoder counts per pulse of a single channel. + */ + static constexpr uint32_t edges_per_pulse{4}; + + /** + * @brief Pulses per revolution selected by each value of the ABIRES field, zero when reserved. + */ + static constexpr std::array pulses_per_revolution{{1024, 512, 256, 2048, 4096, 0, 0, 0}}; + + /** + * @brief Serialize a frame into the wire order of the sensor, filling in its CRC. + * + * @param raw The 24 bit frame, without a valid CRC. + * @return The frame as three bytes, most significant first, with the CRC in the last one. + */ + std::array serialize(uint32_t raw); + + /** + * @brief Exchange one frame with the sensor. + * + * @param frame The frame to send. + * @return The frame received while sending, or nothing if the transfer failed. + */ + std::optional exchange_frame(const std::array& frame); + + /** + * @brief Read back the ABIRES field and derive the counts per revolution from it. + * + * @return Counts per revolution, or nothing if the read failed or reported a reserved value. + */ + std::optional read_resolution(); + /** * @brief SPI for the rotary sensor configuration. */ @@ -100,9 +168,14 @@ class RotarySensor { hal::Crc crc; /** - * @brief Resolution of the rotary sensor. + * @brief Counts per revolution of the quadrature decoder, read back from the sensor. + */ + uint32_t resolution{}; + + /** + * @brief Flag to check if the sensor was configured successfully. */ - uint32_t resolution; + bool initialized{}; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp index 021eda2..7819dcd 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp @@ -43,7 +43,7 @@ struct Registers { uint8_t Dia2_en : 1; }; - Fields Fields; + Fields fields; uint8_t raw; }; diff --git a/micras_proxy/include/micras/proxy/stopwatch.hpp b/micras_proxy/include/micras/proxy/stopwatch.hpp index 5b01a70..ee6c591 100644 --- a/micras_proxy/include/micras/proxy/stopwatch.hpp +++ b/micras_proxy/include/micras/proxy/stopwatch.hpp @@ -5,33 +5,22 @@ #ifndef MICRAS_PROXY_STOPWATCH_HPP #define MICRAS_PROXY_STOPWATCH_HPP -#include "micras/hal/timer.hpp" +#include namespace micras::proxy { /** * @brief Class to measure the time elapsed between two events. + * + * @note The millisecond and microsecond timebases are independent, so an object can be reset in one + * unit and read in the other without the two interfering. */ class Stopwatch { public: /** - * @brief Stopwatch configuration struct. - */ - struct Config { - hal::Timer::Config timer; - }; - - /** - * @brief Construct a new Stopwatch object. + * @brief Construct a new Stopwatch object, starting both timebases. */ Stopwatch(); - /** - * @brief Construct a new Stopwatch object. - * - * @param config Configuration for the timer. - */ - explicit Stopwatch(const Config& config); - /** * @brief Reset the milliseconds timer counter. */ @@ -68,15 +57,18 @@ class Stopwatch { * * @param time Time to sleep in microseconds. */ - void sleep_us(uint32_t time) const; + static void sleep_us(uint32_t time); private: - hal::Timer timer; + /** + * @brief Value of the millisecond counter at the last reset. + */ + uint32_t counter_ms{}; /** - * @brief Stopwatch counter. + * @brief Value of the cycle counter at the last reset. */ - uint32_t counter{}; + uint32_t counter_cycles{}; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index 05cd064..10d67c5 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -47,8 +47,9 @@ class Storage { */ template void create(const std::string& name, const T& data) { - this->primitives[name].ram_pointer = &data; - this->primitives.at(name).size = sizeof(T); + auto& variable = this->primitives[name]; + variable.ram_pointer = &data; + variable.size = sizeof(T); } /** @@ -68,9 +69,11 @@ class Storage { */ template void sync(const std::string& name, T& data) { - if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr and - this->primitives.at(name).size == sizeof(T)) { - std::memcpy(&data, &this->buffer.at(this->primitives.at(name).buffer_address), sizeof(T)); + const auto variable = this->primitives.find(name); + + if (variable != this->primitives.end() and variable->second.ram_pointer == nullptr and + variable->second.size == sizeof(T)) { + std::memcpy(&data, &this->buffer.at(variable->second.buffer_address), sizeof(T)); } this->create(name, data); diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index a0d230c..600372f 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -22,10 +22,10 @@ class TTorqueSensors { * @brief Configuration struct for torque sensors. */ struct Config { - hal::AdcDma::Config adc; - float shunt_resistor; - float max_torque; - float filter_cutoff; + hal::AdcDma::Config adc; + float shunt_resistor; + float max_torque; + core::ButterworthFilter::Config filter; }; /** @@ -37,6 +37,10 @@ class TTorqueSensors { /** * @brief Calibrate the torque sensors. + * + * @note The amplifiers on this board are bidirectional about half the reference voltage, so the + * uncalibrated reading carries a large offset and this has to be called before any reading is + * meaningful. Calling it again refines the baseline rather than discarding it. */ void calibrate(); @@ -85,6 +89,13 @@ class TTorqueSensors { */ float get_adc_reading(uint8_t sensor_index) const; + /** + * @brief Check if the ADC was successfully initialized. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + private: /** * @brief ADC DMA handle. @@ -115,6 +126,11 @@ class TTorqueSensors { * @brief Butterworth filters for the torque reading. */ std::array filters; + + /** + * @brief Flag to check if the ADC was initialized. + */ + bool initialized{}; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 8941eae..f20500f 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -25,7 +25,7 @@ class TWallSensors { struct Config { hal::AdcDma::Config adc; std::array led_pwms; - float filter_cutoff; + core::ButterworthFilter::Config filter; std::array base_readings; float uncertainty; }; @@ -90,6 +90,13 @@ class TWallSensors { */ void calibrate_sensor(uint8_t sensor_index); + /** + * @brief Check if the ADC was initialized and its scan matches the buffer layout. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + private: /** * @brief ADC DMA handle. @@ -102,7 +109,19 @@ class TWallSensors { std::array led_pwms; /** - * @brief Buffer to store the ADC values. + * @brief Buffer to store the ADC values, holding one emitter on and one emitter off scan. + * + * @details The emitter PWM timer is centre aligned and triggers the ADC on its update event, so + * the conversion sequence runs twice per emitter period: once at the underflow, while the + * emitters are on, and once at the overflow, while they are off. Two consecutive scans + * therefore fill the two halves of this buffer with a matching pair, and the difference between + * the halves is the reflected signal with the ambient light cancelled. Taking the absolute + * value makes the result independent of which half currently holds which phase. + * + * @note This depends on the ADC scanning exactly num_of_sensors channels, on the emitter timer + * being centre aligned with its trigger on the update event, and on a scan fitting inside half + * an emitter period. The first of those is checked by the constructor; the other two live in the + * peripheral configuration. */ std::array buffer; @@ -120,6 +139,11 @@ class TWallSensors { * @brief Ratio of the base reading to still consider as seeing a wall. */ float uncertainty; + + /** + * @brief Flag to check if the ADC was initialized. + */ + bool initialized{}; }; } // namespace micras::proxy diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/src/argb.cpp index 41a8fc1..4be7358 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/src/argb.cpp @@ -71,6 +71,11 @@ void TArgb::encode_color(const Color& color, uint8_t index) { this->buffer.at(i) = ((data >> j) & 1) == 1 ? this->high_bit : this->low_bit; } } + +template +bool TArgb::was_initialized() const { + return this->pwm.was_initialized(); +} } // namespace micras::proxy #endif // MICRAS_PROXY_ARGB_CPP diff --git a/micras_proxy/src/battery.cpp b/micras_proxy/src/battery.cpp index 2b10b2b..57f443a 100644 --- a/micras_proxy/src/battery.cpp +++ b/micras_proxy/src/battery.cpp @@ -7,10 +7,9 @@ namespace micras::proxy { Battery::Battery(const Config& config) : adc{config.adc}, - max_voltage{hal::AdcDma::reference_voltage * config.voltage_divider}, - filter{config.filter_cutoff} { - this->adc.start_dma({&(this->raw_reading), 1}); -} + max_voltage{config.adc.reference_voltage * config.voltage_divider}, + filter{config.filter}, + initialized{this->adc.start_dma({&(this->raw_reading), 1}) and this->adc.was_initialized()} { } void Battery::update() { this->filter.update(this->get_adc_reading()); @@ -27,4 +26,8 @@ float Battery::get_voltage_raw() const { float Battery::get_adc_reading() const { return static_cast(this->raw_reading) / this->adc.get_max_reading(); } + +bool Battery::was_initialized() const { + return this->initialized; +} } // namespace micras::proxy diff --git a/micras_proxy/src/buzzer.cpp b/micras_proxy/src/buzzer.cpp index 700b114..8100e22 100644 --- a/micras_proxy/src/buzzer.cpp +++ b/micras_proxy/src/buzzer.cpp @@ -41,4 +41,8 @@ void Buzzer::stop() { this->is_playing = false; this->pwm.set_duty_cycle(0.0F); } + +bool Buzzer::was_initialized() const { + return this->pwm.was_initialized(); +} } // namespace micras::proxy diff --git a/micras_proxy/src/dip_switch.cpp b/micras_proxy/src/dip_switch.cpp index ea808e2..2a1a203 100644 --- a/micras_proxy/src/dip_switch.cpp +++ b/micras_proxy/src/dip_switch.cpp @@ -13,11 +13,11 @@ namespace micras::proxy { template TDipSwitch::TDipSwitch(const Config& config) : - gpio_array{core::make_array(config.gpio_array)} { } + gpio_array{core::make_array(config.gpio_array)}, active_low{config.active_low} { } template bool TDipSwitch::get_switch_state(uint8_t switch_index) const { - return gpio_array.at(switch_index).read(); + return this->gpio_array.at(switch_index).read() != this->active_low; } template @@ -25,7 +25,7 @@ uint8_t TDipSwitch::get_switches_value() const { uint8_t switches_value = 0; for (uint8_t i = 0; i < num_of_sensors; i++) { - switches_value |= (gpio_array[i].read() << i); + switches_value |= static_cast(this->get_switch_state(i) << i); } return switches_value; diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp index 9cf7067..e9c376d 100644 --- a/micras_proxy/src/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -2,24 +2,25 @@ * @file */ +#include + #include "micras/core/utils.hpp" #include "micras/proxy/fan.hpp" namespace micras::proxy { Fan::Fan(const Config& config) : - pwm{config.pwm}, - // direction_gpio{config.direction_gpio}, - enable_gpio{config.enable_gpio}, - max_acceleration{config.max_acceleration} { + pwm{config.pwm}, enable_gpio{config.enable_gpio}, max_acceleration{config.max_acceleration} { this->stop(); this->enable(); } void Fan::enable() { + this->enabled = true; this->enable_gpio.write(true); } void Fan::disable() { + this->enabled = false; this->enable_gpio.write(false); } @@ -35,24 +36,24 @@ float Fan::update() { this->acceleration_stopwatch.reset_ms(); - if (this->current_speed > 0.0F) { - this->set_direction(RotationDirection::FORWARD); - this->pwm.set_duty_cycle(this->current_speed); - } else if (this->current_speed < 0.0F) { - this->set_direction(RotationDirection::BACKWARDS); - this->pwm.set_duty_cycle(-this->current_speed); - } else { - this->stop(); - } + // The driver has no direction control on this board, so a negative speed can only mean off + this->pwm.set_duty_cycle(std::max(this->current_speed, 0.0F)); return this->current_speed; } void Fan::stop() { + // The target has to go with it, or the next update ramps the fan straight back up + this->target_speed = 0.0F; + this->current_speed = 0.0F; this->pwm.set_duty_cycle(0.0F); } -void Fan::set_direction([[maybe_unused]] RotationDirection direction) { - // this->direction_gpio.write(static_cast(direction)); +bool Fan::check_fault() const { + return this->enabled and not this->enable_gpio.read(); +} + +bool Fan::was_initialized() const { + return this->pwm.was_initialized(); } } // namespace micras::proxy diff --git a/micras_proxy/src/fmac_filter.cpp b/micras_proxy/src/fmac_filter.cpp new file mode 100644 index 0000000..0af7dd4 --- /dev/null +++ b/micras_proxy/src/fmac_filter.cpp @@ -0,0 +1,71 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/core/butterworth_filter.hpp" +#include "micras/proxy/fmac_filter.hpp" + +namespace micras::proxy { +FmacFilter::FmacFilter(const Config& config) : fmac{config.fmac} { + const core::ButterworthFilter::Coefficients coefficients = + core::ButterworthFilter::compute_coefficients(config.filter); + + bool representable = true; + + for (std::size_t i = 0; i < coefficients.feed_forward.size(); i++) { + representable &= to_fixed_point(coefficients.feed_forward.at(i), this->feed_forward.at(i)); + } + + // The accelerator adds the feedback contribution where the filter relation subtracts it + for (std::size_t i = 0; i < coefficients.feedback.size(); i++) { + representable &= to_fixed_point(-coefficients.feedback.at(i), this->feedback.at(i)); + } + + if (not representable) { + return; + } + + this->initialized = this->fmac.configure_iir(this->feed_forward, this->feedback); +} + +float FmacFilter::update(float x0) { + if (not this->initialized) { + return 0.0F; + } + + const float clamped = std::clamp(x0, -1.0F, 1.0F - 1.0F / fixed_point_scale); + const auto sample = static_cast(std::lround(clamped * fixed_point_scale)); + + this->last = static_cast(this->fmac.update(sample)) / fixed_point_scale; + + return this->last; +} + +float FmacFilter::get_last() const { + return this->last; +} + +bool FmacFilter::was_initialized() const { + return this->initialized; +} + +bool FmacFilter::to_fixed_point(float value, int16_t& result) { + const float scaled = value * fixed_point_scale; + + if (std::abs(scaled) >= fixed_point_scale) { + result = 0; + return false; + } + + result = static_cast(std::lround(scaled)); + + // A coefficient that rounds to zero is not a small coefficient, it is a missing one + return result != 0 or value == 0.0F; +} +} // namespace micras::proxy diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index f00a4fc..d00db18 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -17,7 +17,8 @@ Imu::Imu(const Config& config) : mdps_to_radps * 4.375F * (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale))) }, - xl_factor{mg_to_mps2 * (0.061F * (1 << static_cast(config.accelerometer_scale)))} { + xl_factor{mg_to_mps2 * (0.061F * (1 << static_cast(config.accelerometer_scale)))}, + calibration_filter{config.calibration_filter} { this->dev_ctx.read_reg = platform_read; this->dev_ctx.write_reg = platform_write; this->dev_ctx.mdelay = proxy::Stopwatch::sleep_ms; @@ -25,7 +26,7 @@ Imu::Imu(const Config& config) : proxy::Stopwatch::sleep_ms(10); - if (not this->check_whoami()) { + if (not this->spi.was_initialized() or not this->check_whoami()) { return; } @@ -57,17 +58,21 @@ bool Imu::check_whoami() { void Imu::update() { std::array raw_data{}; - lsm6dsv_all_sources_t all_sources; - lsm6dsv_all_sources_get(&dev_ctx, &all_sources); - if (all_sources.drdy_xl) { + // Only the two data ready flags are of interest here, and STATUS_REG carries both in one + // transaction, where lsm6dsv_all_sources_get walks the FIFO, the embedded function bank and the + // sensor hub and writes four registers back on the way + lsm6dsv_data_ready_t data_ready{}; + lsm6dsv_flag_data_ready_get(&this->dev_ctx, &data_ready); + + if (data_ready.drdy_xl) { lsm6dsv_acceleration_raw_get(&dev_ctx, raw_data.data()); std::get<0>(this->linear_acceleration) = std::get<0>(raw_data) * xl_factor; std::get<1>(this->linear_acceleration) = std::get<1>(raw_data) * xl_factor; std::get<2>(this->linear_acceleration) = std::get<2>(raw_data) * xl_factor; } - if (all_sources.drdy_gy) { + if (data_ready.drdy_gy) { lsm6dsv_angular_rate_raw_get(&dev_ctx, raw_data.data()); std::get<0>(this->angular_velocity) = std::get<0>(raw_data) * gy_factor; std::get<1>(this->angular_velocity) = std::get<1>(raw_data) * gy_factor; @@ -114,26 +119,28 @@ float Imu::get_linear_acceleration(Axis axis) const { int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t len) { auto* spi = static_cast(handle); - while (not spi->select_device()) { } + if (not spi->select_device()) { + return -1; + } reg |= 0x80; - spi->transmit({®, 1}); - spi->receive({bufp, len}); + const bool transferred = spi->transmit({®, 1}) and spi->receive({bufp, len}); spi->unselect_device(); - return 0; + return transferred ? 0 : -1; } int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len) { auto* spi = static_cast(handle); - while (not spi->select_device()) { } + if (not spi->select_device()) { + return -1; + } - spi->transmit({®, 1}); - spi->transmit({const_cast(bufp), len}); // NOLINT(cppcoreguidelines-pro-type-const-cast) + const bool transferred = spi->transmit({®, 1}) and spi->transmit({bufp, len}); spi->unselect_device(); - return 0; + return transferred ? 0 : -1; } void Imu::calibrate() { diff --git a/micras_proxy/src/locomotion.cpp b/micras_proxy/src/locomotion.cpp index b828a5a..b855f3c 100644 --- a/micras_proxy/src/locomotion.cpp +++ b/micras_proxy/src/locomotion.cpp @@ -2,6 +2,7 @@ * @file */ +#include #include #include "micras/proxy/locomotion.hpp" @@ -30,14 +31,14 @@ void Locomotion::set_command(float linear, float angular) { float left_command = linear - angular; float right_command = linear + angular; - if (std::abs(left_command) > 100.0F) { - left_command *= 100.0F / std::abs(left_command); - right_command *= 100.0F / std::abs(left_command); - } + // Both wheels are scaled by the same factor, so that saturation preserves the commanded ratio + // and therefore the turn radius + const float peak = std::max(std::abs(left_command), std::abs(right_command)); - if (std::abs(right_command) > 100.0F) { - left_command *= 100.0F / std::abs(right_command); - right_command *= 100.0F / std::abs(right_command); + if (peak > 100.0F) { + const float scale = 100.0F / peak; + left_command *= scale; + right_command *= scale; } this->set_wheel_command(left_command, right_command); @@ -46,4 +47,8 @@ void Locomotion::set_command(float linear, float angular) { void Locomotion::stop() { this->set_wheel_command(0.0F, 0.0F); } + +bool Locomotion::was_initialized() const { + return this->left_motor.was_initialized() and this->right_motor.was_initialized(); +} } // namespace micras::proxy diff --git a/micras_proxy/src/motor.cpp b/micras_proxy/src/motor.cpp index 97b3afb..2bd924f 100644 --- a/micras_proxy/src/motor.cpp +++ b/micras_proxy/src/motor.cpp @@ -34,4 +34,8 @@ void Motor::set_command(float command) { this->backwards_pwm.set_duty_cycle(command); } } + +bool Motor::was_initialized() const { + return this->backwards_pwm.was_initialized() and this->forward_pwm.was_initialized(); +} } // namespace micras::proxy diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index 8eab7f1..5c874b6 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -2,78 +2,140 @@ * @file */ -#include +#include #include #include +#include #include "micras/proxy/rotary_sensor.hpp" namespace micras::proxy { -RotarySensor::RotarySensor(const Config& config) : - spi{config.spi}, encoder{config.encoder}, crc{config.crc}, resolution{config.resolution} { - CommandFrame command_frame{}; - DataFrame data_frame{}; - - command_frame.fields.address = Registers::disable_addr; - data_frame.fields.data = config.registers.disable.raw; - this->write_register(command_frame, data_frame); - - command_frame.fields.address = Registers::zposm_addr; - data_frame.fields.data = config.registers.zposm.raw; - this->write_register(command_frame, data_frame); - - command_frame.fields.address = Registers::zposl_addr; - data_frame.fields.data = config.registers.zposl.raw; - this->write_register(command_frame, data_frame); - - command_frame.fields.address = Registers::settings1_addr; - data_frame.fields.data = config.registers.settings1.raw; - this->write_register(command_frame, data_frame); - - command_frame.fields.address = Registers::settings2_addr; - data_frame.fields.data = config.registers.settings2.raw; - this->write_register(command_frame, data_frame); - - command_frame.fields.address = Registers::settings3_addr; - data_frame.fields.data = config.registers.settings3.raw; - this->write_register(command_frame, data_frame); - - command_frame.fields.address = Registers::ecc_addr; - data_frame.fields.data = config.registers.ecc.raw; - this->write_register(command_frame, data_frame); +RotarySensor::RotarySensor(const Config& config) : spi{config.spi}, encoder{config.encoder}, crc{config.crc} { + if (not this->spi.was_initialized() or not this->encoder.was_initialized()) { + return; + } + + this->write_register(Registers::disable_addr, config.registers.disable.raw); + this->write_register(Registers::zposm_addr, config.registers.zposm.raw); + this->write_register(Registers::zposl_addr, config.registers.zposl.raw); + this->write_register(Registers::settings1_addr, config.registers.settings1.raw); + this->write_register(Registers::settings2_addr, config.registers.settings2.raw); + this->write_register(Registers::settings3_addr, config.registers.settings3.raw); + this->write_register(Registers::ecc_addr, config.registers.ecc.raw); + + const std::optional read_back = this->read_resolution(); + + if (not read_back.has_value()) { + return; + } + + // The scale comes from what the sensor reports, not from what was written to it, so a + // configuration write that silently failed cannot turn into a wrongly scaled odometry + this->resolution = read_back.value(); + this->initialized = true; } float RotarySensor::get_position() const { - return encoder.get_counter() * 2.0F * std::numbers::pi_v / this->resolution; + if (this->resolution == 0) { + return 0.0F; + } + + return static_cast(this->encoder.get_counter()) * 2.0F * std::numbers::pi_v / + static_cast(this->resolution); } -uint16_t RotarySensor::read_register(uint16_t address) { - CommandFrame command_frame = {{.crc = 0, .address = address, .rw = 1, .do_not_care = 0}}; - const DataFrame data_frame{}; +uint32_t RotarySensor::get_resolution() const { + return this->resolution; +} - command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2) ^ 0xFF; +bool RotarySensor::was_initialized() const { + return this->initialized; +} - while (not this->spi.select_device()) { } - this->spi.transmit({std::bit_cast(&command_frame.raw), 3}); - this->spi.unselect_device(); +std::array RotarySensor::serialize(uint32_t raw) { + std::array bytes{ + static_cast(raw >> 16), + static_cast(raw >> 8), + static_cast(raw), + }; - while (not this->spi.select_device()) { } - this->spi.receive({std::bit_cast(&data_frame.raw), 3}); - this->spi.unselect_device(); + // The sensor defines the CRC over the two most significant bytes of the frame, which are the + // first two on the wire and the last two in memory on a little endian core + std::get<2>(bytes) = static_cast(this->crc.calculate({bytes.data(), frame_size - 1}) ^ 0xFF); - return data_frame.fields.data; + return bytes; } -void RotarySensor::write_register(CommandFrame& command_frame, DataFrame& data_frame) { - command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2) ^ 0xFF; - data_frame.fields.crc = this->crc.calculate(&data_frame.raw, 2) ^ 0xFF; +std::optional RotarySensor::exchange_frame(const std::array& frame) { + std::array received{}; - while (not this->spi.select_device()) { } - this->spi.transmit({std::bit_cast(&command_frame.raw), 3}); - this->spi.unselect_device(); + if (not this->spi.select_device()) { + return std::nullopt; + } - while (not this->spi.select_device()) { } - this->spi.transmit({std::bit_cast(&data_frame.raw), 3}); + const bool transferred = this->spi.transmit_receive(frame, received); this->spi.unselect_device(); + + if (not transferred) { + return std::nullopt; + } + + return static_cast(std::get<0>(received)) << 16 | static_cast(std::get<1>(received)) << 8 | + std::get<2>(received); +} + +std::optional RotarySensor::read_register(uint16_t address) { + const CommandFrame command{{.crc = 0, .address = address, .rw = 1, .do_not_care = 0}}; + const std::array command_bytes = this->serialize(command.raw); + + // The sensor answers a command in the frame that follows it, so the command is sent twice: the + // first transfer carries it and the second clocks the answer out while repeating it + if (not this->exchange_frame(command_bytes).has_value()) { + return std::nullopt; + } + + const std::optional answer = this->exchange_frame(command_bytes); + + if (not answer.has_value()) { + return std::nullopt; + } + + DataFrame data{}; + data.raw = answer.value(); + + const std::array expected = this->serialize(answer.value()); + + if (std::get<2>(expected) != data.fields.crc or data.fields.error != 0) { + return std::nullopt; + } + + return static_cast(data.fields.data); +} + +bool RotarySensor::write_register(uint16_t address, uint16_t data) { + const CommandFrame command{{.crc = 0, .address = address, .rw = 0, .do_not_care = 0}}; + const DataFrame value{{.crc = 0, .data = data, .error = 0, .warning = 0}}; + + return this->exchange_frame(this->serialize(command.raw)).has_value() and + this->exchange_frame(this->serialize(value.raw)).has_value(); +} + +std::optional RotarySensor::read_resolution() { + const std::optional settings3 = this->read_register(Registers::settings3_addr); + + if (not settings3.has_value()) { + return std::nullopt; + } + + Registers::Settings3 fields{}; + fields.raw = static_cast(settings3.value()); + + const uint16_t pulses = pulses_per_revolution.at(fields.fields.ABIRES); + + if (pulses == 0) { + return std::nullopt; + } + + return edges_per_pulse * pulses; } } // namespace micras::proxy diff --git a/micras_proxy/src/stopwatch.cpp b/micras_proxy/src/stopwatch.cpp index 52a9b40..b742603 100644 --- a/micras_proxy/src/stopwatch.cpp +++ b/micras_proxy/src/stopwatch.cpp @@ -3,50 +3,44 @@ */ #include -#include +#include "micras/hal/timer.hpp" #include "micras/proxy/stopwatch.hpp" namespace micras::proxy { Stopwatch::Stopwatch() { this->reset_ms(); -} - -Stopwatch::Stopwatch(const Config& config) : timer{config.timer} { this->reset_us(); } void Stopwatch::reset_ms() { - this->counter = hal::Timer::get_counter_ms(); + this->counter_ms = hal::Timer::get_counter_ms(); } void Stopwatch::reset_us() { - this->counter = this->timer.get_counter_us(); + this->counter_cycles = hal::Timer::get_counter(); } uint32_t Stopwatch::elapsed_time_ms() const { - return hal::Timer::get_counter_ms() - this->counter; + return hal::Timer::get_counter_ms() - this->counter_ms; } uint32_t Stopwatch::elapsed_time_us() const { - const uint32_t counter = this->timer.get_counter_us(); - - if (counter < this->counter) { - return std::numeric_limits::max() - this->counter + counter; - } - - return counter - this->counter; + // The subtraction happens in cycles, before the conversion, so that the wrap of the cycle + // counter needs no special case + return hal::Timer::to_microseconds(hal::Timer::get_counter() - this->counter_cycles); } void Stopwatch::sleep_ms(uint32_t time) { - const uint32_t start = HAL_GetTick(); + const uint32_t start = hal::Timer::get_counter_ms(); - while (HAL_GetTick() - start < time) { } + while (hal::Timer::get_counter_ms() - start < time) { } } -void Stopwatch::sleep_us(uint32_t time) const { - const uint32_t start = this->timer.get_counter_us(); +void Stopwatch::sleep_us(uint32_t time) { + const uint32_t start = hal::Timer::get_counter(); + const uint32_t cycles = hal::Timer::to_cycles(time); - while (this->timer.get_counter_us() - start < time) { } + while (hal::Timer::get_counter() - start < cycles) { } } } // namespace micras::proxy diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 538e82f..fdd45a6 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -96,9 +96,10 @@ void Storage::create(const std::string& name, const core::ISerializable& data) { } void Storage::sync(const std::string& name, core::ISerializable& data) { - if (this->serializables.contains(name) and this->serializables.at(name).ram_pointer == nullptr) { - const auto& serializable = this->serializables.at(name); - data.deserialize(&this->buffer.at(serializable.buffer_address), serializable.size); + const auto serializable = this->serializables.find(name); + + if (serializable != this->serializables.end() and serializable->second.ram_pointer == nullptr) { + data.deserialize(&this->buffer.at(serializable->second.buffer_address), serializable->second.size); } this->create(name, data); diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index f87bed1..8931345 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -6,32 +6,33 @@ #define MICRAS_PROXY_TORQUE_SENSORS_CPP #include + #include "micras/core/butterworth_filter.hpp" #include "micras/core/utils.hpp" -#include "micras/hal/adc_dma.hpp" #include "micras/proxy/torque_sensors.hpp" namespace micras::proxy { template TTorqueSensors::TTorqueSensors(const Config& config) : adc{config.adc}, - max_current{hal::AdcDma::reference_voltage / config.shunt_resistor}, + max_current{config.adc.reference_voltage / config.shunt_resistor}, max_torque{config.max_torque}, - filters{core::make_array(config.filter_cutoff)} { - this->adc.start_dma(this->buffer); -} + filters{core::make_array(config.filter)}, + initialized{this->adc.start_dma(this->buffer) and this->adc.was_initialized()} { } template void TTorqueSensors::calibrate() { + // The filter is fed already offset readings, so the new baseline is the accumulated offset plus + // whatever is left, which is what makes a second call a refinement instead of a reset for (uint8_t i = 0; i < num_of_sensors; i++) { - this->base_reading[i] = this->filters[i].get_last(); + this->base_reading.at(i) += this->filters.at(i).get_last(); } } template void TTorqueSensors::update() { for (uint8_t i = 0; i < num_of_sensors; i++) { - this->filters[i].update(this->get_adc_reading(i)); + this->filters.at(i).update(this->get_adc_reading(i)); } } @@ -53,7 +54,6 @@ float TTorqueSensors::get_current(uint8_t sensor_index) const { template float TTorqueSensors::get_current_raw(uint8_t sensor_index) const { return this->get_adc_reading(sensor_index) * this->max_current; - ; } template @@ -61,6 +61,11 @@ float TTorqueSensors::get_adc_reading(uint8_t sensor_index) cons return static_cast(this->buffer.at(sensor_index)) / this->adc.get_max_reading() - this->base_reading.at(sensor_index); } + +template +bool TTorqueSensors::was_initialized() const { + return this->initialized; +} } // namespace micras::proxy #endif // MICRAS_PROXY_TORQUE_SENSORS_CPP diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/src/wall_sensors.cpp index cdf80ff..e2b098e 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/src/wall_sensors.cpp @@ -6,6 +6,8 @@ #define MICRAS_PROXY_WALL_SENSORS_CPP #include +#include + #include "micras/core/butterworth_filter.hpp" #include "micras/core/utils.hpp" #include "micras/hal/pwm.hpp" @@ -16,10 +18,13 @@ template TWallSensors::TWallSensors(const Config& config) : adc{config.adc}, led_pwms{core::make_array(config.led_pwms)}, - filters{core::make_array(config.filter_cutoff)}, + filters{core::make_array(config.filter)}, base_readings{config.base_readings}, - uncertainty{config.uncertainty} { - this->adc.start_dma(this->buffer); + uncertainty{config.uncertainty}, + initialized{ + this->adc.start_dma(this->buffer) and this->adc.was_initialized() and + config.adc.handle->Init.NbrOfConversion == num_of_sensors + } { this->turn_off(); } @@ -40,7 +45,7 @@ void TWallSensors::turn_off() { template void TWallSensors::update() { for (uint8_t i = 0; i < num_of_sensors; i++) { - this->filters[i].update(this->get_adc_reading(i)); + this->filters.at(i).update(this->get_adc_reading(i)); } } @@ -72,6 +77,11 @@ template void TWallSensors::calibrate_sensor(uint8_t sensor_index) { this->base_readings.at(sensor_index) = this->get_reading(sensor_index); } + +template +bool TWallSensors::was_initialized() const { + return this->initialized; +} } // namespace micras::proxy #endif // MICRAS_PROXY_WALL_SENSORS_CPP From 3417ee8440fc9d712e4d8b811eed83ca2765916e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 18 Sep 2026 23:17:07 -0300 Subject: [PATCH 471/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Borrow=20the=20pro?= =?UTF-8?q?xies=20instead=20of=20sharing=20ownership=20of=20them?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Micras held ten shared pointers and Interface held five more copies of them, each with an atomic refcount on a single threaded bare metal target and a separate control block, for objects Micras owns for the entire life of the program. A shared pointer that is never shared, never reseated and never outlives its owner is a cost paid on every access and a question the reader has to answer every time. Every proxy is now a member by value and the objects that use them take references. That also removes the two places where a refcount was used as control flow: Odometry and FollowWall decided whether to update a sensor by asking whether anyone else held it, so an extra copy made anywhere would have changed behaviour, and the branch never fired because Micras always held one. Interface turned out to hold the ARGB and the buzzer without ever touching them, which was invisible while they were shared pointers and obvious once they became references, so it now takes the three proxies it actually uses. The battery and the torque sensors are live: constructed, updated every loop and included in the initialisation check, which the torque sensors never were. Both are wanted by a future nav version, and leaving them unexercised until then means their defects surface on the day somebody switches them on. Also here: the control loop refreshes the watchdog and records its own worst case duration, which is the measurement every performance question needs; the abort path goes through hal::Mcu::emergency_stop rather than driving timer and GPIO registers from the application; and saving the best route widens the watchdog window, since erasing a flash sector stalls the core for seconds. --- micras_nav/include/micras/nav/follow_wall.hpp | 7 ++-- micras_nav/include/micras/nav/odometry.hpp | 21 ++++++----- micras_nav/src/follow_wall.cpp | 35 ++++++++----------- micras_nav/src/odometry.cpp | 26 ++++++-------- 4 files changed, 38 insertions(+), 51 deletions(-) diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index e57545a..8aa6519 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -5,8 +5,6 @@ #ifndef MICRAS_NAV_FOLLOW_WALL #define MICRAS_NAV_FOLLOW_WALL -#include - #include "micras/core/pid_controller.hpp" #include "micras/core/types.hpp" #include "micras/nav/state.hpp" @@ -37,7 +35,7 @@ class FollowWall { * @param wall_sensors The wall sensors of the robot. * @param config The configuration for the FollowWall class. */ - FollowWall(const std::shared_ptr>& wall_sensors, const Config& config); + FollowWall(const proxy::TWallSensors<4>& wall_sensors, const Config& config); /** * @brief Calculate the desired angular speed to follow wall. @@ -97,7 +95,8 @@ class FollowWall { /** * @brief Wall sensors of the robot. */ - std::shared_ptr> wall_sensors; + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const proxy::TWallSensors<4>& wall_sensors; /** * @brief PID controller for the wall following. diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 3ed018b..3160545 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -5,8 +5,6 @@ #ifndef MICRAS_NAV_ODOMETRY_HPP #define MICRAS_NAV_ODOMETRY_HPP -#include - #include "micras/core/butterworth_filter.hpp" #include "micras/nav/state.hpp" #include "micras/proxy/imu.hpp" @@ -22,9 +20,9 @@ class Odometry { * @brief Configuration for the odometry. */ struct Config { - float linear_cutoff_frequency; - float wheel_radius; - Pose initial_pose; + core::ButterworthFilter::Config linear_filter; + float wheel_radius; + Pose initial_pose; }; /** @@ -36,9 +34,8 @@ class Odometry { * @param config Configuration for the odometry. */ Odometry( - const std::shared_ptr& left_rotary_sensor, - const std::shared_ptr& right_rotary_sensor, const std::shared_ptr& imu, - Config config + const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, + const proxy::Imu& imu, Config config ); /** @@ -75,20 +72,22 @@ class Odometry { void set_state(const State& new_state); private: + // NOLINTBEGIN(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot /** * @brief Left rotary sensor. */ - std::shared_ptr left_rotary_sensor; + const proxy::RotarySensor& left_rotary_sensor; /** * @brief Right rotary sensor. */ - std::shared_ptr right_rotary_sensor; + const proxy::RotarySensor& right_rotary_sensor; /** * @brief IMU sensor. */ - std::shared_ptr imu; + const proxy::Imu& imu; + // NOLINTEND(*-avoid-const-or-ref-data-members) /** * @brief Wheel radius. diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index be5b121..8b507ff 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -2,7 +2,6 @@ * @file */ -#include #include "micras/core/types.hpp" #include "micras/core/utils.hpp" #include "micras/nav/follow_wall.hpp" @@ -11,7 +10,7 @@ #include "micras/proxy/wall_sensors.hpp" namespace micras::nav { -FollowWall::FollowWall(const std::shared_ptr>& wall_sensors, const Config& config) : +FollowWall::FollowWall(const proxy::TWallSensors<4>& wall_sensors, const Config& config) : wall_sensors{wall_sensors}, pid{config.pid}, sensor_index{config.wall_sensor_index}, @@ -25,10 +24,6 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { float cell_advance = state.pose.to_cell(this->cell_size).y; const GridPose grid_pose = state.pose.to_grid(this->cell_size); - if (this->wall_sensors.use_count() == 1) { - this->wall_sensors->update(); - } - if (grid_pose == this->last_grid_pose) { if (this->check_posts(state.pose)) { clear_position_error(state, this->post_reference - cell_advance); @@ -49,12 +44,12 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { return 0.0F; } - if (not this->following_left and this->wall_sensors->get_wall(this->sensor_index.left)) { + if (not this->following_left and this->wall_sensors.get_wall(this->sensor_index.left)) { this->following_left = true; this->pid.reset(); } - if (not this->following_right and this->wall_sensors->get_wall(this->sensor_index.right)) { + if (not this->following_right and this->wall_sensors.get_wall(this->sensor_index.right)) { this->following_right = true; this->pid.reset(); } @@ -62,12 +57,12 @@ float FollowWall::compute_angular_correction(float elapsed_time, State& state) { float error{}; if (this->following_left and this->following_right) { - error = this->wall_sensors->get_sensor_error(this->sensor_index.left) - - this->wall_sensors->get_sensor_error(this->sensor_index.right); + error = this->wall_sensors.get_sensor_error(this->sensor_index.left) - + this->wall_sensors.get_sensor_error(this->sensor_index.right); } else if (this->following_left) { - error = 2.0F * this->wall_sensors->get_sensor_error(this->sensor_index.left); + error = 2.0F * this->wall_sensors.get_sensor_error(this->sensor_index.left); } else if (this->following_right) { - error = -2.0F * this->wall_sensors->get_sensor_error(this->sensor_index.right); + error = -2.0F * this->wall_sensors.get_sensor_error(this->sensor_index.right); } else { return 0.0F; } @@ -90,34 +85,34 @@ bool FollowWall::check_posts(const Pose& pose) { bool found_posts = false; if (this->following_left and - -(this->wall_sensors->get_adc_reading(this->sensor_index.left) - this->last_left_reading) / delta_distance >= + -(this->wall_sensors.get_adc_reading(this->sensor_index.left) - this->last_left_reading) / delta_distance >= this->post_threshold) { this->following_left = false; found_posts = true; } if (this->following_right and - -(this->wall_sensors->get_adc_reading(this->sensor_index.right) - this->last_right_reading) / delta_distance >= + -(this->wall_sensors.get_adc_reading(this->sensor_index.right) - this->last_right_reading) / delta_distance >= this->post_threshold) { this->following_right = false; found_posts = true; } - this->last_left_reading = this->wall_sensors->get_adc_reading(this->sensor_index.left); - this->last_right_reading = this->wall_sensors->get_adc_reading(this->sensor_index.right); + this->last_left_reading = this->wall_sensors.get_adc_reading(this->sensor_index.left); + this->last_right_reading = this->wall_sensors.get_adc_reading(this->sensor_index.right); return found_posts; } core::Observation FollowWall::get_observation() const { - const bool front_wall = this->wall_sensors->get_wall(this->sensor_index.left_front) and - this->wall_sensors->get_wall(this->sensor_index.right_front); + const bool front_wall = this->wall_sensors.get_wall(this->sensor_index.left_front) and + this->wall_sensors.get_wall(this->sensor_index.right_front); const bool disturbed = front_wall; return { - .left = this->wall_sensors->get_wall(this->sensor_index.left, disturbed), + .left = this->wall_sensors.get_wall(this->sensor_index.left, disturbed), .front = front_wall, - .right = this->wall_sensors->get_wall(this->sensor_index.right, disturbed), + .right = this->wall_sensors.get_wall(this->sensor_index.right, disturbed), }; } diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index f6e4a46..86ab370 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -3,7 +3,6 @@ */ #include -#include #include "micras/nav/odometry.hpp" #include "micras/proxy/imu.hpp" @@ -11,27 +10,22 @@ namespace micras::nav { Odometry::Odometry( - const std::shared_ptr& left_rotary_sensor, - const std::shared_ptr& right_rotary_sensor, const std::shared_ptr& imu, - Config config + const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, + const proxy::Imu& imu, Config config ) : left_rotary_sensor{left_rotary_sensor}, right_rotary_sensor{right_rotary_sensor}, imu{imu}, wheel_radius{config.wheel_radius}, initial_pose(config.initial_pose), - left_last_position{left_rotary_sensor->get_position()}, - right_last_position{right_rotary_sensor->get_position()}, - linear_filter{config.linear_cutoff_frequency}, + left_last_position{left_rotary_sensor.get_position()}, + right_last_position{right_rotary_sensor.get_position()}, + linear_filter{config.linear_filter}, state{.pose = config.initial_pose, .velocity = {.linear = 0.0F, .angular = 0.0F}} { } void Odometry::update(float elapsed_time) { - if (this->imu.use_count() == 1) { - this->imu->update(); - } - - const float left_position = this->left_rotary_sensor->get_position(); - const float right_position = this->right_rotary_sensor->get_position(); + const float left_position = this->left_rotary_sensor.get_position(); + const float right_position = this->right_rotary_sensor.get_position(); const float left_distance = this->wheel_radius * (left_position - this->left_last_position); const float right_distance = this->wheel_radius * (right_position - this->right_last_position); @@ -42,7 +36,7 @@ void Odometry::update(float elapsed_time) { const float linear_distance = (left_distance + right_distance) / 2; this->state.velocity.linear = this->linear_filter.update(linear_distance / elapsed_time); - this->state.velocity.angular = this->imu->get_angular_velocity(proxy::Imu::Axis::Z); + this->state.velocity.angular = this->imu.get_angular_velocity(proxy::Imu::Axis::Z); const float angular_distance = this->state.velocity.angular * elapsed_time; @@ -57,8 +51,8 @@ void Odometry::update(float elapsed_time) { } void Odometry::reset() { - this->left_last_position = this->left_rotary_sensor->get_position(); - this->right_last_position = this->right_rotary_sensor->get_position(); + this->left_last_position = this->left_rotary_sensor.get_position(); + this->right_last_position = this->right_rotary_sensor.get_position(); this->state = {.pose = this->initial_pose, .velocity = {.linear = 0.0F, .angular = 0.0F}}; } From cf2f09611269d2fbd16c9fbe148093c3e5dcc6cd Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 19 Sep 2026 00:31:18 -0300 Subject: [PATCH 472/561] =?UTF-8?q?=F0=9F=93=9D=20Move=20the=20explanation?= =?UTF-8?q?s=20out=20of=20the=20function=20bodies?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The review pass left roughly forty comment blocks inside function bodies across micras_core, micras_hal, micras_proxy and src, explaining why each line that had been wrong is now right. That is not how this project documents itself: the standard is Doxygen on the declaration and nothing in the body, and before this branch the whole of micras_*/src carried one explanatory comment, one note in maze.cpp and one label in storage.cpp. Restating what a line does next to the line is how a reviewer talks, not how the codebase talks. Five of them said something a caller cannot derive from the code, so they moved to the declaration instead of being deleted. Fmac::configure_iir now says that the feedback coefficients carry the sign the accelerator expects, since it adds their contribution where the filter relation subtracts it. RotarySensor says that the sensor defines its CRC over the two most significant bytes of the frame, which are the first two on the wire and the last two in memory, and that it answers a command in the frame that follows it, which is why the command goes out twice. TFsm::update says that it aborts on a state id with no state behind it. Locomotion::set_command says that saturation scales both wheels, so the commanded ratio and with it the turn radius survive it. Two more were already covered by the declaration and are simply gone: Fan::stop already documented that it cancels the speed it was ramping towards, and TorqueSensors::calibrate already documented that a second call refines the baseline rather than discarding it. The Doxygen blocks on file local constants stay. flash.cpp, mcu.cpp and storage.cpp already documented theirs that way, so that part is the standard rather than a departure from it. --- micras_core/include/micras/core/fsm.hpp | 4 ++++ micras_core/src/butterworth_filter.cpp | 5 ----- micras_core/src/fsm.cpp | 3 --- micras_hal/include/micras/hal/fmac.hpp | 4 +++- micras_hal/stm32/src/crc.cpp | 2 -- micras_hal/stm32/src/fmac.cpp | 2 -- micras_hal/stm32/src/mcu.cpp | 2 -- micras_hal/stm32/src/pwm.cpp | 5 ----- micras_hal/stm32/src/pwm_dma.cpp | 1 - micras_hal/stm32/src/spi.cpp | 2 -- micras_hal/stm32/src/timer.cpp | 1 - micras_proxy/include/micras/proxy/locomotion.hpp | 3 +++ micras_proxy/include/micras/proxy/rotary_sensor.hpp | 7 +++++++ micras_proxy/src/fan.cpp | 2 -- micras_proxy/src/fmac_filter.cpp | 2 -- micras_proxy/src/imu.cpp | 3 --- micras_proxy/src/locomotion.cpp | 2 -- micras_proxy/src/rotary_sensor.cpp | 6 ------ micras_proxy/src/stopwatch.cpp | 2 -- micras_proxy/src/torque_sensors.cpp | 2 -- 20 files changed, 17 insertions(+), 43 deletions(-) diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 301c4df..66d8989 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -92,6 +92,10 @@ class TFsm { /** * @brief Run the FSM current state to compute the next state. + * + * @note Aborts when the current state id has no state behind it, which can only be a + * programming error, and running the abort handler beats dispatching through a null pointer + * with the motors turning. */ void update(); diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index ced9614..f6fe9e2 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -11,11 +11,6 @@ namespace micras::core { ButterworthFilter::Coefficients ButterworthFilter::compute_coefficients(const Config& config) { - // The bilinear transform maps the analog prototype 1 / (s^2 + sqrt(2) s + 1) onto the unit - // circle by substituting s = warp (1 - z^-1) / (1 + z^-1), with warp chosen so the cutoff lands - // exactly where it was asked for, which is cot(pi * cutoff / sampling). The cotangent is the - // prewarping, and leaving it out is what would turn the cutoff into an angular frequency and - // put the real one a factor of 2 pi too low. const float warp = 1.0F / std::tan(std::numbers::pi_v * config.cutoff_frequency / config.sampling_frequency); const float warp_2 = warp * warp; diff --git a/micras_core/src/fsm.cpp b/micras_core/src/fsm.cpp index 84ca9f5..18638c0 100644 --- a/micras_core/src/fsm.cpp +++ b/micras_core/src/fsm.cpp @@ -24,9 +24,6 @@ void TFsm::add_state(std::unique_ptr state) { template void TFsm::update() { if (this->current_state_id >= num_of_states or this->states.at(this->current_state_id) == nullptr) { - // An id with no state behind it can only be a programming error. Aborting runs the - // emergency stop handler, which is a far better outcome than dispatching through a null - // pointer with the motors running. std::abort(); } diff --git a/micras_hal/include/micras/hal/fmac.hpp b/micras_hal/include/micras/hal/fmac.hpp index d25e525..fb60d74 100644 --- a/micras_hal/include/micras/hal/fmac.hpp +++ b/micras_hal/include/micras/hal/fmac.hpp @@ -39,7 +39,9 @@ class Fmac { * @brief Configure the accelerator as an infinite impulse response filter in direct form 1. * * @param feed_forward Coefficients applied to the input samples, most recent first, in q1.15. - * @param feedback Coefficients applied to the previous outputs, most recent first, in q1.15. + * @param feedback Coefficients applied to the previous outputs, most recent first, in q1.15, + * with the sign the accelerator expects, since it adds their contribution where the filter + * relation subtracts it. * @return True if the filter was configured and started, false otherwise. */ bool configure_iir(std::span feed_forward, std::span feedback); diff --git a/micras_hal/stm32/src/crc.cpp b/micras_hal/stm32/src/crc.cpp index 149320c..669402f 100644 --- a/micras_hal/stm32/src/crc.cpp +++ b/micras_hal/stm32/src/crc.cpp @@ -12,8 +12,6 @@ namespace micras::hal { Crc::Crc(const Config& config) : handle{config.handle} { } uint32_t Crc::calculate(std::span data) { - // With CRC_INPUTDATA_FORMAT_BYTES the HAL reads the buffer byte by byte, so the word pointer in - // its signature describes the declaration rather than the access return HAL_CRC_Calculate(this->handle, std::bit_cast(data.data()), data.size()); } } // namespace micras::hal diff --git a/micras_hal/stm32/src/fmac.cpp b/micras_hal/stm32/src/fmac.cpp index 84888a5..edee9bd 100644 --- a/micras_hal/stm32/src/fmac.cpp +++ b/micras_hal/stm32/src/fmac.cpp @@ -38,8 +38,6 @@ bool Fmac::configure_iir(std::span feed_forward, std::span(feed_forward.size()), diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp index 172c382..34694ad 100644 --- a/micras_hal/stm32/src/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -99,8 +99,6 @@ void Mcu::emergency_stop(std::span pwm_outputs, std::spanhandle->State == HAL_TIM_STATE_RESET) { config.init_function(); } @@ -42,9 +40,6 @@ Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_cha } void Pwm::set_duty_cycle(float duty_cycle) { - // Adding a half and truncating is exactly round half up for a duty cycle that is never - // negative, and it avoids the libm call std::lround would make in a path that runs five times - // per control loop const float scaled = duty_cycle * static_cast(__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) * 0.01F; // NOLINTNEXTLINE(bugprone-incorrect-roundings) diff --git a/micras_hal/stm32/src/pwm_dma.cpp b/micras_hal/stm32/src/pwm_dma.cpp index 09840fb..2441df6 100644 --- a/micras_hal/stm32/src/pwm_dma.cpp +++ b/micras_hal/stm32/src/pwm_dma.cpp @@ -34,7 +34,6 @@ void PwmDma::stop_dma() { } uint32_t PwmDma::get_compare(float duty_cycle) const { - // Same expression as Pwm::set_duty_cycle, so that the two helpers cannot disagree const float scaled = duty_cycle * static_cast(__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) * 0.01F; // NOLINTNEXTLINE(bugprone-incorrect-roundings) diff --git a/micras_hal/stm32/src/spi.cpp b/micras_hal/stm32/src/spi.cpp index 4756c8a..a862910 100644 --- a/micras_hal/stm32/src/spi.cpp +++ b/micras_hal/stm32/src/spi.cpp @@ -15,8 +15,6 @@ Spi::Spi(const Config& config) : timeout{config.timeout}, clock_polarity{config.clock_polarity}, clock_phase{config.clock_phase} { - // Deassert before the bus exists, so that a device is never left selected by the reset state - // of its chip select pin this->unselect_device(); if (this->handle->State == HAL_SPI_STATE_RESET) { diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index 1500223..cbda211 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -17,7 +17,6 @@ uint32_t Timer::cycles_per_microsecond{1}; void Timer::init() { CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; - // Some Cortex-M7 implementations gate the debug registers behind a software lock if ((DWT->LSR & ITM_LSR_Present_Msk) != 0 and (DWT->LSR & ITM_LSR_Access_Msk) != 0) { DWT->LAR = software_lock_key; } diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index 5aa831b..a36cfea 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -53,6 +53,9 @@ class Locomotion { /** * @brief Set the linear and angular commands of the robot. * + * @note Both wheels are scaled by the same factor when a command saturates, so the commanded + * ratio, and with it the turn radius, survives the saturation. + * * @param linear Linear command of the robot. * @param angular Angular command of the robot. */ diff --git a/micras_proxy/include/micras/proxy/rotary_sensor.hpp b/micras_proxy/include/micras/proxy/rotary_sensor.hpp index db783a1..d68c9f9 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor.hpp @@ -99,6 +99,10 @@ class RotarySensor { /** * @brief Read a register from the rotary sensor. * + * @note The sensor answers a command in the frame that follows it, so the command is sent + * twice: the first transfer carries it and the second clocks the answer out while repeating + * it. + * * @param address Address of the register. * @return The register value, or nothing if a transfer failed or a frame was rejected. */ @@ -132,6 +136,9 @@ class RotarySensor { /** * @brief Serialize a frame into the wire order of the sensor, filling in its CRC. * + * @note The sensor defines the CRC over the two most significant bytes of the frame, which + * are the first two on the wire and the last two in memory on a little endian core. + * * @param raw The 24 bit frame, without a valid CRC. * @return The frame as three bytes, most significant first, with the CRC in the last one. */ diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp index e9c376d..a1d7728 100644 --- a/micras_proxy/src/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -36,14 +36,12 @@ float Fan::update() { this->acceleration_stopwatch.reset_ms(); - // The driver has no direction control on this board, so a negative speed can only mean off this->pwm.set_duty_cycle(std::max(this->current_speed, 0.0F)); return this->current_speed; } void Fan::stop() { - // The target has to go with it, or the next update ramps the fan straight back up this->target_speed = 0.0F; this->current_speed = 0.0F; this->pwm.set_duty_cycle(0.0F); diff --git a/micras_proxy/src/fmac_filter.cpp b/micras_proxy/src/fmac_filter.cpp index 0af7dd4..1c92719 100644 --- a/micras_proxy/src/fmac_filter.cpp +++ b/micras_proxy/src/fmac_filter.cpp @@ -22,7 +22,6 @@ FmacFilter::FmacFilter(const Config& config) : fmac{config.fmac} { representable &= to_fixed_point(coefficients.feed_forward.at(i), this->feed_forward.at(i)); } - // The accelerator adds the feedback contribution where the filter relation subtracts it for (std::size_t i = 0; i < coefficients.feedback.size(); i++) { representable &= to_fixed_point(-coefficients.feedback.at(i), this->feedback.at(i)); } @@ -65,7 +64,6 @@ bool FmacFilter::to_fixed_point(float value, int16_t& result) { result = static_cast(std::lround(scaled)); - // A coefficient that rounds to zero is not a small coefficient, it is a missing one return result != 0 or value == 0.0F; } } // namespace micras::proxy diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index d00db18..e70e109 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -59,9 +59,6 @@ bool Imu::check_whoami() { void Imu::update() { std::array raw_data{}; - // Only the two data ready flags are of interest here, and STATUS_REG carries both in one - // transaction, where lsm6dsv_all_sources_get walks the FIFO, the embedded function bank and the - // sensor hub and writes four registers back on the way lsm6dsv_data_ready_t data_ready{}; lsm6dsv_flag_data_ready_get(&this->dev_ctx, &data_ready); diff --git a/micras_proxy/src/locomotion.cpp b/micras_proxy/src/locomotion.cpp index b855f3c..3e175af 100644 --- a/micras_proxy/src/locomotion.cpp +++ b/micras_proxy/src/locomotion.cpp @@ -31,8 +31,6 @@ void Locomotion::set_command(float linear, float angular) { float left_command = linear - angular; float right_command = linear + angular; - // Both wheels are scaled by the same factor, so that saturation preserves the commanded ratio - // and therefore the turn radius const float peak = std::max(std::abs(left_command), std::abs(right_command)); if (peak > 100.0F) { diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index 5c874b6..c8b277e 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -29,8 +29,6 @@ RotarySensor::RotarySensor(const Config& config) : spi{config.spi}, encoder{conf return; } - // The scale comes from what the sensor reports, not from what was written to it, so a - // configuration write that silently failed cannot turn into a wrongly scaled odometry this->resolution = read_back.value(); this->initialized = true; } @@ -59,8 +57,6 @@ std::array RotarySensor::serialize(uint32_t r static_cast(raw), }; - // The sensor defines the CRC over the two most significant bytes of the frame, which are the - // first two on the wire and the last two in memory on a little endian core std::get<2>(bytes) = static_cast(this->crc.calculate({bytes.data(), frame_size - 1}) ^ 0xFF); return bytes; @@ -88,8 +84,6 @@ std::optional RotarySensor::read_register(uint16_t address) { const CommandFrame command{{.crc = 0, .address = address, .rw = 1, .do_not_care = 0}}; const std::array command_bytes = this->serialize(command.raw); - // The sensor answers a command in the frame that follows it, so the command is sent twice: the - // first transfer carries it and the second clocks the answer out while repeating it if (not this->exchange_frame(command_bytes).has_value()) { return std::nullopt; } diff --git a/micras_proxy/src/stopwatch.cpp b/micras_proxy/src/stopwatch.cpp index b742603..88c52a0 100644 --- a/micras_proxy/src/stopwatch.cpp +++ b/micras_proxy/src/stopwatch.cpp @@ -26,8 +26,6 @@ uint32_t Stopwatch::elapsed_time_ms() const { } uint32_t Stopwatch::elapsed_time_us() const { - // The subtraction happens in cycles, before the conversion, so that the wrap of the cycle - // counter needs no special case return hal::Timer::to_microseconds(hal::Timer::get_counter() - this->counter_cycles); } diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/src/torque_sensors.cpp index 8931345..51edfaa 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/src/torque_sensors.cpp @@ -22,8 +22,6 @@ TTorqueSensors::TTorqueSensors(const Config& config) : template void TTorqueSensors::calibrate() { - // The filter is fed already offset readings, so the new baseline is the accumulated offset plus - // whatever is left, which is what makes a second call a refinement instead of a reset for (uint8_t i = 0; i < num_of_sensors; i++) { this->base_reading.at(i) += this->filters.at(i).get_last(); } From bff88e97225530c1a0301bcb5a5bef550fd50a00 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 19 Sep 2026 02:08:14 -0300 Subject: [PATCH 473/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Take=20the=20gener?= =?UTF-8?q?ated=20initialization=20functions=20instead=20of=20naming=20the?= =?UTF-8?q?m?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit hal::Mcu::init called five symbols that STM32CubeMX generates for one particular project: SystemClock_Config, PeriphCommonClock_Config, MX_GPIO_Init, MX_DMA_Init and MX_CRC_Init. Every other wrapper in the package already takes its initialization function through its Config struct, and Mcu was the one that reached out and named them, which is what made libmicras_hal.a linkable into this application and no other. It now takes an hal::Mcu::Config carrying the two clock functions and a span of the peripheral initializations, and the board layer supplies them, which is where MX_TIM8_Init and &hspi3 were already written down. The two clock functions are separate fields because the order is load bearing: the clock tree has to be running before the timebase is resolved from the core clock frequency and before any peripheral is initialized. peripheral_clock_init may be null on a part where everything runs from a bus clock. With that gone, the public headers no longer need a header per peripheral. They named seven generated ones between them, adc.h, crc.h, tim.h, spi.h, gpio.h, fmac.h and main.h, only ever for the handle typedefs and the HAL macros, and main.h alone carries those. So each header includes main.h and nothing else, and timer.hpp includes neither, since it only ever needed uint32_t. The vendor module headers cannot be named individually in their place: each reaches stm32xx_hal_def.h, which reaches back through the configuration header to the module headers, so whichever is included first sees the others before HAL_StatusTypeDef exists. main.h is also the portable choice, since STM32CubeMX writes it for every family and it includes the stm32xx_hal.h of the part it was generated for. main.h joins the include-cleaner ignore list for the same reason the vendor headers are already on it: every symbol behind it resolves to a header already listed, so nothing is credited to main.h and it is reported as unused. That also removes the suppression flash.cpp carried for the same diagnostic. --- .clang-tidy | 10 +++++-- micras_hal/include/micras/hal/adc_dma.hpp | 2 +- micras_hal/include/micras/hal/crc.hpp | 2 +- micras_hal/include/micras/hal/encoder.hpp | 2 +- micras_hal/include/micras/hal/flash.hpp | 3 +- micras_hal/include/micras/hal/fmac.hpp | 3 +- micras_hal/include/micras/hal/gpio.hpp | 3 +- micras_hal/include/micras/hal/mcu.hpp | 30 +++++++++++++++++-- micras_hal/include/micras/hal/pwm.hpp | 3 +- micras_hal/include/micras/hal/pwm_dma.hpp | 3 +- micras_hal/include/micras/hal/spi.hpp | 2 +- micras_hal/include/micras/hal/timer.hpp | 1 - micras_hal/stm32/src/flash.cpp | 1 - micras_hal/stm32/src/mcu.cpp | 35 +++++++---------------- micras_hal/stm32/src/timer.cpp | 1 + 15 files changed, 60 insertions(+), 41 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 86b8790..8714925 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -61,9 +61,13 @@ ImplementationFileExtensions: HeaderFilterRegex: '(include|config)/.*\.hpp$' FormatStyle: file CheckOptions: - # The CubeMX HAL module headers are not standalone; they require stm32h7xx_hal.h to have - # pulled in stm32h7xx_hal_def.h first, so include-cleaner must not suggest or remove them - misc-include-cleaner.IgnoreHeaders: 'stm32.*;core_cm.*;cmsis.*;system_stm32.*;lsm6dsv.*' + # The vendor HAL module headers are not standalone; they require stm32xx_hal.h to have + # pulled in stm32xx_hal_def.h first, so include-cleaner must not suggest or remove them. + # This is why micras_hal includes the generated main.h, which reaches the right one for the + # family it was generated for, instead of naming a module header of its own. main.h is on the + # list for the same reason: every symbol behind it resolves to a header already listed here, so + # nothing is credited to main.h and it would otherwise be reported as unused. + misc-include-cleaner.IgnoreHeaders: 'stm32.*;core_cm.*;cmsis.*;system_stm32.*;lsm6dsv.*;main\.h' readability-identifier-length.MinimumVariableNameLength: 2 readability-identifier-length.IgnoredVariableNames: ^[nxyz]$ readability-identifier-length.MinimumParameterNameLength: 2 diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index e838609..f908384 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -5,8 +5,8 @@ #ifndef MICRAS_HAL_ADC_DMA_HPP #define MICRAS_HAL_ADC_DMA_HPP -#include #include +#include #include namespace micras::hal { diff --git a/micras_hal/include/micras/hal/crc.hpp b/micras_hal/include/micras/hal/crc.hpp index 649568d..c7e2dcb 100644 --- a/micras_hal/include/micras/hal/crc.hpp +++ b/micras_hal/include/micras/hal/crc.hpp @@ -5,8 +5,8 @@ #ifndef MICRAS_HAL_CRC_HPP #define MICRAS_HAL_CRC_HPP -#include #include +#include #include namespace micras::hal { diff --git a/micras_hal/include/micras/hal/encoder.hpp b/micras_hal/include/micras/hal/encoder.hpp index 43705b4..892e096 100644 --- a/micras_hal/include/micras/hal/encoder.hpp +++ b/micras_hal/include/micras/hal/encoder.hpp @@ -5,7 +5,7 @@ #ifndef MICRAS_HAL_ENCODER_HPP #define MICRAS_HAL_ENCODER_HPP -#include +#include namespace micras::hal { /** diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp index 2158ff0..701bb60 100644 --- a/micras_hal/include/micras/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -7,9 +7,10 @@ #include #include -#include #include +#include + namespace micras::hal { /** * @brief Class to handle the smallest unit of data that can be programmed into the flash memory. diff --git a/micras_hal/include/micras/hal/fmac.hpp b/micras_hal/include/micras/hal/fmac.hpp index fb60d74..d8d6687 100644 --- a/micras_hal/include/micras/hal/fmac.hpp +++ b/micras_hal/include/micras/hal/fmac.hpp @@ -7,9 +7,10 @@ #include #include -#include #include +#include + namespace micras::hal { /** * @brief Class to handle the filter math accelerator on STM32 microcontrollers. diff --git a/micras_hal/include/micras/hal/gpio.hpp b/micras_hal/include/micras/hal/gpio.hpp index 97d2d63..6ce5ba0 100644 --- a/micras_hal/include/micras/hal/gpio.hpp +++ b/micras_hal/include/micras/hal/gpio.hpp @@ -6,7 +6,8 @@ #define MICRAS_HAL_GPIO_HPP #include -#include + +#include namespace micras::hal { /** diff --git a/micras_hal/include/micras/hal/mcu.hpp b/micras_hal/include/micras/hal/mcu.hpp index 17c49d4..9d0e341 100644 --- a/micras_hal/include/micras/hal/mcu.hpp +++ b/micras_hal/include/micras/hal/mcu.hpp @@ -17,15 +17,41 @@ namespace micras::hal { */ class Mcu { public: + /** + * @brief Signature of the initialization function of a peripheral. + */ + using InitFunction = void (*)(); + + /** + * @brief Configuration struct for the microcontroller. + * + * @note The initialization functions are taken rather than named, so that this package refers + * to no symbol the application generates. The two clock functions are separate fields because + * the order matters: the clock tree has to be running before the timebase is resolved from the + * core clock frequency, and before any peripheral is initialized. + */ + struct Config { + InitFunction clock_init; + InitFunction peripheral_clock_init; + std::span peripheral_inits; + }; + /** * @brief Deleted constructor for static class. */ Mcu() = delete; /** - * @brief Initialize MCU and some peripherals. + * @brief Initialize the microcontroller, the timebase and the peripherals that no wrapper owns. + * + * @note Every peripheral held by a wrapper is initialized by that wrapper instead, through the + * initialization function of its own configuration, so peripheral_inits carries only what is + * left over: the pin configuration, the DMA controllers and anything else without an owner. + * + * @param config Initialization functions of the board, which may leave peripheral_clock_init + * null on a part where every peripheral runs from a bus clock. */ - static void init(); + static void init(const Config& config); /** * @brief Silence every actuator as directly as possible. diff --git a/micras_hal/include/micras/hal/pwm.hpp b/micras_hal/include/micras/hal/pwm.hpp index dc60e8b..a513a70 100644 --- a/micras_hal/include/micras/hal/pwm.hpp +++ b/micras_hal/include/micras/hal/pwm.hpp @@ -6,7 +6,8 @@ #define MICRAS_HAL_PWM_HPP #include -#include + +#include namespace micras::hal { /** diff --git a/micras_hal/include/micras/hal/pwm_dma.hpp b/micras_hal/include/micras/hal/pwm_dma.hpp index 3b35902..5890fef 100644 --- a/micras_hal/include/micras/hal/pwm_dma.hpp +++ b/micras_hal/include/micras/hal/pwm_dma.hpp @@ -7,7 +7,8 @@ #include #include -#include + +#include namespace micras::hal { /** diff --git a/micras_hal/include/micras/hal/spi.hpp b/micras_hal/include/micras/hal/spi.hpp index 15d2967..4dfbf15 100644 --- a/micras_hal/include/micras/hal/spi.hpp +++ b/micras_hal/include/micras/hal/spi.hpp @@ -7,8 +7,8 @@ #include #include -#include +#include #include "micras/hal/gpio.hpp" namespace micras::hal { diff --git a/micras_hal/include/micras/hal/timer.hpp b/micras_hal/include/micras/hal/timer.hpp index 434ec5a..7c119e1 100644 --- a/micras_hal/include/micras/hal/timer.hpp +++ b/micras_hal/include/micras/hal/timer.hpp @@ -6,7 +6,6 @@ #define MICRAS_HAL_TIMER_HPP #include -#include namespace micras::hal { /** diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/src/flash.cpp index ed12d41..c5924f7 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/src/flash.cpp @@ -7,7 +7,6 @@ #include #include #include -#include // NOLINT(misc-include-cleaner) #include "micras/hal/flash.hpp" diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp index 34694ad..381445c 100644 --- a/micras_hal/stm32/src/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -3,33 +3,15 @@ */ #include -#include #include -#include -#include #include +#include #include "micras/hal/gpio.hpp" #include "micras/hal/mcu.hpp" #include "micras/hal/pwm.hpp" #include "micras/hal/timer.hpp" -extern "C" { -/** - * @brief Initialize System Clock. - * - * @note Defined by cube. - */ -void SystemClock_Config(); - -/** - * @brief Initialize the kernel clocks of the peripherals that do not run from an APB clock. - * - * @note Defined by cube. - */ -void PeriphCommonClock_Config(); -} - namespace micras::hal { /** * @brief Independent watchdog instance, spelled IWDG1 on the parts that have more than one. @@ -73,19 +55,22 @@ static constexpr uint32_t watchdog_max_reload{0xFFF}; */ static constexpr uint32_t watchdog_timeout_us{1000}; -void Mcu::init() { +void Mcu::init(const Config& config) { SCB_EnableICache(); HAL_Init(); - SystemClock_Config(); - PeriphCommonClock_Config(); + config.clock_init(); + + if (config.peripheral_clock_init != nullptr) { + config.peripheral_clock_init(); + } Timer::init(); - MX_GPIO_Init(); - MX_DMA_Init(); - MX_CRC_Init(); + for (const InitFunction init_function : config.peripheral_inits) { + init_function(); + } } void Mcu::emergency_stop(std::span pwm_outputs, std::span enable_gpios) { diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index cbda211..7d9bde4 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -4,6 +4,7 @@ #include +#include #include "micras/hal/timer.hpp" namespace micras::hal { From 5d4295769aea8d3d7a2cfc8c02409cfb47af8082 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 19 Sep 2026 02:10:09 -0300 Subject: [PATCH 474/561] =?UTF-8?q?=F0=9F=8F=97=EF=B8=8F=20Make=20the=20pa?= =?UTF-8?q?ckages=20depend=20downwards=20only?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Every package named something the layer above it owned. micras_hal linked micras::cube and ${MICRAS_CUBE_OBJECT_LIBRARIES}, both defined in the root CMakeLists, and all four linked micras::warnings, so none of them could be copied into another project and built. The one name micras_hal uses now is stm32cubemx, the interface target the STM32CubeMX generator writes for every project and every family, carrying the CMSIS and vendor HAL include directories and the device macros. Linking it publicly is what puts those headers on everything that includes these ones, so a consuming project adds the four directories, links the layer it wants, and writes nothing else. The generated objects move to the executables. No package refers to a symbol in them any more: the board layer passes the initialization functions in and the vector table belongs to the application. Routing them through micras_hal is what made its archive need --whole-archive to keep the interrupt handlers, which is now gone, and what broke link time optimisation, since CMake adds an object library's files only to the targets that link it directly. Warning flags are the policy of whoever is building rather than a property of a package, so micras::warnings becomes a list and a function that applies it. A private link dependency of a static library still appears in its interface, under LINK_ONLY, so the target could not have stayed without every consumer having to provide one of ours. The C++ standard goes the other way: the sources require C++23, so each package declares it with target_compile_features and propagates it, instead of relying on the root setting CMAKE_CXX_STANDARD. The root CMakeLists had grown to 238 lines, two fifths of them comments. The toolchain configuration and everything about the generated tree move to cmake/toolchain.cmake and cmake/cube.cmake, the test executable loop becomes generate_test_targets next to generate_test_all_target, and what is left reads as project configuration, four includes, four packages, the executable and the tests, in 117 lines. The comments that stayed record a trap or a decision; the ones that narrated what the next line does are gone. The minimum CMake version moves to 3.25 for the SYSTEM target property, which marks the generated headers as system so the project's warning flags do not apply to them. An imported target would get that for free, a normal interface library does not. --- cmake/micras_warnings.cmake | 16 ++++++++++------ micras_core/CMakeLists.txt | 8 ++++---- micras_hal/CMakeLists.txt | 23 +++++++++-------------- micras_nav/CMakeLists.txt | 6 ++---- micras_proxy/CMakeLists.txt | 6 ++---- 5 files changed, 27 insertions(+), 32 deletions(-) diff --git a/cmake/micras_warnings.cmake b/cmake/micras_warnings.cmake index 5f5a70d..d9551b0 100644 --- a/cmake/micras_warnings.cmake +++ b/cmake/micras_warnings.cmake @@ -4,16 +4,11 @@ # These flags deliberately live here and not in cube/cmake/gcc-arm-none-eabi.cmake, # because that file is regenerated by STM32CubeMX and any edit to it is lost. -# The target is linked privately into the micras_* libraries and the executables, so that -# the Cube sources are never compiled with it. option(MICRAS_WERROR "Treat compiler warnings as errors" OFF) -add_library(micras_warnings INTERFACE) -add_library(micras::warnings ALIAS micras_warnings) - # The -Wduplicated-* and -Wuseless-cast flags are GCC only -target_compile_options(micras_warnings INTERFACE +set(MICRAS_WARNING_OPTIONS -Wall -Wextra -Wpedantic @@ -39,3 +34,12 @@ target_compile_options(micras_warnings INTERFACE $<$:-Woverloaded-virtual> $<$:-Wuseless-cast> ) + +# Applied per target rather than through CMAKE_CXX_FLAGS, so that the generated sources are never +# compiled with them, and as options rather than through a target, so that no package ends up +# carrying a build policy of ours in its interface +function(micras_apply_warnings) + foreach(TARGET_NAME ${ARGN}) + target_compile_options(${TARGET_NAME} PRIVATE ${MICRAS_WARNING_OPTIONS}) + endforeach() +endfunction() diff --git a/micras_core/CMakeLists.txt b/micras_core/CMakeLists.txt index dbf768e..afaf191 100644 --- a/micras_core/CMakeLists.txt +++ b/micras_core/CMakeLists.txt @@ -8,10 +8,10 @@ add_library(${PROJECT_NAME} STATIC add_library(micras::core ALIAS ${PROJECT_NAME}) +# C++23 is a requirement of the sources, not a preference of whoever is building, so it is declared +# here and propagated +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_23) + target_include_directories(${PROJECT_NAME} PUBLIC include ) - -target_link_libraries(${PROJECT_NAME} PRIVATE - micras::warnings -) diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt index 3c0e599..76dc8c7 100644 --- a/micras_hal/CMakeLists.txt +++ b/micras_hal/CMakeLists.txt @@ -6,24 +6,19 @@ add_library(${PROJECT_NAME} STATIC ${PROJECT_SOURCES} ) +add_library(micras::hal ALIAS ${PROJECT_NAME}) + +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_23) + target_include_directories(${PROJECT_NAME} PUBLIC include ) -# The object libraries have to be linked directly, since CMake only adds an object library's -# files to the targets that name it, never through an interface target -target_link_libraries(${PROJECT_NAME} PRIVATE - ${MICRAS_CUBE_OBJECT_LIBRARIES} - micras::warnings -) - +# stm32cubemx is the interface target STM32CubeMX generates, carrying the CMSIS and vendor HAL +# include directories and the device macros. It is the only name this package uses from outside +# itself, the generator writes it for every project and every family, and linking it publicly is +# what puts those headers on everything that includes these ones. target_link_libraries(${PROJECT_NAME} PUBLIC micras::core - micras::cube -) - -add_library(micras_hal_whole_archive INTERFACE) -add_library(micras::hal ALIAS micras_hal_whole_archive) -target_link_libraries(micras_hal_whole_archive INTERFACE - "-Wl,--whole-archive" ${PROJECT_NAME} "-Wl,--no-whole-archive" + stm32cubemx ) diff --git a/micras_nav/CMakeLists.txt b/micras_nav/CMakeLists.txt index 00e1670..d981cc5 100644 --- a/micras_nav/CMakeLists.txt +++ b/micras_nav/CMakeLists.txt @@ -8,6 +8,8 @@ add_library(${PROJECT_NAME} STATIC add_library(micras::nav ALIAS ${PROJECT_NAME}) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_23) + target_include_directories(${PROJECT_NAME} PUBLIC include ) @@ -15,7 +17,3 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC micras::proxy ) - -target_link_libraries(${PROJECT_NAME} PRIVATE - micras::warnings -) diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt index 2f3d1ce..504bb31 100644 --- a/micras_proxy/CMakeLists.txt +++ b/micras_proxy/CMakeLists.txt @@ -26,6 +26,8 @@ add_library(${PROJECT_NAME} STATIC add_library(micras::proxy ALIAS ${PROJECT_NAME}) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_23) + target_include_directories(${PROJECT_NAME} PUBLIC include ${lsm6dsv-pid_SOURCE_DIR} @@ -34,7 +36,3 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC micras::hal ) - -target_link_libraries(${PROJECT_NAME} PRIVATE - micras::warnings -) From 174f3f9ed385c45738c0bb7aff1fba814296d5a6 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 19 Sep 2026 02:39:56 -0300 Subject: [PATCH 475/561] =?UTF-8?q?=F0=9F=93=9D=20Justify=20the=20comments?= =?UTF-8?q?=20instead=20of=20narrating=20them?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Several comments explained how the project got somewhere rather than why the line beneath them is there: the generated objects said what routing them through micras_hal used to break, the link time optimization setting described the failure it was introduced for, and was_initialized documented the bug it would have caught. A comment earns its place by justifying the code it sits on, so each now says that and stops. The v0 record listed what reviving it would take as a diff against the current API, which reads as history; it now states what the file needs. Micras and Interface no longer argue against the ownership model they do not use, and the rotary sensor drops an aphorism about two numbers that have to agree. Spelling moves to American English throughout, matching the identifiers the project already uses. --- cmake/micras_warnings.cmake | 5 ++--- micras_core/CMakeLists.txt | 3 +-- micras_hal/CMakeLists.txt | 4 +--- micras_hal/include/micras/hal/adc_dma.hpp | 4 ++-- micras_proxy/include/micras/proxy/fmac_filter.hpp | 8 ++++---- micras_proxy/include/micras/proxy/rotary_sensor.hpp | 2 +- micras_proxy/include/micras/proxy/wall_sensors.hpp | 6 +++--- 7 files changed, 14 insertions(+), 18 deletions(-) diff --git a/cmake/micras_warnings.cmake b/cmake/micras_warnings.cmake index d9551b0..fef905f 100644 --- a/cmake/micras_warnings.cmake +++ b/cmake/micras_warnings.cmake @@ -35,9 +35,8 @@ set(MICRAS_WARNING_OPTIONS $<$:-Wuseless-cast> ) -# Applied per target rather than through CMAKE_CXX_FLAGS, so that the generated sources are never -# compiled with them, and as options rather than through a target, so that no package ends up -# carrying a build policy of ours in its interface +# Applied per target, so that the generated sources are never compiled with them, and as options +# rather than through a target, which a static library would carry in its interface function(micras_apply_warnings) foreach(TARGET_NAME ${ARGN}) target_compile_options(${TARGET_NAME} PRIVATE ${MICRAS_WARNING_OPTIONS}) diff --git a/micras_core/CMakeLists.txt b/micras_core/CMakeLists.txt index afaf191..1b10633 100644 --- a/micras_core/CMakeLists.txt +++ b/micras_core/CMakeLists.txt @@ -8,8 +8,7 @@ add_library(${PROJECT_NAME} STATIC add_library(micras::core ALIAS ${PROJECT_NAME}) -# C++23 is a requirement of the sources, not a preference of whoever is building, so it is declared -# here and propagated +# A requirement of the sources rather than a preference of whoever is building, so it propagates target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_23) target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt index 76dc8c7..f66c13b 100644 --- a/micras_hal/CMakeLists.txt +++ b/micras_hal/CMakeLists.txt @@ -15,9 +15,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC ) # stm32cubemx is the interface target STM32CubeMX generates, carrying the CMSIS and vendor HAL -# include directories and the device macros. It is the only name this package uses from outside -# itself, the generator writes it for every project and every family, and linking it publicly is -# what puts those headers on everything that includes these ones. +# include directories and the device macros. The public headers name them, so it propagates. target_link_libraries(${PROJECT_NAME} PUBLIC micras::core stm32cubemx diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index f908384..2aa5430 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -73,8 +73,8 @@ class AdcDma { /** * @brief Check if the ADC was calibrated and started successfully. * - * @note This is the flag that would have made a missing kernel clock visible instead of turning - * every analogue reading into a silent zero. + * @note A failed calibration or start leaves the DMA buffer at zero, which reads exactly like + * a real measurement, so this is the only way to tell one from the other. * * @return True if the initialization was successful, false otherwise. */ diff --git a/micras_proxy/include/micras/proxy/fmac_filter.hpp b/micras_proxy/include/micras/proxy/fmac_filter.hpp index 257221d..7421293 100644 --- a/micras_proxy/include/micras/proxy/fmac_filter.hpp +++ b/micras_proxy/include/micras/proxy/fmac_filter.hpp @@ -20,12 +20,12 @@ namespace micras::proxy { * idea, and none of them is a property of this class: * * - The accelerator works in q1.15, so every coefficient has to fall inside [-1, 1) and be large - * enough not to quantise to zero. A second order Butterworth has a feed forward coefficient of + * enough not to quantize to zero. A second order Butterworth has a feed forward coefficient of * roughly (fc / fs)^2 / 4, so at a cutoff far below the sampling rate the coefficients underflow. * At a 960 Hz sampling rate the useful range starts around a 100 Hz cutoff, well above the 5 Hz * and 10 Hz cutoffs the sensors of this robot use. was_initialized reports that refusal instead * of filtering with silently truncated coefficients. - * - Samples and results are q1.15 too, so the input has to be normalised to [-1, 1). That suits a + * - Samples and results are q1.15 too, so the input has to be normalized to [-1, 1). That suits a * ratiometric reading such as an ADC fraction, and not a physical quantity such as a rate in * radians per second. * - The accelerator holds one filter configuration at a time. Several instances cannot run @@ -52,7 +52,7 @@ class FmacFilter { /** * @brief Produce a new value from measured data. * - * @param x0 Last measure, normalised to the range [-1, 1). + * @param x0 Last measure, normalized to the range [-1, 1). * @return Filtered value, or zero if the coefficients could not be represented. */ float update(float x0); @@ -87,7 +87,7 @@ class FmacFilter { * * @param value Coefficient to convert. * @param result Converted coefficient. - * @return True if the coefficient is inside the representable range and does not quantise to + * @return True if the coefficient is inside the representable range and does not quantize to * zero, false otherwise. */ static bool to_fixed_point(float value, int16_t& result); diff --git a/micras_proxy/include/micras/proxy/rotary_sensor.hpp b/micras_proxy/include/micras/proxy/rotary_sensor.hpp index d68c9f9..add63ab 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor.hpp @@ -29,7 +29,7 @@ class RotarySensor { * * @note There is no resolution field: the pulses per revolution are whatever the ABIRES field * of the sensor says after configuration, so the scale factor is read back from the hardware - * instead of being asserted here. Two numbers that have to agree are one number too many. + * instead of being asserted here. */ struct Config { hal::Spi::Config spi; diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index f20500f..1a58dd3 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -111,15 +111,15 @@ class TWallSensors { /** * @brief Buffer to store the ADC values, holding one emitter on and one emitter off scan. * - * @details The emitter PWM timer is centre aligned and triggers the ADC on its update event, so + * @details The emitter PWM timer is center aligned and triggers the ADC on its update event, so * the conversion sequence runs twice per emitter period: once at the underflow, while the * emitters are on, and once at the overflow, while they are off. Two consecutive scans * therefore fill the two halves of this buffer with a matching pair, and the difference between - * the halves is the reflected signal with the ambient light cancelled. Taking the absolute + * the halves is the reflected signal with the ambient light canceled. Taking the absolute * value makes the result independent of which half currently holds which phase. * * @note This depends on the ADC scanning exactly num_of_sensors channels, on the emitter timer - * being centre aligned with its trigger on the update event, and on a scan fitting inside half + * being center aligned with its trigger on the update event, and on a scan fitting inside half * an emitter period. The first of those is checked by the constructor; the other two live in the * peripheral configuration. */ From 459f4bbec34c8f96a77b82838e01ea7f893e961f Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 21 Sep 2026 21:10:01 -0300 Subject: [PATCH 476/561] =?UTF-8?q?=E2=9C=A8=20Describe=20the=20variables?= =?UTF-8?q?=20of=20the=20robot=20in=20one=20place?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A variable pool holds a name, a type, an address and a set of access flags per variable, and nothing else: no packets, no flash layout, no schedule. Whatever wants to persist, stream or capture those variables walks the pool and decides for itself what the bytes are for. The descriptors are a flat array of plain structs, so sampling a variable is a memcpy out of it rather than a virtual call through a heap allocation, and the names are views over string literals rather than strings that are copied at registration and returned by value on the transmit path. The identifiers are registration order, so adding one variable shifts every later one. The schema hash over every name, type and flag is what lets a reader notice that, instead of quietly plotting the wrong signal. The framing primitives come with it: COBS leaves a zero byte impossible inside an encoded frame, so a reader resynchronises by scanning to the next one, and a CRC-16 catches what a sum does not, which is a dropped run of bytes joining two frames into one of plausible shape. --- .../include/micras/core/byte_stream.hpp | 64 ++++ micras_core/include/micras/core/cobs.hpp | 52 +++ micras_core/include/micras/core/crc.hpp | 38 ++ .../micras/core/impl/variable_pool.tpp | 60 +++ .../include/micras/core/variable_pool.hpp | 359 ++++++++++++++++++ micras_core/src/cobs.cpp | 80 ++++ micras_core/src/crc.cpp | 41 ++ micras_core/src/variable_pool.cpp | 134 +++++++ 8 files changed, 828 insertions(+) create mode 100644 micras_core/include/micras/core/byte_stream.hpp create mode 100644 micras_core/include/micras/core/cobs.hpp create mode 100644 micras_core/include/micras/core/crc.hpp create mode 100644 micras_core/include/micras/core/impl/variable_pool.tpp create mode 100644 micras_core/include/micras/core/variable_pool.hpp create mode 100644 micras_core/src/cobs.cpp create mode 100644 micras_core/src/crc.cpp create mode 100644 micras_core/src/variable_pool.cpp diff --git a/micras_core/include/micras/core/byte_stream.hpp b/micras_core/include/micras/core/byte_stream.hpp new file mode 100644 index 0000000..bbab2e5 --- /dev/null +++ b/micras_core/include/micras/core/byte_stream.hpp @@ -0,0 +1,64 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_BYTE_STREAM_HPP +#define MICRAS_CORE_BYTE_STREAM_HPP + +#include +#include +#include + +namespace micras::core { +/** + * @brief Interface for a bidirectional stream of bytes. + * + * @note The session layer only ever sees this, so the same protocol runs over the radio, over a + * socket in the simulators and over a pipe in a host test. An interface is used instead of a + * callable because it allocates nothing and costs one virtual call per batch rather than per byte. + */ +class IByteStream { +public: + /** + * @brief Virtual destructor for the IByteStream class. + */ + virtual ~IByteStream() = default; + + /** + * @brief Take whatever has already arrived. + * + * @param into Buffer to read into. + * @return Number of bytes read, which is zero when nothing has arrived. + */ + virtual std::size_t read(std::span into) = 0; + + /** + * @brief Queue data to be sent. + * + * @param from Data to send. + * @return Number of bytes accepted, which is either all of them or none. + */ + virtual std::size_t write(std::span from) = 0; + + /** + * @brief Get how much can be written before the stream refuses data. + * + * @return Number of bytes that can be written right now. + */ + virtual std::size_t writable() const = 0; + +protected: + /** + * @brief Special member functions declared as default. + */ + ///@{ + IByteStream() = default; + IByteStream(const IByteStream&) = default; + IByteStream(IByteStream&&) = default; + IByteStream& operator=(const IByteStream&) = default; + IByteStream& operator=(IByteStream&&) = default; + ///@} +}; +} // namespace micras::core + +#endif // MICRAS_CORE_BYTE_STREAM_HPP diff --git a/micras_core/include/micras/core/cobs.hpp b/micras_core/include/micras/core/cobs.hpp new file mode 100644 index 0000000..f5ea32b --- /dev/null +++ b/micras_core/include/micras/core/cobs.hpp @@ -0,0 +1,52 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_COBS_HPP +#define MICRAS_CORE_COBS_HPP + +#include +#include +#include + +namespace micras::core { +/** + * @brief Byte reserved to delimit frames, which encoding removes from the data. + */ +constexpr uint8_t cobs_delimiter{0x00}; + +/** + * @brief Compute the largest size the encoding of a buffer can have. + * + * @param size Number of bytes of the data. + * @return Number of bytes the encoded data can take at most. + */ +constexpr std::size_t cobs_encoded_size(std::size_t size) { + return size + size / 254 + 1; +} + +/** + * @brief Encode a buffer so that it contains no delimiter byte. + * + * @note Consistent Overhead Byte Stuffing costs one byte every 254, against up to one byte per byte + * for escaping, and leaves the delimiter impossible to produce by construction. Resynchronizing + * after the module silently drops a run of bytes is then just a matter of scanning to the next + * delimiter. + * + * @param from Data to encode. + * @param into Buffer to encode into, which should hold cobs_encoded_size bytes. + * @return Number of bytes written, or zero if the buffer is too small. + */ +std::size_t cobs_encode(std::span from, std::span into); + +/** + * @brief Decode a frame that was encoded by cobs_encode. + * + * @param from Encoded frame, without any delimiter. + * @param into Buffer to decode into. + * @return Number of bytes written, or zero if the frame is malformed or the buffer is too small. + */ +std::size_t cobs_decode(std::span from, std::span into); +} // namespace micras::core + +#endif // MICRAS_CORE_COBS_HPP diff --git a/micras_core/include/micras/core/crc.hpp b/micras_core/include/micras/core/crc.hpp new file mode 100644 index 0000000..c2dbcfb --- /dev/null +++ b/micras_core/include/micras/core/crc.hpp @@ -0,0 +1,38 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_CRC_HPP +#define MICRAS_CORE_CRC_HPP + +#include +#include + +namespace micras::core { +/** + * @brief Compute the CRC-16/CCITT-FALSE of a buffer. + * + * @note Implemented in software, over a nibble table, instead of through the CRC peripheral. The + * peripheral is configured for the polynomial of the rotary sensors and is stateful, so sharing it + * with the 8 kHz sensor path would couple two unrelated things to save a few cycles on a path that + * runs a hundred times a second. + * + * @param data Buffer to compute the CRC of. + * @param seed Initial value of the register, so that a CRC can be computed in several calls. + * @return CRC of the buffer. + */ +uint16_t crc16(std::span data, uint16_t seed = 0xFFFF); + +/** + * @brief Compute the CRC-32 of a buffer. + * + * @note Computed bit by bit, with no lookup table, because the only caller runs once at startup. + * + * @param data Buffer to compute the CRC of. + * @param seed Initial value of the register, so that a CRC can be computed in several calls. + * @return CRC of the buffer. + */ +uint32_t crc32(std::span data, uint32_t seed = 0xFFFFFFFF); +} // namespace micras::core + +#endif // MICRAS_CORE_CRC_HPP diff --git a/micras_core/include/micras/core/impl/variable_pool.tpp b/micras_core/include/micras/core/impl/variable_pool.tpp new file mode 100644 index 0000000..83817f6 --- /dev/null +++ b/micras_core/include/micras/core/impl/variable_pool.tpp @@ -0,0 +1,60 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_VARIABLE_POOL_TPP +#define MICRAS_CORE_VARIABLE_POOL_TPP + +#include +#include +#include + +namespace micras::core { +template +VariableId VariablePool::add(std::string_view prefix, std::string_view name, T& value, Access access) { + Variable* variable = this->next(); + + if (variable == nullptr) { + return invalid_id; + } + + *variable = { + .prefix = prefix, + .name = name, + .address = static_cast(&value), + .size = sizeof(T), + .type = TypeCodeOf>::value, + .access = access, + }; + + return this->count++; +} + +template +VariableId VariablePool::add(std::string_view prefix, std::string_view name, const T& value, Access access) { + access.write = false; + access.idle = false; + + Variable* variable = this->next(); + + if (variable == nullptr) { + return invalid_id; + } + + // The pool holds one mutable address for every kind of variable, so that no consumer has to + // branch on constness. Casting it away is safe because this overload has just cleared the only + // flag through which anything can write. + *variable = { + .prefix = prefix, + .name = name, + .address = const_cast(&value), // NOLINT(cppcoreguidelines-pro-type-const-cast) + .size = sizeof(T), + .type = TypeCodeOf>::value, + .access = access, + }; + + return this->count++; +} +} // namespace micras::core + +#endif // MICRAS_CORE_VARIABLE_POOL_TPP diff --git a/micras_core/include/micras/core/variable_pool.hpp b/micras_core/include/micras/core/variable_pool.hpp new file mode 100644 index 0000000..8d629b2 --- /dev/null +++ b/micras_core/include/micras/core/variable_pool.hpp @@ -0,0 +1,359 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_VARIABLE_POOL_HPP +#define MICRAS_CORE_VARIABLE_POOL_HPP + +#include +#include +#include +#include +#include +#include + +#include "micras/core/serializable.hpp" + +namespace micras::core { +/** + * @brief Type of a registered variable, as it is written to the flash and to the wire. + */ +enum class TypeCode : uint8_t { + BOOL = 0, + U8 = 1, + I8 = 2, + U16 = 3, + I16 = 4, + U32 = 5, + I32 = 6, + U64 = 7, + I64 = 8, + F32 = 9, + F64 = 10, + BLOB = 11 +}; + +/** + * @brief Map from a C++ type to the type code that describes it. + * + * @note Left undefined for every type without a specialization, so that registering an unsupported + * type fails to compile instead of being written out as raw bytes nothing can read back. + * + * @tparam T Type to map. + */ +template +struct TypeCodeOf { }; + +/** + * @brief Specializations of the type code map. + */ +///@{ +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::BOOL}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::U8}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::I8}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::U16}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::I16}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::U32}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::I32}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::U64}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::I64}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::F32}; +}; + +template <> +struct TypeCodeOf { + static constexpr TypeCode value{TypeCode::F64}; +}; + +template +requires std::is_enum_v +struct TypeCodeOf : TypeCodeOf> { }; + +///@} + +/** + * @brief Types that can be registered by value, as opposed to through ISerializable. + * + * @tparam T Type to check. + */ +template +concept Registrable = std::is_trivially_copyable_v and requires { TypeCodeOf>::value; }; + +/** + * @brief What each consumer of the pool is allowed to do with a variable. + */ +struct Access { + /** + * @brief Whether the variable can be part of a stream or of a trace group. + */ + bool stream : 1 {}; + + /** + * @brief Whether the variable can be written from outside the robot. + */ + bool write : 1 {}; + + /** + * @brief Whether writes are restricted to a stopped robot, ignored when write is not set. + */ + bool idle : 1 {}; + + /** + * @brief Whether the variable is part of the flash image. + */ + bool persist : 1 {}; +}; + +/** + * @brief A variable registered in the pool. + * + * @note The name is split in two views so that an owner can be given a prefix at registration + * without concatenating anything at run time. Both are expected to be string literals, and must + * outlive the pool. + */ +struct Variable { + std::string_view prefix; + std::string_view name; + + /** + * @brief Where the value lives, or the ISerializable that owns it when the type is BLOB. + */ + void* address{}; + + /** + * @brief Number of bytes of the value, zero for a BLOB, whose size is only known once it is + * serialized. + */ + uint16_t size{}; + + TypeCode type{}; + Access access{}; +}; + +/** + * @brief Index of a variable in the pool, and the identifier used on the wire. + */ +using VariableId = uint16_t; + +/** + * @brief Reflection over the variables the rest of the firmware is willing to expose. + * + * @note This class knows nothing about flash, packets or schedules. It is a flat array of plain + * descriptors, and every consumer walks it and decides for itself what the bytes are for. + * + * @note The pool does not own the storage of the values, only their addresses, so a registered + * object must outlive the pool. + */ +class VariablePool { +public: + /** + * @brief Result of an attempt to write a variable from outside the robot. + */ + enum class WriteStatus : uint8_t { + OK = 0, + NO_SUCH_ID = 1, + READ_ONLY = 2, + NEEDS_IDLE = 3, + WRONG_SIZE = 4 + }; + + /** + * @brief Identifier returned when a variable could not be registered. + */ + static constexpr VariableId invalid_id{0xFFFF}; + + /** + * @brief Construct a new VariablePool object over storage owned by the caller. + * + * @param storage Array the descriptors are written into, which bounds the number of variables. + */ + explicit VariablePool(std::span storage); + + /** + * @brief Register a writable variable. + * + * @tparam T Type of the variable. + * @param prefix Prefix of the name, usually identifying the owner. + * @param name Name of the variable inside its owner. + * @param value Reference to the variable, which must outlive the pool. + * @param access What the consumers of the pool may do with the variable. + * @return Identifier of the variable, or invalid_id if the pool is full. + */ + template + VariableId add(std::string_view prefix, std::string_view name, T& value, Access access); + + /** + * @brief Register a read only variable. + * + * @note The write flag is cleared whatever the caller asked for, so that constness is enough to + * state the intent at the registration site. + * + * @tparam T Type of the variable. + * @param prefix Prefix of the name, usually identifying the owner. + * @param name Name of the variable inside its owner. + * @param value Reference to the variable, which must outlive the pool. + * @param access What the consumers of the pool may do with the variable. + * @return Identifier of the variable, or invalid_id if the pool is full. + */ + template + VariableId add(std::string_view prefix, std::string_view name, const T& value, Access access); + + /** + * @brief Register an object with its own encoding. + * + * @note Serializing costs a heap allocation, so a blob is meant for composite state that is + * saved and loaded, never for something sampled at loop rate. + * + * @param prefix Prefix of the name, usually identifying the owner. + * @param name Name of the variable inside its owner. + * @param object Reference to the object, which must outlive the pool. + * @param access What the consumers of the pool may do with the variable. + * @return Identifier of the variable, or invalid_id if the pool is full. + */ + VariableId add(std::string_view prefix, std::string_view name, ISerializable& object, Access access); + + /** + * @brief Find a variable by its full name. + * + * @param full_name Prefix and name of the variable, concatenated. + * @return Identifier of the variable, or no value if it is not registered. + */ + std::optional find(std::string_view full_name) const; + + /** + * @brief Get a registered variable. + * + * @param id Identifier of the variable, which must be valid. + * @return The descriptor of the variable. + */ + const Variable& at(VariableId id) const; + + /** + * @brief Get every registered variable, in registration order. + * + * @return View over the descriptors. + */ + std::span all() const; + + /** + * @brief Copy the current value of a variable. + * + * @note Only defined for variables that are not blobs, which are read through their own + * interface instead. + * + * @param id Identifier of the variable, which must be valid. + * @param into Buffer to copy into, which must hold at least the size of the variable. + * @return Number of bytes copied, zero if the variable is a blob or the buffer is too small. + */ + uint16_t read(VariableId id, std::span into) const; + + /** + * @brief Write a variable from outside the robot. + * + * @param id Identifier of the variable. + * @param bytes New value of the variable. + * @param robot_is_idle Whether the robot is currently stopped. + * @return Whether the write was performed, and why not otherwise. + */ + WriteStatus write(VariableId id, std::span bytes, bool robot_is_idle); + + /** + * @brief Compute a hash over the layout of the pool. + * + * @note Identifiers are registration order, so adding one variable shifts every later one. An + * application holding a cached schema would then plot the wrong signal with nothing to warn it. + * Comparing this hash against the one the schema was fetched with is that warning. + * + * @return Hash over the name, type and access flags of every variable, in order. + */ + uint32_t schema_hash() const; + +private: + /** + * @brief Reserve the next descriptor. + * + * @return Pointer to the descriptor, or nullptr if the pool is full. + */ + Variable* next(); + + /** + * @brief Descriptors of the registered variables, owned by the caller. + */ + std::span storage; + + /** + * @brief Number of variables registered so far. + */ + VariableId count{}; +}; + +namespace detail { +/** + * @brief Storage for a variable pool, held in a base class so that it is already constructed when + * the pool base is given a view over it. + * + * @tparam N Maximum number of variables. + */ +template +struct VariableStorage { + std::array variables{}; +}; +} // namespace detail + +/** + * @brief A variable pool owning storage for a fixed number of variables. + * + * @tparam N Maximum number of variables. + */ +template +// NOLINTNEXTLINE(misc-multiple-inheritance) the first base only exists to hold the storage +class TVariablePool : private detail::VariableStorage, public VariablePool { +public: + /** + * @brief Construct a new TVariablePool object. + */ + TVariablePool() : VariablePool{this->variables} { } +}; +} // namespace micras::core + +#include "micras/core/impl/variable_pool.tpp" // IWYU pragma: export + +#endif // MICRAS_CORE_VARIABLE_POOL_HPP diff --git a/micras_core/src/cobs.cpp b/micras_core/src/cobs.cpp new file mode 100644 index 0000000..30d2af3 --- /dev/null +++ b/micras_core/src/cobs.cpp @@ -0,0 +1,80 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/core/cobs.hpp" + +namespace micras::core { +// Both functions index buffers the caller sized, and every access is bounded by a check just +// above it, which is the whole content of the algorithm. +// NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) +std::size_t cobs_encode(std::span from, std::span into) { + if (into.size() < cobs_encoded_size(from.size())) { + return 0; + } + + std::size_t code_index = 0; + std::size_t write_index = 1; + uint8_t code = 1; + + for (const uint8_t byte : from) { + if (byte != cobs_delimiter) { + into[write_index++] = byte; + + if (++code != 0xFF) { + continue; + } + } + + into[code_index] = code; + code_index = write_index++; + code = 1; + } + + if (not from.empty() and from.back() != cobs_delimiter and code == 1) { + return write_index - 1; + } + + into[code_index] = code; + return write_index; +} + +std::size_t cobs_decode(std::span from, std::span into) { + std::size_t read_index = 0; + std::size_t write_index = 0; + + while (read_index < from.size()) { + const uint8_t code = from[read_index]; + + if (code == cobs_delimiter or read_index + code > from.size()) { + return 0; + } + + read_index++; + + for (uint8_t i = 1; i < code; i++) { + if (write_index >= into.size()) { + return 0; + } + + into[write_index++] = from[read_index++]; + } + + if (code != 0xFF and read_index < from.size()) { + if (write_index >= into.size()) { + return 0; + } + + into[write_index++] = cobs_delimiter; + } + } + + return write_index; +} + +// NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) +} // namespace micras::core diff --git a/micras_core/src/crc.cpp b/micras_core/src/crc.cpp new file mode 100644 index 0000000..f8301ec --- /dev/null +++ b/micras_core/src/crc.cpp @@ -0,0 +1,41 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/core/crc.hpp" + +namespace micras::core { +static constexpr std::array crc16_table{ + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, + 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, +}; + +uint16_t crc16(std::span data, uint16_t seed) { + uint16_t crc = seed; + + for (const uint8_t byte : data) { + crc = static_cast(crc << 4) ^ crc16_table.at((crc >> 12) ^ (byte >> 4)); + crc = static_cast(crc << 4) ^ crc16_table.at((crc >> 12) ^ (byte & 0x0F)); + } + + return crc; +} + +uint32_t crc32(std::span data, uint32_t seed) { + uint32_t crc = seed; + + for (const uint8_t byte : data) { + crc ^= static_cast(byte) << 24; + + for (uint8_t bit = 0; bit < 8; bit++) { + crc = (crc & 0x80000000) != 0 ? (crc << 1) ^ 0x04C11DB7 : crc << 1; + } + } + + return crc; +} +} // namespace micras::core diff --git a/micras_core/src/variable_pool.cpp b/micras_core/src/variable_pool.cpp new file mode 100644 index 0000000..e77a43b --- /dev/null +++ b/micras_core/src/variable_pool.cpp @@ -0,0 +1,134 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/core/crc.hpp" +#include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::core { +static std::span as_bytes(std::string_view text) { + return {std::bit_cast(text.data()), text.size()}; +} + +VariablePool::VariablePool(std::span storage) : storage{storage} { } + +Variable* VariablePool::next() { + if (this->count >= this->storage.size()) { + return nullptr; + } + + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + return &this->storage[this->count]; +} + +VariableId VariablePool::add(std::string_view prefix, std::string_view name, ISerializable& object, Access access) { + Variable* variable = this->next(); + + if (variable == nullptr) { + return invalid_id; + } + + access.stream = false; + + *variable = { + .prefix = prefix, + .name = name, + .address = static_cast(&object), + .size = 0, + .type = TypeCode::BLOB, + .access = access, + }; + + return this->count++; +} + +std::optional VariablePool::find(std::string_view full_name) const { + for (VariableId id = 0; id < this->count; id++) { + const Variable& variable = this->at(id); + + if (full_name.size() == variable.prefix.size() + variable.name.size() and + full_name.starts_with(variable.prefix) and full_name.ends_with(variable.name)) { + return id; + } + } + + return std::nullopt; +} + +const Variable& VariablePool::at(VariableId id) const { + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + return this->storage[id]; +} + +std::span VariablePool::all() const { + return this->storage.first(this->count); +} + +uint16_t VariablePool::read(VariableId id, std::span into) const { + if (id >= this->count) { + return 0; + } + + const Variable& variable = this->at(id); + + if (variable.type == TypeCode::BLOB or into.size() < variable.size) { + return 0; + } + + std::memcpy(into.data(), variable.address, variable.size); + return variable.size; +} + +// NOLINTNEXTLINE(readability-make-member-function-const) it writes through the stored addresses +VariablePool::WriteStatus VariablePool::write(VariableId id, std::span bytes, bool robot_is_idle) { + if (id >= this->count) { + return WriteStatus::NO_SUCH_ID; + } + + const Variable& variable = this->at(id); + + if (not variable.access.write) { + return WriteStatus::READ_ONLY; + } + + if (variable.access.idle and not robot_is_idle) { + return WriteStatus::NEEDS_IDLE; + } + + if (variable.type == TypeCode::BLOB) { + static_cast(variable.address)->deserialize(bytes.data(), bytes.size()); + return WriteStatus::OK; + } + + if (bytes.size() != variable.size) { + return WriteStatus::WRONG_SIZE; + } + + std::memcpy(variable.address, bytes.data(), variable.size); + return WriteStatus::OK; +} + +uint32_t VariablePool::schema_hash() const { + uint32_t hash = 0xFFFFFFFF; + + for (const Variable& variable : this->all()) { + const std::array tail{std::to_underlying(variable.type), std::bit_cast(variable.access)}; + + hash = crc32(as_bytes(variable.prefix), hash); + hash = crc32(as_bytes(variable.name), hash); + hash = crc32(tail, hash); + } + + return hash; +} +} // namespace micras::core From 7c6457362d7834fab63648cb7939033e133c7507 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 21 Sep 2026 21:11:55 -0300 Subject: [PATCH 477/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Keep=20the=20flash?= =?UTF-8?q?=20image=20and=20the=20variables=20that=20fill=20it=20apart?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The storage knew how to reach every variable it saved and every variable knew it was being saved. It kept two maps of its own, a serializer and a deserializer for each of them, and four explicit instantiations to tie them together. All of that is the variable pool now, and what is left is the flash layout and its validation. Registration and loading also stop being the same step. `sync` meant "load this if the image has it, then remember it", so the two had to be interleaved and something had to track which entries had been loaded but not yet claimed. Everything registers first and `restore` runs once. The image gains a type code per entry, because a float saved by one build and read into an integer by the next was accepted silently, and that is the likely failure on a robot whose variable list changes every week. It gains no checksum: the flash corrects single bit errors and reports double ones in hardware, and what that does not cover is an interrupted save, which is answered by giving the header a flash word of its own and writing it last. A save that stops halfway leaves the start symbol erased and the image is refused. Owners register what they already hold, so the readings are sampled where they live and there is no second copy to keep up to date. --- .../micras/core/butterworth_filter.hpp | 15 + micras_core/src/butterworth_filter.cpp | 6 + .../micras/proxy/impl/wall_sensors.tpp | 13 + micras_proxy/include/micras/proxy/imu.hpp | 10 + micras_proxy/include/micras/proxy/storage.hpp | 163 ++++------- .../include/micras/proxy/wall_sensors.hpp | 10 + micras_proxy/src/imu.cpp | 11 + micras_proxy/src/storage.cpp | 274 ++++++++---------- 8 files changed, 249 insertions(+), 253 deletions(-) diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp index 56add89..17b36be 100644 --- a/micras_core/include/micras/core/butterworth_filter.hpp +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -8,6 +8,9 @@ #include #include #include +#include + +#include "micras/core/variable_pool.hpp" namespace micras::core { /** @@ -116,6 +119,18 @@ class ButterworthFilter { */ float get_last() const; + /** + * @brief Expose the filtered value for monitoring. + * + * @note The value is registered where it already lives, so sampling it costs a copy of four + * bytes and the filter gains nothing to keep up to date. + * + * @param pool Pool to register into. + * @param prefix Prefix of the name, identifying the owner of the filter. + * @param name Name of the filtered value inside its owner. + */ + void register_variables(VariablePool& pool, std::string_view prefix, std::string_view name); + private: /** * @brief Last two input values of the filter, most recent first. diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index ba6b03d..2d015f9 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -4,8 +4,10 @@ #include #include +#include #include "micras/core/butterworth_filter.hpp" +#include "micras/core/variable_pool.hpp" namespace micras::core { ButterworthFilter::Coefficients ButterworthFilter::compute_coefficients(const Config& config) { @@ -48,4 +50,8 @@ float ButterworthFilter::update(float x0) { float ButterworthFilter::get_last() const { return this->output; } + +void ButterworthFilter::register_variables(VariablePool& pool, std::string_view prefix, std::string_view name) { + pool.add(prefix, name, this->output, {.stream = true}); +} } // namespace micras::core diff --git a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp index 6ba12e8..849f435 100644 --- a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp +++ b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp @@ -6,12 +6,15 @@ #define MICRAS_PROXY_WALL_SENSORS_TPP #include +#include #include #include #include +#include #include "micras/core/butterworth_filter.hpp" #include "micras/core/utils.hpp" +#include "micras/core/variable_pool.hpp" #include "micras/hal/pwm.hpp" namespace micras::proxy { @@ -96,6 +99,16 @@ void TWallSensors::calibrate_sensor(uint8_t sensor_index) { this->base_readings.at(sensor_index) = this->get_reading(sensor_index); } +template +void TWallSensors::register_variables(core::VariablePool& pool, std::string_view prefix) { + static constexpr std::array names{"0", "1", "2", "3", "4", "5", "6", "7"}; + static_assert(num_of_sensors <= names.size(), "Every sensor needs a name literal to register under"); + + for (uint8_t i = 0; i < num_of_sensors; i++) { + this->filters.at(i).register_variables(pool, prefix, names.at(i)); + } +} + template bool TWallSensors::was_initialized() const { return this->initialized; diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 85cf376..e85fc23 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -9,8 +9,10 @@ #include #include #include +#include #include "micras/core/butterworth_filter.hpp" +#include "micras/core/variable_pool.hpp" #include "micras/hal/spi.hpp" namespace micras::proxy { @@ -108,6 +110,14 @@ class Imu { * * @return True if the device was successfully initialized, false otherwise. */ + /** + * @brief Register the angular velocity and the linear acceleration for monitoring. + * + * @param pool Pool to register into. + * @param prefix Prefix of the names, identifying this sensor. + */ + void register_variables(core::VariablePool& pool, std::string_view prefix); + bool was_initialized() const; private: diff --git a/micras_proxy/include/micras/proxy/storage.hpp b/micras_proxy/include/micras/proxy/storage.hpp index 10d67c5..fd2fdd3 100644 --- a/micras_proxy/include/micras/proxy/storage.hpp +++ b/micras_proxy/include/micras/proxy/storage.hpp @@ -5,21 +5,21 @@ #ifndef MICRAS_PROXY_STORAGE_HPP #define MICRAS_PROXY_STORAGE_HPP +#include #include -#include -#include -#include -#include -#include +#include +#include -#include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" +#include "micras/hal/flash.hpp" namespace micras::proxy { -template -concept Fundamental = std::is_fundamental_v; - /** - * @brief Class for storing variable and classes in the flash memory. + * @brief Class for storing the persistent variables of a pool in the flash memory. + * + * @note Registration and loading are two separate phases. Everything registers into the pool + * first, and a single call to restore then fills whatever the image happens to carry, so nothing + * has to remember which variables have already been claimed. */ class Storage { public: @@ -32,145 +32,108 @@ class Storage { }; /** - * @brief Construct a new Storage object. + * @brief Construct a new Storage object, reading and validating the image in the flash memory. * * @param config Configuration for the storage. */ explicit Storage(const Config& config); /** - * @brief Create a new primitive variable in the storage. + * @brief Check if a valid image was found in the flash memory. * - * @tparam T Type of the variable. - * @param name Name of the variable. - * @param data Reference to the variable. + * @return True if an image was found and parsed, false if the storage started empty. */ - template - void create(const std::string& name, const T& data) { - auto& variable = this->primitives[name]; - variable.ram_pointer = &data; - variable.size = sizeof(T); - } + bool is_valid() const; /** - * @brief Create a new serializable variable in the storage. + * @brief Load every variable of the pool that the image carries. * - * @param name Name of the variable. - * @param data Reference to the variable. - */ - void create(const std::string& name, const core::ISerializable& data); - - /** - * @brief Sync a primitive variable with the storage. + * @note Entries whose name is not registered are ignored, and so are entries whose type no + * longer matches the one registered under that name, which is what stops a float saved by one + * firmware from being read back as an integer by the next. * - * @tparam T Type of the variable. - * @param name Name of the variable. - * @param data Reference to the variable. + * @param pool Pool to load the variables into. + * @return Number of variables that were loaded. */ - template - void sync(const std::string& name, T& data) { - const auto variable = this->primitives.find(name); - - if (variable != this->primitives.end() and variable->second.ram_pointer == nullptr and - variable->second.size == sizeof(T)) { - std::memcpy(&data, &this->buffer.at(variable->second.buffer_address), sizeof(T)); - } - - this->create(name, data); - } + std::size_t restore(core::VariablePool& pool); /** - * @brief Sync a serializable variable with the storage. + * @brief Write every persistent variable of the pool to the flash memory. * - * @param name Name of the variable. - * @param data Reference to the variable. - */ - void sync(const std::string& name, core::ISerializable& data); - - /** - * @brief Save the storage to the flash. + * @note This operation blocks the processor for a few seconds, so it must only be called with + * the robot stopped. * - * @note This operation blocks the processor for a few seconds, so it must only be called with the robot - * stopped. + * @note The header is written last and occupies a flash word of its own, so an interrupted + * save leaves it erased and the image is rejected on the next boot. That is what makes a torn + * write safe without a checksum, which the error correction of the flash memory would make + * redundant anyway. * + * @param pool Pool to take the variables from. * @return True if the data was successfully written to the flash, false otherwise. */ - bool save(); - - /** - * @brief Check if valid data was loaded from the flash. - * - * @return True if the storage was loaded from the flash, false if it started empty. - */ - bool is_valid() const; + bool save(const core::VariablePool& pool); private: /** - * @brief Struct for primitive variables. + * @brief One variable as it is described in the flash image. */ - struct PrimitiveVariable { - const void* ram_pointer{nullptr}; - uint16_t buffer_address{}; - uint16_t size{}; + struct Entry { + std::string_view name; + core::TypeCode type; + uint16_t offset; + uint16_t size; }; /** - * @brief Struct for serializable variables. + * @brief Read and validate the image currently in the flash memory. */ - struct SerializableVariable { - const core::ISerializable* ram_pointer{nullptr}; - uint16_t buffer_address{}; - uint16_t size{}; - }; + void load(); /** - * @brief Serialize a map of variables. + * @brief Parse the entry at the front of a table and advance past it. * - * @tparam T Type of the variables. - * @param variables Map of variables. - * @return Serialized buffer. + * @param table Remaining entry table, advanced past the entry that was read. + * @param entry Entry to fill. + * @return True if an entry could be read and its data fits in the value area, false otherwise. */ - template - static std::vector serialize_var_map(const std::unordered_map& variables); + bool take_entry(std::span& table, Entry& entry) const; /** - * @brief Deserialize a map of variables. - * - * @tparam T Type of the variables. - * @param buffer Serialized buffer, consumed up to the end of the map. - * @param num_vars Number of variables. - * @param variables Map to store the deserialized variables. - * @return True if the buffer contained a consistent map, false otherwise. + * @brief Start symbol, to tell a written image apart from erased flash. + */ + static constexpr uint16_t start_symbol{0xABAB}; + + /** + * @brief Version of the layout described by this class, so that an image written by a firmware + * with a different layout is ignored instead of being misread. */ - template - static bool deserialize_var_map( - std::vector& buffer, uint16_t num_vars, std::unordered_map& variables - ); + static constexpr uint8_t format_version{1}; /** - * @brief Start symbol to avoid reading garbage from flash. + * @brief Number of bytes reserved for the header, which is a whole flash word so that writing + * it cannot disturb the rest of the image. */ - static constexpr uint16_t start_symbol = 0xABAB; + static constexpr uint32_t header_size{hal::FlashWord::size}; /** - * @brief Number of bytes of the header stored at the beginning of the storage. + * @brief Number of bytes of an entry, not counting its name. */ - static constexpr uint16_t header_size = 8; + static constexpr uint32_t entry_overhead{6}; /** - * @brief Map of primitive variables. + * @brief Description of every variable in the image, as a view over the flash memory. */ - std::unordered_map primitives; + std::span entries; /** - * @brief Map of serializable variables. + * @brief Values of every variable in the image, as a view over the flash memory. */ - std::unordered_map serializables; + std::span values; /** - * @brief Serialized buffer for the storage. + * @brief Number of entries in the image. */ - std::vector buffer; + uint8_t entry_count{}; /** * @brief Start sector of the storage in the flash memory. @@ -183,7 +146,7 @@ class Storage { uint16_t number_of_sectors; /** - * @brief Whether valid data was loaded from the flash memory. + * @brief Whether a valid image was found in the flash memory. */ bool valid{}; }; diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 52e2425..cbd9989 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -7,8 +7,10 @@ #include #include +#include #include "micras/core/butterworth_filter.hpp" +#include "micras/core/variable_pool.hpp" #include "micras/hal/adc_dma.hpp" #include "micras/hal/pwm.hpp" @@ -109,6 +111,14 @@ class TWallSensors { * * @return True if the initialization was successful, false otherwise. */ + /** + * @brief Register the readings of the sensors for monitoring. + * + * @param pool Pool to register into. + * @param prefix Prefix of the names, identifying this set of sensors. + */ + void register_variables(core::VariablePool& pool, std::string_view prefix); + bool was_initialized() const; private: diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 5aae772..0e12a77 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -6,8 +6,10 @@ #include #include #include +#include #include "lsm6dsv_reg.h" +#include "micras/core/variable_pool.hpp" #include "micras/proxy/imu.hpp" #include "micras/proxy/stopwatch.hpp" @@ -185,4 +187,13 @@ void Imu::calibrate() { bool Imu::was_initialized() const { return this->initialized; } + +void Imu::register_variables(core::VariablePool& pool, std::string_view prefix) { + pool.add(prefix, "gyro_x", this->angular_velocity.at(0), {.stream = true}); + pool.add(prefix, "gyro_y", this->angular_velocity.at(1), {.stream = true}); + pool.add(prefix, "gyro_z", this->angular_velocity.at(2), {.stream = true}); + pool.add(prefix, "accel_x", this->linear_acceleration.at(0), {.stream = true}); + pool.add(prefix, "accel_y", this->linear_acceleration.at(1), {.stream = true}); + pool.add(prefix, "accel_z", this->linear_acceleration.at(2), {.stream = true}); +} } // namespace micras::proxy diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index fdd45a6..7c8afb7 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -2,88 +2,75 @@ * @file */ -#include #include #include #include +#include #include -#include -#include +#include +#include #include #include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" #include "micras/hal/flash.hpp" #include "micras/proxy/storage.hpp" namespace micras::proxy { -/** - * @brief Read a 16 bit little endian value from a buffer. - * - * @param buffer Buffer to read from. - * @param address Address of the first byte of the value. - * @return Value read from the buffer. - */ -static uint16_t read_uint16(std::span buffer, uint16_t address) { +static uint16_t read_uint16(std::span buffer, uint32_t address) { // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) return static_cast(buffer[address] | buffer[address + 1U] << 8); } -/** - * @brief Append a 16 bit little endian value to a buffer. - * - * @param buffer Buffer to append to. - * @param value Value to append. - */ static void append_uint16(std::vector& buffer, uint16_t value) { buffer.emplace_back(value); buffer.emplace_back(value >> 8); } -/** - * @brief Check if the data of every variable of a map fits inside the buffer. - * - * @tparam T Type of the variables. - * @param variables Map of variables to check. - * @param buffer_size Number of bytes of the buffer. - * @return True if every variable is inside the buffer, false otherwise. - */ -template -static bool validate_var_map(const std::unordered_map& variables, std::size_t buffer_size) { - return std::ranges::all_of(variables, [buffer_size](const auto& variable) { - return variable.second.buffer_address + static_cast(variable.second.size) <= buffer_size; - }); +static constexpr uint32_t align_size(std::size_t size) { + return (size + hal::FlashWord::size - 1) / hal::FlashWord::size * hal::FlashWord::size; } Storage::Storage(const Config& config) : start_sector{config.start_sector}, number_of_sectors{config.number_of_sectors} { + this->load(); +} + +void Storage::load() { + this->entries = {}; + this->values = {}; + this->entry_count = 0; + this->valid = false; + const std::span header = hal::Flash::read(this->start_sector, 0, header_size); - if (header.size() < header_size or read_uint16(header, 0) != start_symbol) { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + if (header.size() < header_size or read_uint16(header, 0) != start_symbol or header[2] != format_version) { return; } - const uint16_t total_size = read_uint16(header, 2); - const uint16_t num_primitives = read_uint16(header, 4); - const uint16_t num_serializables = read_uint16(header, 6); + this->entry_count = header[3]; + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) - const std::span payload = hal::Flash::read(this->start_sector, header_size, 4UL * total_size); + const uint32_t body_size = read_uint16(header, 4) * hal::FlashWord::size; + const std::span body = hal::Flash::read(this->start_sector, header_size, body_size); - if (payload.size() < 4UL * total_size) { + if (body.size() < body_size) { return; } - this->buffer.assign(payload.begin(), payload.end()); + std::span table = body; - if (not deserialize_var_map(this->buffer, num_primitives, this->primitives) or - not deserialize_var_map(this->buffer, num_serializables, this->serializables) or - not validate_var_map(this->primitives, this->buffer.size()) or - not validate_var_map(this->serializables, this->buffer.size())) { - this->primitives.clear(); - this->serializables.clear(); - this->buffer.clear(); - return; + for (uint8_t index = 0; index < this->entry_count; index++) { + if (table.empty() or table.size() < table.front() + entry_overhead) { + return; + } + + table = table.subspan(table.front() + entry_overhead); } + this->entries = body.first(body.size() - table.size()); + this->values = table; this->valid = true; } @@ -91,151 +78,132 @@ bool Storage::is_valid() const { return this->valid; } -void Storage::create(const std::string& name, const core::ISerializable& data) { - this->serializables[name].ram_pointer = &data; -} +bool Storage::take_entry(std::span& table, Entry& entry) const { + const uint8_t name_size = table.front(); -void Storage::sync(const std::string& name, core::ISerializable& data) { - const auto serializable = this->serializables.find(name); + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) + entry.name = {std::bit_cast(table.data() + 1), name_size}; + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + entry.type = static_cast(table[name_size + 1U]); + entry.offset = read_uint16(table, name_size + 2U); + entry.size = read_uint16(table, name_size + 4U); - if (serializable != this->serializables.end() and serializable->second.ram_pointer == nullptr) { - data.deserialize(&this->buffer.at(serializable->second.buffer_address), serializable->second.size); - } + table = table.subspan(name_size + entry_overhead); - this->create(name, data); + return entry.offset + static_cast(entry.size) <= this->values.size(); } -bool Storage::save() { - this->buffer.clear(); +std::size_t Storage::restore(core::VariablePool& pool) { + if (not this->valid) { + return 0; + } + + std::span table = this->entries; + std::size_t restored = 0; - for (auto it = this->primitives.begin(); it != this->primitives.end();) { - auto& [name, variable] = *it; + for (uint8_t index = 0; index < this->entry_count; index++) { + Entry entry{}; - if (variable.ram_pointer == nullptr) { - it = this->primitives.erase(it); + if (not this->take_entry(table, entry)) { continue; } - const auto* aux = std::bit_cast(variable.ram_pointer); - variable.buffer_address = buffer.size(); - - // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) - this->buffer.insert(this->buffer.end(), aux, aux + variable.size); - it++; - } - - for (auto it = this->serializables.begin(); it != this->serializables.end();) { - auto& [name, variable] = *it; + const auto id = pool.find(entry.name); - if (variable.ram_pointer == nullptr) { - it = this->serializables.erase(it); + if (not id.has_value()) { continue; } - std::vector aux = variable.ram_pointer->serialize(); - variable.buffer_address = this->buffer.size(); - variable.size = aux.size(); - this->buffer.insert(this->buffer.end(), aux.begin(), aux.end()); - it++; - } - - auto serialized_serializables = serialize_var_map(this->serializables); - this->buffer.insert(this->buffer.begin(), serialized_serializables.begin(), serialized_serializables.end()); - - auto serialized_primitives = serialize_var_map(this->primitives); - this->buffer.insert(this->buffer.begin(), serialized_primitives.begin(), serialized_primitives.end()); - - this->buffer.insert(this->buffer.end(), (4 - (this->buffer.size() % 4)) % 4, 0); - const uint16_t total_size = this->buffer.size() / 4; + const core::Variable& variable = pool.at(id.value()); - std::vector header; - header.reserve(header_size); - append_uint16(header, start_symbol); - append_uint16(header, total_size); - append_uint16(header, this->primitives.size()); - append_uint16(header, this->serializables.size()); - - this->buffer.insert(this->buffer.begin(), header.begin(), header.end()); + if (not variable.access.persist or variable.type != entry.type) { + continue; + } - if (this->buffer.size() > this->number_of_sectors * hal::Flash::sector_size) { - return false; - } + const std::span data = this->values.subspan(entry.offset, entry.size); - if (hal::Flash::erase_sectors(this->start_sector, this->number_of_sectors) != hal::Flash::Status::OK) { - return false; - } + if (variable.type == core::TypeCode::BLOB) { + static_cast(variable.address)->deserialize(data.data(), data.size()); + } else if (variable.size != entry.size) { + continue; + } else { + std::memcpy(variable.address, data.data(), data.size()); + } - if (hal::Flash::write(this->start_sector, 0, this->buffer) != hal::Flash::Status::OK) { - return false; + restored++; } - this->valid = true; - return true; + return restored; } -template -std::vector Storage::serialize_var_map(const std::unordered_map& variables) { - std::vector buffer; - - for (const auto& [name, variable] : variables) { - buffer.emplace_back(name.size()); - buffer.insert(buffer.end(), name.begin(), name.end()); +bool Storage::save(const core::VariablePool& pool) { + std::vector table; + std::vector payload; + uint8_t count = 0; - buffer.emplace_back(variable.buffer_address); - buffer.emplace_back(variable.buffer_address >> 8); - - buffer.emplace_back(variable.size); - buffer.emplace_back(variable.size >> 8); - } - - return buffer; -} + for (const core::Variable& variable : pool.all()) { + if (not variable.access.persist) { + continue; + } -template -bool Storage::deserialize_var_map( - std::vector& buffer, uint16_t num_vars, std::unordered_map& variables -) { - uint16_t current_addr = 0; + const std::size_t name_size = variable.prefix.size() + variable.name.size(); + const std::size_t offset = payload.size(); - for (uint16_t decoded_vars = 0; decoded_vars < num_vars; decoded_vars++) { - if (current_addr >= buffer.size()) { - return false; + if (variable.type == core::TypeCode::BLOB) { + const std::vector data = static_cast(variable.address)->serialize(); + payload.insert(payload.end(), data.begin(), data.end()); + } else { + const auto* bytes = std::bit_cast(variable.address); + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) + payload.insert(payload.end(), bytes, bytes + variable.size); } - const uint8_t var_name_len = buffer.at(current_addr); + const std::size_t size = payload.size() - offset; - if (current_addr + var_name_len + 5UL > buffer.size()) { + if (count == UINT8_MAX or name_size > UINT8_MAX or offset > UINT16_MAX or size > UINT16_MAX) { return false; } - const std::string var_name(buffer.begin() + current_addr + 1, buffer.begin() + current_addr + 1 + var_name_len); - current_addr += var_name_len + 1; + table.emplace_back(name_size); + table.insert(table.end(), variable.prefix.begin(), variable.prefix.end()); + table.insert(table.end(), variable.name.begin(), variable.name.end()); + table.emplace_back(std::to_underlying(variable.type)); + append_uint16(table, offset); + append_uint16(table, size); - variables[var_name].buffer_address = read_uint16(buffer, current_addr); - current_addr += 2; + count++; + } - variables.at(var_name).size = read_uint16(buffer, current_addr); - current_addr += 2; + std::vector body = std::move(table); + body.insert(body.end(), payload.begin(), payload.end()); + body.resize(align_size(body.size()), hal::FlashWord::erased_value); + + if (body.size() / hal::FlashWord::size > UINT16_MAX or + header_size + body.size() > this->number_of_sectors * hal::Flash::sector_size) { + return false; } - buffer.erase(buffer.begin(), buffer.begin() + current_addr); - return true; -} + if (hal::Flash::erase_sectors(this->start_sector, this->number_of_sectors) != hal::Flash::Status::OK) { + return false; + } -// Explicit instantiation of template functions -template std::vector - Storage::serialize_var_map(const std::unordered_map& variables); + if (hal::Flash::write(this->start_sector, header_size, body) != hal::Flash::Status::OK) { + return false; + } -template bool Storage::deserialize_var_map( - std::vector& buffer, uint16_t num_vars, - std::unordered_map& variables -); + std::vector header; + header.reserve(header_size); + append_uint16(header, start_symbol); + header.emplace_back(format_version); + header.emplace_back(count); + append_uint16(header, body.size() / hal::FlashWord::size); + header.resize(header_size, hal::FlashWord::erased_value); -template std::vector - Storage::serialize_var_map(const std::unordered_map& variables); + if (hal::Flash::write(this->start_sector, 0, header) != hal::Flash::Status::OK) { + return false; + } -template bool Storage::deserialize_var_map( - std::vector& buffer, uint16_t num_vars, - std::unordered_map& variables -); + this->load(); + return this->valid; +} } // namespace micras::proxy From d2c04a5ed9fce17c3675cb638bf8230369584a60 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 21 Sep 2026 21:12:14 -0300 Subject: [PATCH 478/561] =?UTF-8?q?=E2=9C=A8=20Let=20the=20robot=20be=20wa?= =?UTF-8?q?tched=20and=20steered=20over=20a=20link?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The session walks the variable pool and answers for it: a handshake stamped with the schema hash, the schema itself in pages, coherent groups of samples, reads, writes, commands, logs and a full rate capture. Samples come in groups rather than one variable at a time. A response plotted against a setpoint captured two iterations later is not a plot of a control loop, it is a plot of a control loop plus an unknown delay, and at eight kilohertz there is no way to see which. Every variable of a group is copied in the same iteration and carries one timestamp for all of them. The robot never has more than a window of bytes outstanding. The radio module has no flow control on this board and drops silently when its buffer fills, so the only thing that can bound what it is handed is the application saying how much it has taken. When the window is closed samples are dropped and the sequence number shows the gap, which is the right answer for live monitoring. Replies are not charged to the window, because the rate of the requests already bounds them. Writes set a level and are acknowledged with a reason when they are refused; commands happen once, when they arrive. Keeping those apart is what stops a command from being re-applied on every iteration. The link is twenty to a hundred times too slow to carry the control loop, so the trace records a group into a ring in RAM at loop rate, keeps a share of it from before the trigger, and is read out afterwards with the robot stopped. Nothing here knows what it is running over, so it can be finished and debugged against a socket. --- micras_comm/CMakeLists.txt | 19 + micras_comm/README.md | 84 ++++ micras_comm/include/micras/comm/frame.hpp | 192 ++++++++ micras_comm/include/micras/comm/group.hpp | 71 +++ micras_comm/include/micras/comm/link.hpp | 264 ++++++++++ micras_comm/include/micras/comm/protocol.hpp | 127 +++++ micras_comm/include/micras/comm/trace.hpp | 190 ++++++++ micras_comm/src/frame.cpp | 179 +++++++ micras_comm/src/group.cpp | 26 + micras_comm/src/link.cpp | 487 +++++++++++++++++++ micras_comm/src/trace.cpp | 163 +++++++ 11 files changed, 1802 insertions(+) create mode 100644 micras_comm/CMakeLists.txt create mode 100644 micras_comm/README.md create mode 100644 micras_comm/include/micras/comm/frame.hpp create mode 100644 micras_comm/include/micras/comm/group.hpp create mode 100644 micras_comm/include/micras/comm/link.hpp create mode 100644 micras_comm/include/micras/comm/protocol.hpp create mode 100644 micras_comm/include/micras/comm/trace.hpp create mode 100644 micras_comm/src/frame.cpp create mode 100644 micras_comm/src/group.cpp create mode 100644 micras_comm/src/link.cpp create mode 100644 micras_comm/src/trace.cpp diff --git a/micras_comm/CMakeLists.txt b/micras_comm/CMakeLists.txt new file mode 100644 index 0000000..ad0bdc6 --- /dev/null +++ b/micras_comm/CMakeLists.txt @@ -0,0 +1,19 @@ +project(micras_comm) + +file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") + +add_library(${PROJECT_NAME} STATIC + ${PROJECT_SOURCES} +) + +add_library(micras::comm ALIAS ${PROJECT_NAME}) + +target_include_directories(${PROJECT_NAME} PUBLIC + include +) + +# Only the reflection and the framing, so that the whole session layer can be built and tested +# without a single peripheral behind it +target_link_libraries(${PROJECT_NAME} PUBLIC + micras::core +) diff --git a/micras_comm/README.md b/micras_comm/README.md new file mode 100644 index 0000000..6c9e719 --- /dev/null +++ b/micras_comm/README.md @@ -0,0 +1,84 @@ +# Bluetooth link + +The robot exposes its variables over an HM-19 BLE module wired to `UART4` (`PA11` = RX, +`PA12` = TX) through the USB-C receptacle. The companion application is +[micras-monitor](https://github.com/Team-Micras/micras-monitor). + +## Wiring + +The module's serial port *is* the USB-C receptacle. `D-` reaches `PA11` and `D+` reaches `PA12`, +each through a 33 ฮฉ series resistor, and `SBU1`/`SBU2` supply 3V3 through a 300 mA resettable fuse. +`CC1` and `CC2` terminate in 5.1 kฮฉ resistors and never reach the microcontroller. + +Two consequences follow from that, and both shape the protocol: + +- **There is no hardware flow control.** The CC2640R2F inside the module has `RTS`/`CTS`, but the + board cannot reach them, and every other conductor of the receptacle is already used. The module + therefore drops bytes silently when its buffer fills, which is what the credit window in the + session layer exists to prevent. +- **USB device mode does not exist on this board.** The receptacle is a serial port, not a USB one. + +## Configuring the module + +The module needs these once, over AT commands, with no application connected. Send them without a +line ending and wait for each reply. + +| Command | Reply | Why | +| --- | --- | --- | +| `AT` | `OK` | Check that the module is in command mode. | +| `AT+ROLE0` | `OK+Set:0` | Peripheral. The application is the central. | +| `AT+NAMEMicras` | `OK+Set:Micras` | What the application looks for. | +| `AT+BAUD4` | `OK+Set:4` | 115 200, matching `MX_UART4_Init`. | +| `AT+COMI0` | `OK+Set:0` | Ask for a 7.5 ms minimum connection interval. | +| `AT+COMA0` | `OK+Set:0` | Ask for a 7.5 ms maximum connection interval. | +| `AT+RESET` | `OK+RESET` | Apply. | + +`AT+COMI` and `AT+COMA` are a request: the central decides the interval it actually uses, and a +browser gives the page no way to read or influence it. The throughput is therefore only knowable by +measuring it, which the `link/dropped_samples` counter and the sample sequence numbers make easy. + +Raising the serial speed to 230 400 is `AT+BAUD8` here and `Dma.UART4_RX` plus the baud rate in +`cube/micras_v1.ioc` on the firmware side. It is only worth doing once a measurement shows the +serial port, rather than the radio, is the limit. + +The application connects to service `FFE0`, characteristic `FFE1`, with notifications enabled. + +## Protocol + +Frames are [COBS](https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing) encoded and +delimited by a zero byte: + +```text +COBS( type | payload | crc16 ) 0x00 +``` + +The CRC-16/CCITT-FALSE covers the type and the payload, before encoding. It is not protecting +against the radio, which has a CRC-24 and retransmits until acknowledged. It covers the two hops +BLE never sees: the 8N1 serial port with no parity between the microcontroller and the module, and +the module's buffer, which drops runs of bytes with no indication that it did. A dropped run can +join the head of one frame to the tail of the next into something with a plausible shape, which a +sum passes far too often and a CRC does not. + +Message types, their payloads and the meaning of every field are in +`micras_comm/include/micras/comm/protocol.hpp`. Everything on the wire is little endian, which is +what both the microcontroller and `DataView` in the browser already are. + +### The parts worth knowing before reading the code + +- **The schema is fetched once per firmware build, not once per connection.** `HELLO_ACK` carries a + `schema_hash` over every name, type and access flag in order. Identifiers are registration order, + so adding one variable shifts every later one; comparing the hash against the one a cached schema + was fetched with is what stops the application from plotting the wrong signal. +- **Samples come in groups, not one variable at a time.** A `SAMPLE` carries several variables + captured in the same control loop iteration under one timestamp. A response plotted against a + setpoint captured two iterations later is not a plot of a control loop. +- **The application has to return credit.** The robot may have at most `initial_credit` bytes + outstanding. `CREDIT` says how many more bytes the application has taken. When the window is + closed, samples are dropped and the sequence number shows the gap; replies to requests are not + charged to the window, because they are already bounded by the rate of the requests themselves. +- **Writes are levels and commands are edges.** `WRITE` sets a gain or a flag and is acknowledged + with a result; the `idle` flag on a variable refuses the dangerous ones while the robot is moving. + `COMMAND` happens once, when it arrives. +- **The trace is a scope.** The link is between twenty and a hundred times too slow to carry the + 8 kHz control loop, so `TRACE_ARM` records a group into a RAM ring at loop rate, keeping a share + of the capture from before the trigger, and `TRACE_READ` pulls it out afterwards. diff --git a/micras_comm/include/micras/comm/frame.hpp b/micras_comm/include/micras/comm/frame.hpp new file mode 100644 index 0000000..7927d15 --- /dev/null +++ b/micras_comm/include/micras/comm/frame.hpp @@ -0,0 +1,192 @@ +/** + * @file + */ + +#ifndef MICRAS_COMM_FRAME_HPP +#define MICRAS_COMM_FRAME_HPP + +#include +#include +#include +#include +#include + +#include "micras/comm/protocol.hpp" + +namespace micras::comm { +/** + * @brief Append little endian values to a payload buffer. + * + * @note Every write is checked against the end of the buffer and the first one that does not fit + * poisons the writer, so a caller only has to check once, at the end. + */ +class Writer { +public: + /** + * @brief Construct a new Writer object. + * + * @param into Buffer to write into. + */ + explicit Writer(std::span into); + + /** + * @brief Append a value of one, two or four bytes. + */ + ///@{ + void u8(uint8_t value); + void u16(uint16_t value); + void u32(uint32_t value); + ///@} + + /** + * @brief Append raw bytes. + * + * @param bytes Bytes to append. + */ + void raw(std::span bytes); + + /** + * @brief Append a string with no length prefix. + * + * @param text Text to append. + */ + void text(std::string_view value); + + /** + * @brief Get what has been written so far. + * + * @return View over the written bytes, empty if anything did not fit. + */ + std::span done() const; + + /** + * @brief Get how many bytes can still be appended. + * + * @return Number of bytes left in the buffer. + */ + std::size_t left() const; + +private: + std::span buffer; + std::size_t size{}; + bool overflowed{}; +}; + +/** + * @brief Take little endian values from a payload buffer. + * + * @note Reading past the end yields zero and marks the reader as bad, so a caller only has to + * check once, after taking everything it expects. + */ +class Reader { +public: + /** + * @brief Construct a new Reader object. + * + * @param from Buffer to read from. + */ + explicit Reader(std::span from); + + /** + * @brief Take a value of one, two or four bytes. + */ + ///@{ + uint8_t u8(); + uint16_t u16(); + uint32_t u32(); + ///@} + + /** + * @brief Take everything that is left. + * + * @return View over the remaining bytes. + */ + std::span rest() const; + + /** + * @brief Check whether every value taken so far was inside the buffer. + * + * @return True if nothing was read past the end, false otherwise. + */ + bool valid() const; + +private: + std::span buffer; + std::size_t index{}; + bool underflowed{}; +}; + +/** + * @brief Build a complete frame around a payload. + * + * @param type Type of the message. + * @param payload Payload of the message. + * @param into Buffer to build the frame in, which should hold max_frame_size bytes. + * @return Number of bytes written, or zero if the frame does not fit. + */ +std::size_t encode_frame(MessageType type, std::span payload, std::span into); + +/** + * @brief Recover frames from a stream of bytes. + * + * @note Every encoded frame is free of the delimiter by construction, so a corrupt or truncated + * frame costs exactly the bytes up to the next delimiter and never desynchronizes the reader. + */ +class FrameReader { +public: + /** + * @brief Feed one byte. + * + * @param byte Byte taken from the stream. + * @return True if a whole valid frame is now available, false otherwise. + */ + bool push(uint8_t byte); + + /** + * @brief Get the type of the frame that was just completed. + * + * @return Type of the message. + */ + MessageType type() const; + + /** + * @brief Get the payload of the frame that was just completed. + * + * @return View over the payload. + */ + std::span payload() const; + + /** + * @brief Get how many frames were thrown away for failing the frame check or for being + * malformed. + * + * @return Number of frames discarded since the reader was created. + */ + uint32_t discarded() const; + +private: + /** + * @brief Decode and check the frame currently in the buffer. + * + * @return True if the frame is whole and passes the frame check, false otherwise. + */ + bool finish(); + + /** + * @brief Bytes of the current frame, still encoded. + */ + std::array encoded{}; + + /** + * @brief Decoded type, payload and frame check of the current frame. + */ + std::array decoded{}; + + std::size_t encoded_size{}; + std::size_t decoded_size{}; + uint32_t discarded_frames{}; + bool overrun{}; +}; +} // namespace micras::comm + +#endif // MICRAS_COMM_FRAME_HPP diff --git a/micras_comm/include/micras/comm/group.hpp b/micras_comm/include/micras/comm/group.hpp new file mode 100644 index 0000000..9a9ff77 --- /dev/null +++ b/micras_comm/include/micras/comm/group.hpp @@ -0,0 +1,71 @@ +/** + * @file + */ + +#ifndef MICRAS_COMM_GROUP_HPP +#define MICRAS_COMM_GROUP_HPP + +#include +#include +#include +#include + +#include "micras/comm/protocol.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::comm { +/** + * @brief A set of variables captured in the same loop iteration. + * + * @note Sampling several variables under one header and one timestamp is not only cheaper on a link + * this slow, it is the only way the samples mean anything together. A response plotted against a + * setpoint captured two iterations later is a plot of the loop plus an unknown delay. + */ +struct Group { + /** + * @brief Variables of the group, in the order their values are packed. + */ + std::array ids{}; + + /** + * @brief Number of variables in the group. + */ + uint8_t count{}; + + /** + * @brief Number of loop iterations between two samples. + */ + uint16_t period{1}; + + /** + * @brief Number of bytes of the values of one sample. + */ + uint16_t sample_size{}; + + /** + * @brief Iterations left until the next sample. + */ + uint16_t counter{}; + + /** + * @brief Number of samples taken, which lets the application see the ones that were dropped. + */ + uint16_t sequence{}; + + /** + * @brief Whether the group is being sent. + */ + bool enabled{}; + + /** + * @brief Copy the current value of every variable of the group. + * + * @param pool Pool the variables belong to. + * @param into Buffer to copy into. + * @return Number of bytes copied, zero if the buffer is too small. + */ + std::size_t sample(const core::VariablePool& pool, std::span into) const; +}; +} // namespace micras::comm + +#endif // MICRAS_COMM_GROUP_HPP diff --git a/micras_comm/include/micras/comm/link.hpp b/micras_comm/include/micras/comm/link.hpp new file mode 100644 index 0000000..ede0aaf --- /dev/null +++ b/micras_comm/include/micras/comm/link.hpp @@ -0,0 +1,264 @@ +/** + * @file + */ + +#ifndef MICRAS_COMM_LINK_HPP +#define MICRAS_COMM_LINK_HPP + +#include +#include +#include +#include +#include + +#include "micras/comm/frame.hpp" +#include "micras/comm/group.hpp" +#include "micras/comm/protocol.hpp" +#include "micras/comm/trace.hpp" +#include "micras/core/byte_stream.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::comm { +/** + * @brief Result of a command sent over the link. + */ +enum class CommandResult : uint8_t { + OK = 0, + UNKNOWN = 1, + REFUSED = 2 +}; + +/** + * @brief Interface for whatever turns a command into an action on the robot. + * + * @note Commands are edges and writes are levels, which is the distinction that keeps a command + * from being re-applied on every iteration. The link never holds a command flag of its own. + */ +class ICommandHandler { +public: + /** + * @brief Virtual destructor for the ICommandHandler class. + */ + virtual ~ICommandHandler() = default; + + /** + * @brief Act on a command. + * + * @param code Command to run. + * @param argument Argument of the command. + * @return Whether the command ran. + */ + virtual CommandResult handle_command(uint8_t code, uint32_t argument) = 0; + +protected: + /** + * @brief Special member functions declared as default. + */ + ///@{ + ICommandHandler() = default; + ICommandHandler(const ICommandHandler&) = default; + ICommandHandler(ICommandHandler&&) = default; + ICommandHandler& operator=(const ICommandHandler&) = default; + ICommandHandler& operator=(ICommandHandler&&) = default; + ///@} +}; + +/** + * @brief Session over a byte stream, exposing the variable pool to the outside world. + */ +class Link { +public: + /** + * @brief Configuration struct for the link. + */ + struct Config { + /** + * @brief Period of the control loop in microseconds, which is the unit of a group period. + */ + uint32_t loop_time_us; + }; + + /** + * @brief Construct a new Link object. + * + * @param stream Transport the session runs over. + * @param pool Variables the session exposes. + * @param trace Capture buffer the session arms and reads out. + * @param commands Handler the commands are given to. + * @param config Configuration for the link. + */ + Link( + core::IByteStream& stream, core::VariablePool& pool, Trace& trace, ICommandHandler& commands, + const Config& config + ); + + /** + * @brief Take whatever arrived and act on at most one message. + * + * @note Limiting this to one message per iteration is what keeps a burst of requests from + * spending the loop budget, and one message per 125 us is far faster than the link can deliver + * them anyway. + * + * @param robot_is_idle Whether the robot is stopped, which gates the guarded writes. + */ + void poll(bool robot_is_idle); + + /** + * @brief Send whatever is due. + * + * @note Called every iteration. It costs a few compares when no group is due. + * + * @param timestamp_us Current time in microseconds, stamped on the samples. + */ + void pump(uint32_t timestamp_us); + + /** + * @brief Send a message to the application. + * + * @note Dropped when the transport is full rather than queued, and the number of dropped + * messages is itself registered in the pool, so a gap is visible instead of silent. + * + * @param severity Severity of the message. + * @param text Message to send. + */ + void log(Severity severity, std::string_view text); + + /** + * @brief Register the counters of the link itself. + * + * @param pool Pool to register into. + * @param prefix Prefix of the names. + */ + void register_variables(core::VariablePool& pool, std::string_view prefix); + +private: + /** + * @brief Act on one decoded message. + * + * @param robot_is_idle Whether the robot is stopped. + */ + void execute(bool robot_is_idle); + + /** + * @brief Handlers for each kind of request. + */ + ///@{ + void on_hello(); + void on_schema_request(Reader& reader); + void on_group_define(Reader& reader); + void on_group_enable(Reader& reader); + void on_credit(Reader& reader); + void on_write(Reader& reader, bool robot_is_idle); + void on_read(Reader& reader); + void on_command(Reader& reader); + void on_trace_arm(Reader& reader); + void on_trace_read(Reader& reader); + ///@} + + /** + * @brief Send one page of the schema, if one was asked for. + * + * @return True if a frame was sent, false otherwise. + */ + bool send_schema_page(); + + /** + * @brief Send one block of the capture, if one was asked for. + * + * @return True if a frame was sent, false otherwise. + */ + bool send_trace_block(); + + /** + * @brief Send the state of the capture. + */ + void send_trace_status(); + + /** + * @brief Send an error for a message that could not be acted on. + * + * @param code Reason the message was refused. + * @param context Whatever identifies the offending message. + */ + void send_error(ErrorCode code, uint16_t context); + + /** + * @brief Send a frame the application asked for, which is bounded by its own request rate and + * so is not charged to the credit window. + * + * @param type Type of the message. + * @param payload Payload of the message. + * @return True if the transport took the frame, false otherwise. + */ + bool send(MessageType type, std::span payload); + + /** + * @brief Send a frame the robot produced on its own, which the credit window has to allow. + * + * @param type Type of the message. + * @param payload Payload of the message. + * @return True if the transport took the frame, false otherwise. + */ + bool send_metered(MessageType type, std::span payload); + + // NOLINTBEGIN(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + core::IByteStream& stream; + core::VariablePool& pool; + Trace& trace; + ICommandHandler& commands; + // NOLINTEND(*-avoid-const-or-ref-data-members) + + Config config; + + FrameReader reader; + + /** + * @brief Bytes taken from the transport but not yet fed to the frame reader. + */ + ///@{ + std::array staging{}; + std::size_t staged{}; + std::size_t consumed{}; + ///@} + + /** + * @brief Scratch buffers for the message being built and for the frame around it. + */ + ///@{ + std::array payload{}; + std::array frame{}; + ///@} + + std::array groups{}; + + /** + * @brief Bytes the application still allows to be sent on the robot's own initiative. + */ + int32_t credit{initial_credit}; + + /** + * @brief Index of the next schema entry to send, past the last one when no page is due. + */ + uint16_t schema_index{}; + + /** + * @brief Offset of the next block of the capture to send, past the end when none is due. + */ + uint32_t trace_offset{}; + + /** + * @brief Whether a capture is being read out. + */ + bool trace_dumping{}; + + /** + * @brief Counters worth watching from the application. + */ + ///@{ + uint32_t dropped_samples{}; + uint32_t dropped_logs{}; + ///@} +}; +} // namespace micras::comm + +#endif // MICRAS_COMM_LINK_HPP diff --git a/micras_comm/include/micras/comm/protocol.hpp b/micras_comm/include/micras/comm/protocol.hpp new file mode 100644 index 0000000..4588130 --- /dev/null +++ b/micras_comm/include/micras/comm/protocol.hpp @@ -0,0 +1,127 @@ +/** + * @file + */ + +#ifndef MICRAS_COMM_PROTOCOL_HPP +#define MICRAS_COMM_PROTOCOL_HPP + +#include +#include + +#include "micras/core/cobs.hpp" + +namespace micras::comm { +/** + * @brief Version of the wire format, refused by the application when it does not match. + */ +constexpr uint8_t protocol_version{1}; + +/** + * @brief Largest payload of a message, not counting the type and the frame check. + */ +constexpr std::size_t max_payload_size{200}; + +/** + * @brief Largest size of an encoded frame, including its delimiter. + */ +constexpr std::size_t max_frame_size{core::cobs_encoded_size(max_payload_size + 3) + 1}; + +/** + * @brief Number of groups that can be defined at the same time. + */ +constexpr uint8_t max_groups{4}; + +/** + * @brief Number of variables a single group can hold. + */ +constexpr uint8_t max_group_variables{16}; + +/** + * @brief Number of bytes the robot may send before the application returns any credit. + * + * @note The radio module has no flow control and drops silently when its buffer fills, so this is + * the only thing keeping the firmware from overrunning it. + */ +constexpr uint16_t initial_credit{256}; + +/** + * @brief Type of a message, which is the first byte of every frame. + * + * @note Requests have the top bit clear and responses have it set, so that the direction of a + * frame can be told from its type alone. + * + * @note PING exists because a radio link can stall without either end being told, and because the + * only other round trip that proves the robot is alive is HELLO, which resets the session. + */ +enum class MessageType : uint8_t { + HELLO = 0x01, + SCHEMA_REQUEST = 0x02, + GROUP_DEFINE = 0x03, + GROUP_ENABLE = 0x04, + CREDIT = 0x05, + WRITE = 0x06, + READ = 0x07, + COMMAND = 0x08, + TRACE_ARM = 0x09, + TRACE_READ = 0x0A, + PING = 0x0B, + + HELLO_ACK = 0x81, + SCHEMA_PAGE = 0x82, + GROUP_ACK = 0x83, + SAMPLE = 0x85, + WRITE_ACK = 0x86, + VALUE = 0x87, + COMMAND_ACK = 0x88, + LOG = 0x89, + TRACE_STATUS = 0x8A, + TRACE_DATA = 0x8B, + PONG = 0x8C, + ERROR = 0x8F +}; + +/** + * @brief Reason a frame could not be acted on. + */ +enum class ErrorCode : uint8_t { + UNKNOWN_TYPE = 0, + MALFORMED = 1, + NO_SUCH_GROUP = 2, + GROUP_TOO_LARGE = 3, + NOT_STREAMABLE = 4, + NO_SUCH_VARIABLE = 5, + BUSY = 6 +}; + +/** + * @brief Severity of a log message. + */ +enum class Severity : uint8_t { + DEBUG = 0, + INFO = 1, + WARNING = 2, + ERROR = 3 +}; + +/** + * @brief What starts a trace capture. + */ +enum class TriggerType : uint8_t { + IMMEDIATE = 0, + COMMAND = 1, + ABOVE = 2, + BELOW = 3 +}; + +/** + * @brief State of the trace capture. + */ +enum class TraceState : uint8_t { + IDLE = 0, + ARMED = 1, + TRIGGERED = 2, + FULL = 3 +}; +} // namespace micras::comm + +#endif // MICRAS_COMM_PROTOCOL_HPP diff --git a/micras_comm/include/micras/comm/trace.hpp b/micras_comm/include/micras/comm/trace.hpp new file mode 100644 index 0000000..fd51392 --- /dev/null +++ b/micras_comm/include/micras/comm/trace.hpp @@ -0,0 +1,190 @@ +/** + * @file + */ + +#ifndef MICRAS_COMM_TRACE_HPP +#define MICRAS_COMM_TRACE_HPP + +#include +#include +#include + +#include "micras/comm/group.hpp" +#include "micras/comm/protocol.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::comm { +/** + * @brief Full rate capture of a group into a ring in RAM. + * + * @note The link is between twenty and a hundred times too slow to carry the control loop, so the + * only way to see what happened inside a fast run is to record it at loop rate and read it out + * afterwards. Capturing is a copy into a ring and nothing else, and nothing goes on the wire until + * the capture has stopped. + * + * @note Samples carry no timestamp of their own. The period is fixed at arming, so the instant of + * sample i is the trigger timestamp plus i times the period, which is four bytes per sample saved + * on the only path where they would be paid at loop rate. + */ +class Trace { +public: + /** + * @brief Construct a new Trace object. + * + * @param pool Pool the captured variables belong to. + * @param ring Memory to capture into, which bounds how long a capture can be. + */ + Trace(const core::VariablePool& pool, std::span ring); + + /** + * @brief Arm a capture. + * + * @param group Group to capture, which must already be defined. + * @param pre_trigger Share of the capture kept from before the trigger, in percent. + * @param trigger What starts the capture. + * @param id Variable watched by a threshold trigger. + * @param threshold Value the watched variable has to cross. + * @return True if the capture was armed, false if the group does not fit or the trigger makes + * no sense for the variable it watches. + */ + bool arm(const Group& group, uint8_t pre_trigger, TriggerType trigger, core::VariableId id, float threshold); + + /** + * @brief Start a capture waiting on a command trigger. + */ + void fire(); + + /** + * @brief Abandon the current capture. + */ + void stop(); + + /** + * @brief Take one sample, if a capture is running. + * + * @note Called every loop iteration. It costs a compare when nothing is armed. + * + * @param timestamp_us Current time in microseconds, recorded at the trigger. + */ + void sample(uint32_t timestamp_us); + + /** + * @brief Get the state of the capture. + * + * @return Current state. + */ + TraceState state() const; + + /** + * @brief Get how many samples are held. + * + * @return Number of samples that can be read. + */ + uint32_t held() const; + + /** + * @brief Get how many of the held samples precede the trigger. + * + * @return Number of samples taken before the trigger fired. + */ + uint32_t pre_trigger() const; + + /** + * @brief Get the number of bytes of one sample. + * + * @return Size of a sample in bytes. + */ + uint16_t sample_size() const; + + /** + * @brief Get the period of the capture. + * + * @return Number of loop iterations between two samples. + */ + uint16_t period() const; + + /** + * @brief Get the time the trigger fired. + * + * @return Timestamp of the first sample after the trigger, in microseconds. + */ + uint32_t timestamp() const; + + /** + * @brief Copy captured bytes, oldest first. + * + * @param offset Byte offset into the capture, counting from the oldest sample held. + * @param into Buffer to copy into. + * @return Number of bytes copied. + */ + std::size_t read(uint32_t offset, std::span into) const; + +private: + /** + * @brief Check whether a threshold trigger has just been crossed. + * + * @return True if the capture should start now, false otherwise. + */ + bool triggered_by_threshold(); + + /** + * @brief Write the current values of the group at the head of the ring. + */ + void store(); + + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const core::VariablePool& pool; + + std::span ring; + + Group group; + + /** + * @brief Number of samples the ring can hold. + */ + uint32_t capacity{}; + + /** + * @brief Number of samples written since the capture was armed, saturated at the capacity. + */ + uint32_t filled{}; + + /** + * @brief Index of the sample that will be written next. + */ + uint32_t head{}; + + /** + * @brief Number of samples still to take before the capture is complete. + */ + uint32_t remaining{}; + + /** + * @brief Number of samples requested from before the trigger, and how many were actually kept. + */ + ///@{ + uint32_t requested_pre{}; + uint32_t kept_pre{}; + ///@} + + /** + * @brief Iterations left until the next sample. + */ + uint16_t counter{}; + + core::VariableId watched{}; + float threshold{}; + float last_value{}; + bool has_last_value{}; + + TriggerType trigger{TriggerType::IMMEDIATE}; + TraceState current_state{TraceState::IDLE}; + + /** + * @brief Time of the first sample taken after the trigger. + */ + uint32_t trigger_timestamp{}; +}; +} // namespace micras::comm + +#endif // MICRAS_COMM_TRACE_HPP diff --git a/micras_comm/src/frame.cpp b/micras_comm/src/frame.cpp new file mode 100644 index 0000000..3aa04c6 --- /dev/null +++ b/micras_comm/src/frame.cpp @@ -0,0 +1,179 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/comm/frame.hpp" +#include "micras/comm/protocol.hpp" +#include "micras/core/cobs.hpp" +#include "micras/core/crc.hpp" + +namespace micras::comm { +// The buffers here are all fixed size arrays and spans the caller owns, indexed right after the +// check that bounds the index, so the checked accessors would only repeat the comparison. +// NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access,cppcoreguidelines-pro-bounds-constant-array-index) +Writer::Writer(std::span into) : buffer{into} { } + +void Writer::u8(uint8_t value) { + if (this->size >= this->buffer.size()) { + this->overflowed = true; + return; + } + + this->buffer[this->size++] = value; +} + +void Writer::u16(uint16_t value) { + this->u8(value); + this->u8(value >> 8); +} + +void Writer::u32(uint32_t value) { + this->u16(value); + this->u16(value >> 16); +} + +void Writer::raw(std::span bytes) { + if (bytes.size() > this->buffer.size() - this->size) { + this->overflowed = true; + return; + } + + if (not bytes.empty()) { + std::memcpy(&this->buffer[this->size], bytes.data(), bytes.size()); + this->size += bytes.size(); + } +} + +void Writer::text(std::string_view value) { + this->raw({std::bit_cast(value.data()), value.size()}); +} + +std::span Writer::done() const { + return this->overflowed ? std::span{} : this->buffer.first(this->size); +} + +std::size_t Writer::left() const { + return this->overflowed ? 0 : this->buffer.size() - this->size; +} + +Reader::Reader(std::span from) : buffer{from} { } + +uint8_t Reader::u8() { + if (this->index >= this->buffer.size()) { + this->underflowed = true; + return 0; + } + + return this->buffer[this->index++]; +} + +uint16_t Reader::u16() { + const uint16_t low = this->u8(); + return static_cast(low | this->u8() << 8); +} + +uint32_t Reader::u32() { + const uint32_t low = this->u16(); + return low | static_cast(this->u16()) << 16; +} + +std::span Reader::rest() const { + return this->underflowed ? std::span{} : this->buffer.subspan(this->index); +} + +bool Reader::valid() const { + return not this->underflowed; +} + +std::size_t encode_frame(MessageType type, std::span payload, std::span into) { + if (payload.size() > max_payload_size or into.empty()) { + return 0; + } + + std::array plain{}; + plain[0] = std::to_underlying(type); + + if (not payload.empty()) { + std::memcpy(&plain[1], payload.data(), payload.size()); + } + + const std::size_t checked_size = payload.size() + 1; + const uint16_t check = core::crc16(std::span{plain}.first(checked_size)); + + plain[checked_size] = check; + plain[checked_size + 1] = check >> 8; + + const std::size_t size = core::cobs_encode(std::span{plain}.first(checked_size + 2), into.first(into.size() - 1)); + + if (size == 0) { + return 0; + } + + into[size] = core::cobs_delimiter; + return size + 1; +} + +bool FrameReader::push(uint8_t byte) { + if (byte != core::cobs_delimiter) { + if (this->encoded_size >= this->encoded.size()) { + this->overrun = true; + } else { + this->encoded[this->encoded_size++] = byte; + } + + return false; + } + + const bool complete = not this->overrun and this->encoded_size > 0 and this->finish(); + + if (not complete and (this->overrun or this->encoded_size > 0)) { + this->discarded_frames++; + } + + this->encoded_size = 0; + this->overrun = false; + + return complete; +} + +bool FrameReader::finish() { + const std::size_t size = core::cobs_decode(std::span{this->encoded}.first(this->encoded_size), this->decoded); + + if (size < 3) { + return false; + } + + const std::span checked = std::span{this->decoded}.first(size - 2); + const auto expected = static_cast(this->decoded[size - 2] | this->decoded[size - 1] << 8); + + if (core::crc16(checked) != expected) { + return false; + } + + this->decoded_size = size - 2; + return true; +} + +MessageType FrameReader::type() const { + return static_cast(this->decoded[0]); +} + +std::span FrameReader::payload() const { + return std::span{this->decoded}.subspan(1, this->decoded_size - 1); +} + +uint32_t FrameReader::discarded() const { + return this->discarded_frames; +} + +// NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access,cppcoreguidelines-pro-bounds-constant-array-index) +} // namespace micras::comm diff --git a/micras_comm/src/group.cpp b/micras_comm/src/group.cpp new file mode 100644 index 0000000..ae1a366 --- /dev/null +++ b/micras_comm/src/group.cpp @@ -0,0 +1,26 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/comm/group.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::comm { +std::size_t Group::sample(const core::VariablePool& pool, std::span into) const { + if (into.size() < this->sample_size) { + return 0; + } + + std::size_t offset = 0; + + for (uint8_t index = 0; index < this->count; index++) { + offset += pool.read(this->ids.at(index), into.subspan(offset)); + } + + return offset; +} +} // namespace micras::comm diff --git a/micras_comm/src/link.cpp b/micras_comm/src/link.cpp new file mode 100644 index 0000000..87e8814 --- /dev/null +++ b/micras_comm/src/link.cpp @@ -0,0 +1,487 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/comm/frame.hpp" +#include "micras/comm/group.hpp" +#include "micras/comm/link.hpp" +#include "micras/comm/protocol.hpp" +#include "micras/comm/trace.hpp" +#include "micras/core/byte_stream.hpp" +#include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::comm { +Link::Link( + core::IByteStream& stream, core::VariablePool& pool, Trace& trace, ICommandHandler& commands, const Config& config +) : + stream{stream}, pool{pool}, trace{trace}, commands{commands}, config{config} { + this->schema_index = pool.all().size(); +} + +void Link::register_variables(core::VariablePool& pool, std::string_view prefix) { + pool.add(prefix, "dropped_samples", this->dropped_samples, {.stream = true}); + pool.add(prefix, "dropped_logs", this->dropped_logs, {.stream = true}); + pool.add(prefix, "credit", this->credit, {.stream = true}); +} + +void Link::poll(bool robot_is_idle) { + if (this->consumed == this->staged) { + this->staged = this->stream.read(this->staging); + this->consumed = 0; + } + + while (this->consumed < this->staged) { + if (this->reader.push(this->staging.at(this->consumed++))) { + this->execute(robot_is_idle); + return; + } + } +} + +void Link::execute(bool robot_is_idle) { + Reader payload_reader{this->reader.payload()}; + + switch (this->reader.type()) { + case MessageType::HELLO: + this->on_hello(); + return; + + case MessageType::SCHEMA_REQUEST: + this->on_schema_request(payload_reader); + return; + + case MessageType::GROUP_DEFINE: + this->on_group_define(payload_reader); + return; + + case MessageType::GROUP_ENABLE: + this->on_group_enable(payload_reader); + return; + + case MessageType::CREDIT: + this->on_credit(payload_reader); + return; + + case MessageType::WRITE: + this->on_write(payload_reader, robot_is_idle); + return; + + case MessageType::READ: + this->on_read(payload_reader); + return; + + case MessageType::COMMAND: + this->on_command(payload_reader); + return; + + case MessageType::TRACE_ARM: + this->on_trace_arm(payload_reader); + return; + + case MessageType::TRACE_READ: + this->on_trace_read(payload_reader); + return; + + case MessageType::PING: + this->send(MessageType::PONG, {}); + return; + + default: + this->send_error(ErrorCode::UNKNOWN_TYPE, std::to_underlying(this->reader.type())); + return; + } +} + +void Link::on_hello() { + for (Group& group : this->groups) { + group = {}; + } + + this->trace.stop(); + this->trace_dumping = false; + this->schema_index = this->pool.all().size(); + this->credit = initial_credit; + + Writer writer{this->payload}; + writer.u8(protocol_version); + writer.u32(this->pool.schema_hash()); + writer.u16(this->pool.all().size()); + writer.u32(this->config.loop_time_us); + writer.u16(initial_credit); + + this->send(MessageType::HELLO_ACK, writer.done()); +} + +void Link::on_schema_request(Reader& reader) { + const uint16_t first = reader.u16(); + + if (not reader.valid() or first > this->pool.all().size()) { + this->send_error(ErrorCode::MALFORMED, first); + return; + } + + this->schema_index = first; +} + +void Link::on_group_define(Reader& reader) { + const uint8_t index = reader.u8(); + const uint16_t period = reader.u16(); + const uint8_t count = reader.u8(); + + if (not reader.valid() or index >= max_groups) { + this->send_error(ErrorCode::NO_SUCH_GROUP, index); + return; + } + + if (count > max_group_variables) { + this->send_error(ErrorCode::GROUP_TOO_LARGE, count); + return; + } + + Group group{}; + group.count = count; + group.period = std::max(period, 1); + + for (uint8_t position = 0; position < count; position++) { + const core::VariableId id = reader.u16(); + + if (id >= this->pool.all().size()) { + this->send_error(ErrorCode::NO_SUCH_VARIABLE, id); + return; + } + + const core::Variable& variable = this->pool.at(id); + + if (not variable.access.stream) { + this->send_error(ErrorCode::NOT_STREAMABLE, id); + return; + } + + group.ids.at(position) = id; + group.sample_size += variable.size; + } + + if (not reader.valid() or group.sample_size + 7U > max_payload_size) { + this->send_error(ErrorCode::GROUP_TOO_LARGE, group.sample_size); + return; + } + + this->groups.at(index) = group; + + Writer writer{this->payload}; + writer.u8(index); + writer.u16(group.period); + writer.u16(group.sample_size); + + this->send(MessageType::GROUP_ACK, writer.done()); +} + +void Link::on_group_enable(Reader& reader) { + const uint8_t index = reader.u8(); + const bool enable = reader.u8() != 0; + + if (not reader.valid() or index >= max_groups or this->groups.at(index).count == 0) { + this->send_error(ErrorCode::NO_SUCH_GROUP, index); + return; + } + + Group& group = this->groups.at(index); + group.enabled = enable; + group.counter = 0; + + Writer writer{this->payload}; + writer.u8(index); + writer.u16(group.period); + writer.u16(group.sample_size); + + this->send(MessageType::GROUP_ACK, writer.done()); +} + +void Link::on_credit(Reader& reader) { + const uint16_t bytes = reader.u16(); + + if (reader.valid()) { + this->credit = std::min(this->credit + bytes, initial_credit); + } +} + +void Link::on_write(Reader& reader, bool robot_is_idle) { + const core::VariableId id = reader.u16(); + + if (not reader.valid()) { + this->send_error(ErrorCode::MALFORMED, id); + return; + } + + const auto status = this->pool.write(id, reader.rest(), robot_is_idle); + + Writer writer{this->payload}; + writer.u16(id); + writer.u8(std::to_underlying(status)); + + this->send(MessageType::WRITE_ACK, writer.done()); +} + +void Link::on_read(Reader& reader) { + const core::VariableId id = reader.u16(); + + if (not reader.valid() or id >= this->pool.all().size()) { + this->send_error(ErrorCode::NO_SUCH_VARIABLE, id); + return; + } + + Writer writer{this->payload}; + writer.u16(id); + + const core::Variable& variable = this->pool.at(id); + + if (variable.type == core::TypeCode::BLOB) { + const auto data = static_cast(variable.address)->serialize(); + + if (data.size() > writer.left()) { + this->send_error(ErrorCode::GROUP_TOO_LARGE, id); + return; + } + + writer.raw(data); + } else { + writer.raw({std::bit_cast(variable.address), variable.size}); + } + + this->send(MessageType::VALUE, writer.done()); +} + +void Link::on_command(Reader& reader) { + const uint8_t code = reader.u8(); + const uint32_t argument = reader.u32(); + + if (not reader.valid()) { + this->send_error(ErrorCode::MALFORMED, code); + return; + } + + const CommandResult result = this->commands.handle_command(code, argument); + + Writer writer{this->payload}; + writer.u8(code); + writer.u8(std::to_underlying(result)); + + this->send(MessageType::COMMAND_ACK, writer.done()); +} + +void Link::on_trace_arm(Reader& reader) { + const uint8_t index = reader.u8(); + const uint8_t pre_trigger = reader.u8(); + const auto trigger = static_cast(reader.u8()); + const core::VariableId watched = reader.u16(); + const uint32_t raw_threshold = reader.u32(); + + if (not reader.valid() or index >= max_groups) { + this->send_error(ErrorCode::NO_SUCH_GROUP, index); + return; + } + + this->trace_dumping = false; + + if (not this->trace.arm( + this->groups.at(index), pre_trigger, trigger, watched, std::bit_cast(raw_threshold) + )) { + this->send_error(ErrorCode::MALFORMED, index); + return; + } + + this->send_trace_status(); +} + +void Link::on_trace_read(Reader& reader) { + const uint32_t offset = reader.u32(); + + if (not reader.valid()) { + this->send_error(ErrorCode::MALFORMED, 0); + return; + } + + if (this->trace.state() != TraceState::FULL) { + this->send_trace_status(); + return; + } + + this->trace_offset = offset; + this->trace_dumping = true; +} + +void Link::send_trace_status() { + Writer writer{this->payload}; + writer.u8(std::to_underlying(this->trace.state())); + writer.u32(this->trace.held()); + writer.u32(this->trace.pre_trigger()); + writer.u16(this->trace.sample_size()); + writer.u16(this->trace.period()); + writer.u32(this->trace.timestamp()); + + this->send(MessageType::TRACE_STATUS, writer.done()); +} + +void Link::send_error(ErrorCode code, uint16_t context) { + Writer writer{this->payload}; + writer.u8(std::to_underlying(code)); + writer.u16(context); + + this->send(MessageType::ERROR, writer.done()); +} + +void Link::log(Severity severity, std::string_view text) { + Writer writer{this->payload}; + writer.u8(std::to_underlying(severity)); + writer.text(text.substr(0, std::min(text.size(), max_payload_size - 1))); + + if (not this->send(MessageType::LOG, writer.done())) { + this->dropped_logs++; + } +} + +bool Link::send(MessageType type, std::span payload) { + const std::size_t size = encode_frame(type, payload, this->frame); + + if (size == 0 or this->stream.writable() < size) { + return false; + } + + this->stream.write(std::span{this->frame}.first(size)); + return true; +} + +bool Link::send_metered(MessageType type, std::span payload) { + const std::size_t size = encode_frame(type, payload, this->frame); + + if (size == 0 or std::cmp_less(this->credit, size) or this->stream.writable() < size) { + return false; + } + + this->stream.write(std::span{this->frame}.first(size)); + this->credit -= static_cast(size); + return true; +} + +bool Link::send_schema_page() { + const std::span variables = this->pool.all(); + + if (this->schema_index >= variables.size()) { + return false; + } + + constexpr std::size_t count_offset{8}; + + Writer writer{this->payload}; + writer.u32(this->pool.schema_hash()); + writer.u16(this->schema_index); + writer.u16(variables.size()); + writer.u8(0); + + uint16_t index = this->schema_index; + uint8_t count = 0; + + while (index < variables.size() and count < UINT8_MAX) { + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) bounded by the loop + const core::Variable& variable = variables[index]; + const std::size_t name_size = variable.prefix.size() + variable.name.size(); + + if (name_size + 3 > writer.left()) { + break; + } + + writer.u8(std::to_underlying(variable.type)); + writer.u8(std::bit_cast(variable.access)); + writer.u8(name_size); + writer.text(variable.prefix); + writer.text(variable.name); + + index++; + count++; + } + + if (count == 0) { + this->schema_index = variables.size(); + return false; + } + + this->payload.at(count_offset) = count; + + if (not this->send_metered(MessageType::SCHEMA_PAGE, writer.done())) { + return false; + } + + this->schema_index = index; + return true; +} + +bool Link::send_trace_block() { + if (not this->trace_dumping) { + return false; + } + + constexpr std::size_t header_size{4}; + + const std::size_t taken = this->trace.read(this->trace_offset, std::span{this->payload}.subspan(header_size)); + + if (taken == 0) { + this->trace_dumping = false; + return false; + } + + Writer writer{this->payload}; + writer.u32(this->trace_offset); + + if (not this->send_metered(MessageType::TRACE_DATA, std::span{this->payload}.first(header_size + taken))) { + return false; + } + + this->trace_offset += taken; + return true; +} + +void Link::pump(uint32_t timestamp_us) { + for (uint8_t index = 0; index < max_groups; index++) { + Group& group = this->groups.at(index); + + if (not group.enabled) { + continue; + } + + if (group.counter > 0) { + group.counter--; + continue; + } + + group.counter = group.period - 1; + + constexpr std::size_t header_size{7}; + + Writer writer{this->payload}; + writer.u8(index); + writer.u16(group.sequence++); + writer.u32(timestamp_us); + + group.sample(this->pool, std::span{this->payload}.subspan(header_size, group.sample_size)); + + const std::span message = std::span{this->payload}.first(header_size + group.sample_size); + + if (not this->send_metered(MessageType::SAMPLE, message)) { + this->dropped_samples++; + } + } + + if (not this->send_schema_page()) { + this->send_trace_block(); + } +} +} // namespace micras::comm diff --git a/micras_comm/src/trace.cpp b/micras_comm/src/trace.cpp new file mode 100644 index 0000000..fd0b3bc --- /dev/null +++ b/micras_comm/src/trace.cpp @@ -0,0 +1,163 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/comm/group.hpp" +#include "micras/comm/protocol.hpp" +#include "micras/comm/trace.hpp" +#include "micras/core/variable_pool.hpp" + +namespace micras::comm { +Trace::Trace(const core::VariablePool& pool, std::span ring) : pool{pool}, ring{ring} { } + +bool Trace::arm(const Group& group, uint8_t pre_trigger, TriggerType trigger, core::VariableId id, float threshold) { + if (group.count == 0 or group.sample_size == 0 or group.sample_size > this->ring.size()) { + return false; + } + + if (trigger == TriggerType::ABOVE or trigger == TriggerType::BELOW) { + if (id >= this->pool.all().size() or this->pool.at(id).type != core::TypeCode::F32) { + return false; + } + } + + this->group = group; + this->capacity = this->ring.size() / group.sample_size; + this->requested_pre = std::min(this->capacity * std::min(pre_trigger, 100) / 100, this->capacity - 1); + this->trigger = trigger; + this->watched = id; + this->threshold = threshold; + + this->filled = 0; + this->head = 0; + this->kept_pre = 0; + this->remaining = 0; + this->counter = 0; + this->has_last_value = false; + this->current_state = TraceState::ARMED; + + return true; +} + +void Trace::fire() { + if (this->current_state == TraceState::ARMED and this->trigger == TriggerType::COMMAND) { + this->kept_pre = std::min(this->filled, this->requested_pre); + this->remaining = this->capacity - this->kept_pre; + this->current_state = TraceState::TRIGGERED; + } +} + +void Trace::stop() { + this->current_state = TraceState::IDLE; +} + +bool Trace::triggered_by_threshold() { + float value = 0.0F; + std::memcpy(&value, this->pool.at(this->watched).address, sizeof(float)); + + const bool crossed = this->has_last_value and (this->trigger == TriggerType::ABOVE ? + this->last_value <= this->threshold and value > this->threshold : + this->last_value >= this->threshold and value < this->threshold); + + this->last_value = value; + this->has_last_value = true; + + return crossed; +} + +void Trace::store() { + const std::size_t offset = static_cast(this->head) * this->group.sample_size; + this->group.sample(this->pool, this->ring.subspan(offset, this->group.sample_size)); + + this->head = (this->head + 1) % this->capacity; + this->filled = std::min(this->filled + 1, this->capacity); +} + +void Trace::sample(uint32_t timestamp_us) { + if (this->current_state != TraceState::ARMED and this->current_state != TraceState::TRIGGERED) { + return; + } + + if (this->current_state == TraceState::ARMED) { + if (this->trigger == TriggerType::IMMEDIATE or + ((this->trigger == TriggerType::ABOVE or this->trigger == TriggerType::BELOW) and + this->triggered_by_threshold())) { + this->kept_pre = std::min(this->filled, this->requested_pre); + this->remaining = this->capacity - this->kept_pre; + this->current_state = TraceState::TRIGGERED; + } + } + + if (this->counter > 0) { + this->counter--; + return; + } + + this->counter = this->group.period - 1; + + if (this->current_state == TraceState::TRIGGERED and this->remaining == this->capacity - this->kept_pre) { + this->trigger_timestamp = timestamp_us; + } + + this->store(); + + if (this->current_state == TraceState::TRIGGERED and --this->remaining == 0) { + this->current_state = TraceState::FULL; + } +} + +TraceState Trace::state() const { + return this->current_state; +} + +uint32_t Trace::held() const { + return this->current_state == TraceState::FULL ? this->filled : 0; +} + +uint32_t Trace::pre_trigger() const { + return this->kept_pre; +} + +uint16_t Trace::sample_size() const { + return this->group.sample_size; +} + +uint16_t Trace::period() const { + return this->group.period; +} + +uint32_t Trace::timestamp() const { + return this->trigger_timestamp; +} + +std::size_t Trace::read(uint32_t offset, std::span into) const { + const std::size_t usable = static_cast(this->capacity) * this->group.sample_size; + const std::size_t total = static_cast(this->held()) * this->group.sample_size; + const std::size_t from = offset; + + if (from >= total) { + return 0; + } + + const std::size_t oldest = static_cast(this->head) * this->group.sample_size; + const std::size_t size = std::min(into.size(), total - from); + const std::size_t start = (oldest + from) % usable; + const std::size_t first = std::min(size, usable - start); + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) bounded just above + std::memcpy(into.data(), &this->ring[start], first); + + if (first < size) { + std::memcpy(&into[first], this->ring.data(), size - first); + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + + return size; +} +} // namespace micras::comm From 026e7e408d57f7f5753ace4f6553fb63b5c166bd Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 21 Sep 2026 21:12:30 -0300 Subject: [PATCH 479/561] =?UTF-8?q?=F0=9F=94=8C=20Reach=20the=20radio=20mo?= =?UTF-8?q?dule=20without=20spending=20the=20control=20loop?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Reception is a circular DMA that is never stopped, polled once per iteration by comparing the transfer counter against what was already taken. Nothing happens in an interrupt, there is no window between stopping and restarting in which bytes are lost, and the latency is a hundred and twenty five microseconds, which is far below anything the radio adds. Transmission is a linear DMA out of a ring that takes whole frames or none of them, so a frame is never cut in half by a full buffer. The RX stream is switched to circular in the CubeMX project rather than in the generated code, so regenerating it keeps the change. --- micras_hal/include/micras/hal/uart_dma.hpp | 129 ++++++++++++++++++ micras_hal/stm32/src/uart_dma.cpp | 76 +++++++++++ .../include/micras/proxy/bluetooth_serial.hpp | 106 ++++++++++++++ micras_proxy/src/bluetooth_serial.cpp | 71 ++++++++++ 4 files changed, 382 insertions(+) create mode 100644 micras_hal/include/micras/hal/uart_dma.hpp create mode 100644 micras_hal/stm32/src/uart_dma.cpp create mode 100644 micras_proxy/include/micras/proxy/bluetooth_serial.hpp create mode 100644 micras_proxy/src/bluetooth_serial.cpp diff --git a/micras_hal/include/micras/hal/uart_dma.hpp b/micras_hal/include/micras/hal/uart_dma.hpp new file mode 100644 index 0000000..1d73937 --- /dev/null +++ b/micras_hal/include/micras/hal/uart_dma.hpp @@ -0,0 +1,129 @@ +/** + * @file + */ + +#ifndef MICRAS_HAL_UART_DMA_HPP +#define MICRAS_HAL_UART_DMA_HPP + +#include +#include +#include +#include + +namespace micras::hal { +/** + * @brief Class to handle a UART peripheral on STM32 microcontrollers using DMA. + * + * @note Reception is a circular DMA that is never stopped, and is polled by comparing the transfer + * counter against what was already taken. Nothing is done in an interrupt, there is no window + * between stopping and restarting in which bytes are lost, and the latency is one control loop + * iteration, which is far below anything the radio adds. + */ +class UartDma { +public: + /** + * @brief Configuration struct for the UART. + */ + struct Config { + void (*init_function)(); + UART_HandleTypeDef* handle; + }; + + /** + * @brief Construct a new UartDma object. + * + * @param config UART configuration struct. + */ + explicit UartDma(const Config& config); + + /** + * @brief Special member functions deleted. + * + * @note The object owns a transfer in flight that points into its own buffers, so it cannot be + * copied or moved. + */ + ///@{ + UartDma(const UartDma&) = delete; + UartDma(UartDma&&) = delete; + UartDma& operator=(const UartDma&) = delete; + UartDma& operator=(UartDma&&) = delete; + ~UartDma() = default; + ///@} + + /** + * @brief Start receiving into a circular buffer. + * + * @note The buffer is borrowed and has to outlive the reception. It should live in a memory + * the DMA controller can reach without going through the bus matrix of another domain. + * + * @param buffer Buffer the DMA writes into, wrapping around. + * @return True if the reception was started, false otherwise. + */ + bool start_rx(std::span buffer); + + /** + * @brief Take the bytes that arrived since the last call. + * + * @note Bytes are lost silently if this is called more rarely than the buffer takes to fill, + * which the framing recovers from by resynchronizing on the next delimiter. + * + * @param into Buffer to copy the received bytes into. + * @return Number of bytes copied. + */ + std::size_t read(std::span into); + + /** + * @brief Start sending a buffer. + * + * @note The buffer is borrowed and has to stay valid until the transfer completes. + * + * @param from Data to send. + * @return True if the transfer was started, false if one is still running or it failed. + */ + bool start_tx(std::span from); + + /** + * @brief Check if a transfer is still running. + * + * @return True if the peripheral is still sending, false otherwise. + */ + bool is_transmitting() const; + + /** + * @brief Check if the peripheral was initialized and is receiving. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + /** + * @brief Get how many bytes the DMA has written into the buffer so far. + * + * @return Index the DMA will write the next byte at. + */ + std::size_t rx_head() const; + + /** + * @brief UART handle. + */ + UART_HandleTypeDef* handle; + + /** + * @brief Destination buffer of the reception, written around by the DMA. + */ + std::span rx_buffer; + + /** + * @brief Index of the first byte that has not been taken yet. + */ + std::size_t rx_tail{}; + + /** + * @brief Whether the reception is running. + */ + bool initialized{}; +}; +} // namespace micras::hal + +#endif // MICRAS_HAL_UART_DMA_HPP diff --git a/micras_hal/stm32/src/uart_dma.cpp b/micras_hal/stm32/src/uart_dma.cpp new file mode 100644 index 0000000..853cf94 --- /dev/null +++ b/micras_hal/stm32/src/uart_dma.cpp @@ -0,0 +1,76 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include "micras/hal/uart_dma.hpp" + +namespace micras::hal { +UartDma::UartDma(const Config& config) : handle{config.handle} { + if (this->handle->gState == HAL_UART_STATE_RESET) { + config.init_function(); + } +} + +bool UartDma::start_rx(std::span buffer) { + this->rx_buffer = buffer; + this->rx_tail = 0; + this->initialized = HAL_UART_Receive_DMA(this->handle, buffer.data(), buffer.size()) == HAL_OK; + + return this->initialized; +} + +std::size_t UartDma::rx_head() const { + return this->rx_buffer.size() - __HAL_DMA_GET_COUNTER(this->handle->hdmarx); +} + +std::size_t UartDma::read(std::span into) { + if (this->rx_buffer.empty()) { + return 0; + } + + if (this->handle->RxState != HAL_UART_STATE_BUSY_RX) { + this->start_rx(this->rx_buffer); + return 0; + } + + const std::size_t size = this->rx_buffer.size(); + const std::size_t available = (this->rx_head() + size - this->rx_tail) % size; + const std::size_t taken = std::min(available, into.size()); + const std::size_t first = std::min(taken, size - this->rx_tail); + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) bounded just above + std::memcpy(into.data(), &this->rx_buffer[this->rx_tail], first); + + if (first < taken) { + std::memcpy(&into[first], this->rx_buffer.data(), taken - first); + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + + this->rx_tail = (this->rx_tail + taken) % size; + + return taken; +} + +bool UartDma::start_tx(std::span from) { + if (from.empty() or this->is_transmitting()) { + return false; + } + + return HAL_UART_Transmit_DMA(this->handle, std::bit_cast(from.data()), from.size()) == HAL_OK; +} + +bool UartDma::is_transmitting() const { + return this->handle->gState == HAL_UART_STATE_BUSY_TX; +} + +bool UartDma::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_proxy/include/micras/proxy/bluetooth_serial.hpp b/micras_proxy/include/micras/proxy/bluetooth_serial.hpp new file mode 100644 index 0000000..6da544d --- /dev/null +++ b/micras_proxy/include/micras/proxy/bluetooth_serial.hpp @@ -0,0 +1,106 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_BLUETOOTH_SERIAL_HPP +#define MICRAS_PROXY_BLUETOOTH_SERIAL_HPP + +#include +#include +#include + +#include "micras/core/byte_stream.hpp" +#include "micras/hal/uart_dma.hpp" + +namespace micras::proxy { +/** + * @brief Byte stream over the UART the radio module is wired to. + * + * @note The module has no flow control on this board and drops silently when its buffer fills, so + * the ring here only bounds what the firmware can hand to the peripheral. What bounds what the + * module can take is the credit window in the session layer. + */ +class BluetoothSerial : public core::IByteStream { +public: + /** + * @brief Configuration struct for the bluetooth serial. + */ + struct Config { + void (*init_function)(); + UART_HandleTypeDef* handle; + }; + + /** + * @brief Construct a new BluetoothSerial object. + * + * @note The buffers are borrowed and have to outlive the object, and both are written by the + * DMA controller rather than by the processor. + * + * @param config Configuration for the bluetooth serial. + * @param rx_buffer Buffer the reception wraps around, which bounds how long the link can go + * unpolled before bytes are lost. + * @param tx_buffer Buffer the frames waiting to be sent are queued in. + */ + BluetoothSerial(const Config& config, std::span rx_buffer, std::span tx_buffer); + + /** + * @brief Hand the peripheral whatever is queued. + */ + void update(); + + /** + * @brief Take the bytes that arrived since the last call. + * + * @param into Buffer to read into. + * @return Number of bytes read. + */ + std::size_t read(std::span into) override; + + /** + * @brief Queue data to be sent. + * + * @note Either all of the data is queued or none of it is, so a frame is never cut in half. + * + * @param from Data to send. + * @return Number of bytes queued. + */ + std::size_t write(std::span from) override; + + /** + * @brief Get how much can be queued right now. + * + * @return Number of bytes that fit in the ring. + */ + std::size_t writable() const override; + + /** + * @brief Check if the peripheral was initialized and is receiving. + * + * @return True if the initialization was successful, false otherwise. + */ + bool was_initialized() const; + +private: + hal::UartDma uart; + + /** + * @brief Bytes queued to be sent, written at the head and sent from the tail. + */ + std::span tx_buffer; + + std::size_t head{}; + std::size_t tail{}; + + /** + * @brief Number of bytes in the ring, including the ones already handed to the DMA. + */ + std::size_t stored{}; + + /** + * @brief Number of bytes the current transfer is sending, counted from the tail. + */ + std::size_t pending{}; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_BLUETOOTH_SERIAL_HPP diff --git a/micras_proxy/src/bluetooth_serial.cpp b/micras_proxy/src/bluetooth_serial.cpp new file mode 100644 index 0000000..e5f5ac5 --- /dev/null +++ b/micras_proxy/src/bluetooth_serial.cpp @@ -0,0 +1,71 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/proxy/bluetooth_serial.hpp" + +namespace micras::proxy { +BluetoothSerial::BluetoothSerial(const Config& config, std::span rx_buffer, std::span tx_buffer) : + uart{{.init_function = config.init_function, .handle = config.handle}}, tx_buffer{tx_buffer} { + this->uart.start_rx(rx_buffer); +} + +void BluetoothSerial::update() { + if (this->uart.is_transmitting()) { + return; + } + + this->tail = (this->tail + this->pending) % this->tx_buffer.size(); + this->stored -= this->pending; + this->pending = 0; + + if (this->stored == 0) { + return; + } + + const std::size_t run = std::min(this->stored, this->tx_buffer.size() - this->tail); + + if (this->uart.start_tx(this->tx_buffer.subspan(this->tail, run))) { + this->pending = run; + } +} + +std::size_t BluetoothSerial::read(std::span into) { + return this->uart.read(into); +} + +std::size_t BluetoothSerial::write(std::span from) { + if (from.size() > this->writable()) { + return 0; + } + + const std::size_t first = std::min(from.size(), this->tx_buffer.size() - this->head); + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) bounded just above + std::memcpy(&this->tx_buffer[this->head], from.data(), first); + + if (first < from.size()) { + std::memcpy(this->tx_buffer.data(), &from[first], from.size() - first); + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + + this->head = (this->head + from.size()) % this->tx_buffer.size(); + this->stored += from.size(); + + return from.size(); +} + +std::size_t BluetoothSerial::writable() const { + return this->tx_buffer.size() - this->stored; +} + +bool BluetoothSerial::was_initialized() const { + return this->uart.was_initialized(); +} +} // namespace micras::proxy From 17a804eacd108787123bca2b8bb03e37f5e1b072 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 21 Sep 2026 21:13:28 -0300 Subject: [PATCH 480/561] =?UTF-8?q?=E2=9C=A8=20Put=20the=20link=20in=20the?= =?UTF-8?q?=20control=20loop?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Draining the receiver, taking a sample and building a frame come after the state machine, and cost under a microsecond of the hundred and twenty five the loop has. At most one message is acted on per iteration, so a burst of requests cannot take the budget, and one message every hundred and twenty five microseconds is faster than the link can deliver them anyway. The options of a run become one writable byte. The switches wrote four flags, of which three were sent as events nothing ever consumed, and the link would have had to fight them for the fourth. Now the switches write the byte when they move and the link writes it whenever it likes: last writer wins, which is a rule that can be stated in one sentence and predicted from outside. Commands reach the same event queue as a button press, so nothing has to hold a flag that something else remembers to clear. The ones that stop the robot for seconds, like saving to flash, are refused while it is moving, as are writes to anything that would change the geometry under way. The capture and the frames are in ordinary SRAM rather than RAM_D2. The linker section CubeMX generates for that region is not NOLOAD, so anything placed there puts a segment at 0x30000000 in the programmed image; the DMA reaches SRAM through the bus matrix and the data cache is off, so there is nothing to gain by fighting it. --- micras_core/include/micras/core/fsm.hpp | 7 +++++++ micras_core/include/micras/core/impl/fsm.tpp | 5 +++++ 2 files changed, 12 insertions(+) diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 5434d64..239e7ea 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -99,6 +99,13 @@ class TFsm { */ void update(); + /** + * @brief Get the id of the state currently running. + * + * @return The id of the current state. + */ + uint8_t get_current_state_id() const; + private: /** * @brief States of the machine, indexed by their id. diff --git a/micras_core/include/micras/core/impl/fsm.tpp b/micras_core/include/micras/core/impl/fsm.tpp index 3f9ebe0..323c267 100644 --- a/micras_core/include/micras/core/impl/fsm.tpp +++ b/micras_core/include/micras/core/impl/fsm.tpp @@ -19,6 +19,11 @@ void TFsm::add_state(std::unique_ptr state) { this->states.at(state->get_id()) = std::move(state); } +template +uint8_t TFsm::get_current_state_id() const { + return this->current_state_id; +} + template void TFsm::update() { if (this->current_state_id >= num_of_states or this->states.at(this->current_state_id) == nullptr) { From 1235787c99ecebeee03e2aa22271f3c23c96a6ea Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 21 Sep 2026 21:13:35 -0300 Subject: [PATCH 481/561] =?UTF-8?q?=E2=9C=85=20Run=20the=20parts=20with=20?= =?UTF-8?q?no=20hardware=20behind=20them=20on=20the=20host?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The variable pool, the flash image, the framing and the whole session layer need nothing but a compiler, and every writer and reader disagreement this project has had was invisible until something was on the wire. They build with the host compiler and run in the terminal: cmake -S tests/host -B build/host && cmake --build build/host && ctest --test-dir build/host The checks worth naming: both CRCs are pinned to their published check values, COBS is checked against the canonical vectors and two hundred thousand random round trips, the flash stands in for a buffer that can be told to stop accepting writes so that an interrupted save can be tested at all, and the session is driven end to end over a loopback, including a closed credit window and a capture read back out. The frame vectors are the same table that micras-monitor holds, so the two implementations cannot drift apart without a test saying so. --- tests/CMakeLists.txt | 64 ++++++ tests/stub/micras/hal/flash.hpp | 67 ++++++ tests/test_cobs.cpp | 133 ++++++++++++ tests/test_crc.cpp | 23 +++ tests/test_frame.cpp | 121 +++++++++++ tests/test_host.hpp | 25 +++ tests/test_link.cpp | 349 ++++++++++++++++++++++++++++++++ tests/test_storage.cpp | 114 +++++++++++ tests/test_variable_pool.cpp | 85 ++++++++ 9 files changed, 981 insertions(+) create mode 100644 tests/CMakeLists.txt create mode 100644 tests/stub/micras/hal/flash.hpp create mode 100644 tests/test_cobs.cpp create mode 100644 tests/test_crc.cpp create mode 100644 tests/test_frame.cpp create mode 100644 tests/test_host.hpp create mode 100644 tests/test_link.cpp create mode 100644 tests/test_storage.cpp create mode 100644 tests/test_variable_pool.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 0000000..bbbd7d9 --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,64 @@ +# Tests for the parts that have no hardware behind them: the variable pool, the flash image, the +# framing and the whole session layer. They build with the host compiler and run in the terminal, +# which is what makes them worth writing at all, and is configured separately from the firmware +# because it is a different toolchain: +# +# cmake -S tests/host -B build/host && cmake --build build/host && ctest --test-dir build/host + +cmake_minimum_required(VERSION 3.25...4.4) + +project(micras_host_tests CXX) + +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(MICRAS_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/../..) + +enable_testing() + +add_library(micras_host_core STATIC + ${MICRAS_ROOT}/micras_core/src/cobs.cpp + ${MICRAS_ROOT}/micras_core/src/crc.cpp + ${MICRAS_ROOT}/micras_core/src/variable_pool.cpp +) + +target_include_directories(micras_host_core PUBLIC ${MICRAS_ROOT}/micras_core/include) + +add_library(micras_host_comm STATIC + ${MICRAS_ROOT}/micras_comm/src/frame.cpp + ${MICRAS_ROOT}/micras_comm/src/group.cpp + ${MICRAS_ROOT}/micras_comm/src/link.cpp + ${MICRAS_ROOT}/micras_comm/src/trace.cpp +) + +target_include_directories(micras_host_comm PUBLIC ${MICRAS_ROOT}/micras_comm/include) +target_link_libraries(micras_host_comm PUBLIC micras_host_core) + +# The flash of the storage is stood in for by a buffer that can be told to stop accepting writes, +# which is the only way to test what an interrupted save leaves behind +add_library(micras_host_proxy STATIC + ${MICRAS_ROOT}/micras_proxy/src/storage.cpp +) + +target_include_directories(micras_host_proxy PUBLIC + ${MICRAS_ROOT}/micras_proxy/include + ${CMAKE_CURRENT_SOURCE_DIR}/stub +) + +target_link_libraries(micras_host_proxy PUBLIC micras_host_core) + +foreach(TEST_NAME variable_pool crc cobs frame storage link) + add_executable(test_${TEST_NAME} test_${TEST_NAME}.cpp) + target_include_directories(test_${TEST_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + target_compile_options(test_${TEST_NAME} PRIVATE -Wall -Wextra -fsanitize=address,undefined) + target_link_options(test_${TEST_NAME} PRIVATE -fsanitize=address,undefined) + add_test(NAME ${TEST_NAME} COMMAND test_${TEST_NAME}) +endforeach() + +target_link_libraries(test_variable_pool PRIVATE micras_host_core) +target_link_libraries(test_crc PRIVATE micras_host_core) +target_link_libraries(test_cobs PRIVATE micras_host_core) +target_link_libraries(test_frame PRIVATE micras_host_comm) +target_link_libraries(test_storage PRIVATE micras_host_proxy) +target_link_libraries(test_link PRIVATE micras_host_comm) diff --git a/tests/stub/micras/hal/flash.hpp b/tests/stub/micras/hal/flash.hpp new file mode 100644 index 0000000..ab27e0a --- /dev/null +++ b/tests/stub/micras/hal/flash.hpp @@ -0,0 +1,67 @@ +#ifndef MICRAS_HAL_FLASH_HPP +#define MICRAS_HAL_FLASH_HPP +#include +#include +#include +#include +#include + +namespace micras::hal { +class FlashWord { +public: + static constexpr uint32_t words{8}; + static constexpr uint32_t size{words * sizeof(uint32_t)}; + static constexpr uint8_t erased_value{0xFF}; +}; + +class Flash { +public: + enum class Status : uint8_t { + OK = 0, + MISALIGNED = 1, + OUT_OF_BOUNDS = 2, + ERROR = 3 + }; + static constexpr uint32_t sector_size{128 * 1024}; + static constexpr uint16_t total_sectors{2}; + static constexpr uint32_t total_size{total_sectors * sector_size}; + + static inline std::vector memory = std::vector(total_size, 0xFF); + static inline uint32_t write_budget = UINT32_MAX; // flash words allowed before a simulated power loss + + static std::span read(uint16_t sector, uint32_t sector_address, uint32_t size) { + uint64_t address = uint64_t(sector) * sector_size + sector_address; + if (address + size > total_size) + return {}; + return std::span{memory.data() + address, size}; + } + + static Status write(uint16_t sector, uint32_t sector_address, std::span data) { + if (sector_address % FlashWord::size != 0) + return Status::MISALIGNED; + uint64_t address = uint64_t(sector) * sector_size + sector_address; + uint32_t padded = (data.size() + FlashWord::size - 1) / FlashWord::size * FlashWord::size; + if (address + padded > total_size) + return Status::OUT_OF_BOUNDS; + for (uint32_t i = 0; i < padded; i += FlashWord::size) { + if (write_budget == 0) + return Status::ERROR; + write_budget--; + for (uint32_t j = 0; j < FlashWord::size; j++) { + uint32_t k = i + j; + memory[address + k] = k < data.size() ? data[k] : FlashWord::erased_value; + } + } + return Status::OK; + } + + static Status erase_sectors(uint16_t start_sector, uint16_t number_of_sectors = 1) { + uint64_t address = uint64_t(start_sector) * sector_size; + if (address + uint64_t(number_of_sectors) * sector_size > total_size) + return Status::OUT_OF_BOUNDS; + std::memset(memory.data() + address, 0xFF, size_t(number_of_sectors) * sector_size); + return Status::OK; + } +}; +} // namespace micras::hal +#endif diff --git a/tests/test_cobs.cpp b/tests/test_cobs.cpp new file mode 100644 index 0000000..3b46dc7 --- /dev/null +++ b/tests/test_cobs.cpp @@ -0,0 +1,133 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/core/cobs.hpp" +#include "test_host.hpp" + +using namespace micras::core; +using V = std::vector; + +static V enc(const V& in) { + V out(cobs_encoded_size(in.size())); + out.resize(cobs_encode(in, out)); + return out; +} + +static V dec(const V& in) { + V out(in.size() + 1); + out.resize(cobs_decode(in, out)); + return out; +} + +static void vec(const V& raw, const V& expected) { + V got = enc(raw); + if (got != expected) { + std::printf("encode mismatch for %zu bytes: got %zu expected %zu\n", raw.size(), got.size(), expected.size()); + + for (auto b : got) { + std::printf("%02X ", b); + } + + std::puts(""); + + for (auto b : expected) { + std::printf("%02X ", b); + } + + std::puts(""); + CHECK(false); + } + CHECK(dec(got) == raw); +} + +static V range(int a, int b) { + V v; + for (int i = a; i <= b; i++) + v.push_back(uint8_t(i)); + return v; +} + +int main() { + vec({}, {0x01}); + vec({0x00}, {0x01, 0x01}); + vec({0x00, 0x00}, {0x01, 0x01, 0x01}); + vec({0x00, 0x11, 0x00}, {0x01, 0x02, 0x11, 0x01}); + vec({0x11, 0x22, 0x00, 0x33}, {0x03, 0x11, 0x22, 0x02, 0x33}); + vec({0x11, 0x22, 0x33, 0x44}, {0x05, 0x11, 0x22, 0x33, 0x44}); + vec({0x11, 0x00, 0x00, 0x00}, {0x02, 0x11, 0x01, 0x01, 0x01}); + + { + V r = range(1, 254); + V e{0xFF}; + for (auto b : r) + e.push_back(b); + vec(r, e); + } + { + V r = range(0, 254); + V e{0x01, 0xFF}; + for (int i = 1; i <= 254; i++) + e.push_back(i); + vec(r, e); + } + { + V r = range(1, 255); + V e{0xFF}; + for (int i = 1; i <= 254; i++) + e.push_back(i); + e.push_back(0x02); + e.push_back(0xFF); + vec(r, e); + } + { + V r = range(2, 255); + r.push_back(0x00); + V e{0xFF}; + for (int i = 2; i <= 255; i++) + e.push_back(i); + e.push_back(0x01); + e.push_back(0x01); + vec(r, e); + } + { + V r = range(3, 255); + r.push_back(0x00); + r.push_back(0x01); + V e{0xFE}; + for (int i = 3; i <= 255; i++) + e.push_back(i); + e.push_back(0x02); + e.push_back(0x01); + vec(r, e); + } + + // every encoded frame is free of the delimiter, and round trips + std::mt19937 rng{7}; + for (int trial = 0; trial < 200000; trial++) { + V raw(rng() % 600); + for (auto& b : raw) + b = uint8_t(rng() % (trial % 3 == 0 ? 3 : 256)); + V e = enc(raw); + CHECK(e.size() <= cobs_encoded_size(raw.size())); + for (auto b : e) + CHECK(b != 0); + CHECK(dec(e) == raw); + } + + // malformed frames are rejected rather than half decoded + V scratch(8); + CHECK(cobs_decode(V{0x00}, scratch) == 0); + CHECK(cobs_decode(V{0x05, 0x01}, scratch) == 0); + { + V out(2); + CHECK(cobs_decode(V{0x06, 1, 2, 3, 4, 5}, out) == 0); + } + + std::puts("cobs ok"); +} diff --git a/tests/test_crc.cpp b/tests/test_crc.cpp new file mode 100644 index 0000000..238d485 --- /dev/null +++ b/tests/test_crc.cpp @@ -0,0 +1,23 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/core/crc.hpp" +#include "test_host.hpp" + +using namespace micras::core; + +int main() { + std::string_view s = "123456789"; + std::span b{std::bit_cast(s.data()), s.size()}; + printf("crc16=%04X crc32=%08X\n", crc16(b), crc32(b)); + CHECK(crc16(b) == 0x29B1); + CHECK(crc32(b) == 0x0376E6E7); + // incremental must equal one shot + CHECK(crc16(b.subspan(4), crc16(b.first(4))) == crc16(b)); + puts("crc ok"); +} diff --git a/tests/test_frame.cpp b/tests/test_frame.cpp new file mode 100644 index 0000000..049f090 --- /dev/null +++ b/tests/test_frame.cpp @@ -0,0 +1,121 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/comm/frame.hpp" +#include "micras/comm/protocol.hpp" +#include "test_host.hpp" + +using namespace micras::comm; + +namespace { +struct Vector { + std::string_view name; + MessageType type; + std::vector payload; + std::vector frame; +}; + +// The same table lives in micras-monitor, as src/lib/comm/__tests__/frameVectors.ts. Every writer +// and reader disagreement this project has had was invisible until something was on the wire, so +// the two implementations are pinned to the same bytes rather than to each other's good intentions. +const std::vector vectors{ + {"hello", MessageType{0x01}, {}, {4, 1, 209, 241, 0}}, + {"hello_ack", + MessageType{0x81}, + {1, 133, 114, 115, 247, 4, 0, 125, 0, 0, 0, 0, 1}, + {8, 129, 1, 133, 114, 115, 247, 4, 2, 125, 1, 1, 1, 4, 1, 40, 160, 0}}, + {"schema_request", MessageType{0x02}, {0, 0}, {2, 2, 1, 3, 252, 162, 0}}, + {"group_define", MessageType{0x03}, {0, 80, 0, 2, 0, 0, 1, 0}, {2, 3, 2, 80, 2, 2, 1, 2, 1, 3, 19, 168, 0}}, + {"credit", MessageType{0x05}, {0, 1}, {2, 5, 4, 1, 77, 55, 0}}, + {"write", MessageType{0x06}, {2, 0, 0, 0, 128, 63}, {3, 6, 2, 1, 1, 5, 128, 63, 143, 7, 0}}, + {"zeros", MessageType{0x85}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {2, 133, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 3, 142, 100, 0}}, + {"ramp", + MessageType{0x8B}, + {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, + 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, + 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, + 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, + 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, + 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, + 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, + 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, + 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199}, + {2, 139, 202, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, + 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, + 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, + 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, + 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, + 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, + 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, + 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, + 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 115, 21, 0}}, + {"high", + MessageType{0x89}, + {200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, + 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239}, + {44, 137, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, + 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 42, 80, 0}}, +}; +} // namespace + +int main() { + for (const Vector& vector : vectors) { + std::vector built(max_frame_size); + built.resize(encode_frame(vector.type, vector.payload, built)); + + if (built != vector.frame) { + std::printf("%.*s: built", int(vector.name.size()), vector.name.data()); + + for (uint8_t byte : built) { + std::printf(" %02X", byte); + } + + std::puts(""); + CHECK(false); + } + + FrameReader reader; + bool complete = false; + + for (uint8_t byte : vector.frame) { + complete = reader.push(byte); + } + + CHECK(complete); + CHECK(reader.type() == vector.type); + CHECK(std::vector(reader.payload().begin(), reader.payload().end()) == vector.payload); + CHECK(reader.discarded() == 0); + } + + // A frame that lost a byte is discarded on its own, and the next one still arrives + { + const std::vector& good = vectors.at(3).frame; + FrameReader reader; + bool complete = false; + + for (std::size_t i = 0; i + 3 < good.size(); i++) { + CHECK(not reader.push(good[i])); + } + + CHECK(not reader.push(0x00)); + CHECK(reader.discarded() == 1); + + for (uint8_t byte : good) { + complete = reader.push(byte); + } + + CHECK(complete); + CHECK(reader.discarded() == 1); + } + + std::printf("frame ok: %zu vectors\n", vectors.size()); +} diff --git a/tests/test_host.hpp b/tests/test_host.hpp new file mode 100644 index 0000000..8f28101 --- /dev/null +++ b/tests/test_host.hpp @@ -0,0 +1,25 @@ +/** + * @file + */ + +#ifndef MICRAS_TEST_HOST_HPP +#define MICRAS_TEST_HOST_HPP + +#include +#include + +/** + * @brief Fail the test with the condition and where it was written. + * + * @note Spelled out instead of using assert so that it holds whatever the build turns NDEBUG into. + */ +// NOLINTNEXTLINE(cppcoreguidelines-macro-usage) it has to capture the text and the line +#define CHECK(condition) \ + do { \ + if (not(condition)) { \ + std::fprintf(stderr, "%s:%d: %s\n", __FILE__, __LINE__, #condition); \ + std::abort(); \ + } \ + } while (false) + +#endif // MICRAS_TEST_HOST_HPP diff --git a/tests/test_link.cpp b/tests/test_link.cpp new file mode 100644 index 0000000..44c79d4 --- /dev/null +++ b/tests/test_link.cpp @@ -0,0 +1,349 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/comm/frame.hpp" +#include "micras/comm/link.hpp" +#include "micras/comm/trace.hpp" +#include "micras/core/byte_stream.hpp" +#include "micras/core/variable_pool.hpp" +#include "test_host.hpp" + +using namespace micras; +using namespace micras::comm; +using V = std::vector; + +struct Loopback : core::IByteStream { + std::deque to_robot; + std::deque from_robot; + std::size_t capacity{4096}; + + std::size_t read(std::span into) override { + std::size_t n = std::min(into.size(), to_robot.size()); + for (std::size_t i = 0; i < n; i++) { + into[i] = to_robot.front(); + to_robot.pop_front(); + } + return n; + } + + std::size_t write(std::span from) override { + if (writable() < from.size()) + return 0; + for (uint8_t b : from) + from_robot.push_back(b); + return from.size(); + } + + std::size_t writable() const override { return capacity - from_robot.size(); } +}; + +struct Commands : ICommandHandler { + std::vector> seen; + + CommandResult handle_command(uint8_t code, uint32_t argument) override { + seen.emplace_back(code, argument); + return code == 42 ? CommandResult::OK : CommandResult::UNKNOWN; + } +}; + +// --- host side of the protocol --- +static void send(Loopback& io, MessageType type, const V& payload) { + V frame(max_frame_size); + frame.resize(encode_frame(type, payload, frame)); + CHECK(!frame.empty()); + for (uint8_t b : frame) + io.to_robot.push_back(b); +} + +struct Msg { + MessageType type; + V payload; +}; + +// Stands in for the application: takes everything that arrived and returns the credit for it, +// which is what keeps the window open. +static std::vector drain(Loopback& io) { + static FrameReader rx; + std::vector out; + std::size_t taken = io.from_robot.size(); + while (!io.from_robot.empty()) { + uint8_t b = io.from_robot.front(); + io.from_robot.pop_front(); + if (rx.push(b)) + out.push_back({rx.type(), V(rx.payload().begin(), rx.payload().end())}); + } + if (taken > 0) + send(io, MessageType::CREDIT, {uint8_t(taken), uint8_t(taken >> 8)}); + return out; +} + +static const Msg& only(const std::vector& msgs, MessageType type) { + const Msg* found = nullptr; + for (const Msg& m : msgs) { + if (m.type == type) { + CHECK(found == nullptr); + found = &m; + } + } + CHECK(found != nullptr); + return *found; +} + +static uint16_t u16(const V& v, size_t i) { + return uint16_t(v[i] | v[i + 1] << 8); +} + +static uint32_t u32(const V& v, size_t i) { + return uint32_t(v[i] | v[i + 1] << 8 | v[i + 2] << 16 | uint32_t(v[i + 3]) << 24); +} + +static float f32(const V& v, size_t i) { + uint32_t r = u32(v, i); + float f; + std::memcpy(&f, &r, 4); + return f; +} + +int main() { + core::TVariablePool<32> pool; + float linear = 1.0F, angular = 2.0F, gain = 0.5F; + uint8_t profile = 0; + pool.add("cmd/", "linear", linear, {.stream = true}); + pool.add("cmd/", "angular", angular, {.stream = true}); + pool.add("model/", "gain", gain, {.stream = true, .write = true, .idle = true, .persist = true}); + pool.add("", "run_profile", profile, {.stream = true, .write = true}); + + std::vector ring(1024); + Trace trace{pool, ring}; + Commands commands; + Loopback io; + Link link{io, pool, trace, commands, {.loop_time_us = 125}}; + + auto step = [&](uint32_t t, bool idle = true) { + link.poll(idle); + link.pump(t); + trace.sample(t); + }; + auto settle = [&](uint32_t& t, int n = 40, bool idle = true) { + for (int i = 0; i < n; i++) + step(t += 125, idle); + }; + + uint32_t now = 0; + + // --- hello --- + send(io, MessageType::HELLO, {}); + settle(now, 4); + auto msgs = drain(io); + CHECK(msgs.size() == 1 && msgs[0].type == MessageType::HELLO_ACK); + CHECK(msgs[0].payload[0] == protocol_version); + const uint32_t hash = u32(msgs[0].payload, 1); + CHECK(hash == pool.schema_hash()); + CHECK(u16(msgs[0].payload, 5) == 4); + CHECK(u32(msgs[0].payload, 7) == 125); + + // --- schema, paged --- + send(io, MessageType::SCHEMA_REQUEST, {0, 0}); + settle(now, 40); + msgs = drain(io); + std::map names; + for (auto& m : msgs) { + CHECK(m.type == MessageType::SCHEMA_PAGE); + CHECK(u32(m.payload, 0) == hash); + uint16_t first = u16(m.payload, 4), total = u16(m.payload, 6); + CHECK(total == 4); + uint8_t count = m.payload[8]; + size_t i = 9; + for (uint8_t k = 0; k < count; k++) { + uint8_t len = m.payload[i + 2]; + names[first + k] = std::string(m.payload.begin() + i + 3, m.payload.begin() + i + 3 + len); + i += 3 + len; + } + } + CHECK(names.size() == 4); + CHECK(names[0] == "cmd/linear" && names[2] == "model/gain" && names[3] == "run_profile"); + + // --- a group streams at its period, coherently --- + send(io, MessageType::GROUP_DEFINE, {0, 4, 0, 2, 0, 0, 1, 0}); // group 0, period 4, ids {0,1} + send(io, MessageType::GROUP_ENABLE, {0, 1}); + settle(now, 4); + msgs = drain(io); + CHECK(msgs.size() >= 2 && msgs[0].type == MessageType::GROUP_ACK && msgs[1].type == MessageType::GROUP_ACK); + CHECK(u16(msgs[0].payload, 1) == 4 && u16(msgs[0].payload, 3) == 8); + + linear = 3.5F; + angular = -1.25F; + drain(io); + settle(now, 16); + msgs = drain(io); + CHECK(msgs.size() == 4); // 16 iterations at period 4 + + for (size_t k = 0; k < msgs.size(); k++) { + CHECK(msgs[k].type == MessageType::SAMPLE); + CHECK(msgs[k].payload[0] == 0); + CHECK(u16(msgs[k].payload, 1) == u16(msgs[0].payload, 1) + k); + CHECK(f32(msgs[k].payload, 7) == 3.5F); + CHECK(f32(msgs[k].payload, 11) == -1.25F); + } + + // --- writes, with the idle guard --- + send(io, MessageType::WRITE, {2, 0, 0, 0, 0x80, 0x3F}); // model/gain = 1.0 + settle(now, 4, false); + msgs = drain(io); + CHECK(only(msgs, MessageType::WRITE_ACK).payload[2] == uint8_t(core::VariablePool::WriteStatus::NEEDS_IDLE)); + CHECK(gain == 0.5F); + + send(io, MessageType::WRITE, {2, 0, 0, 0, 0x80, 0x3F}); + settle(now, 4, true); + msgs = drain(io); + CHECK(only(msgs, MessageType::WRITE_ACK).payload[2] == uint8_t(core::VariablePool::WriteStatus::OK)); + CHECK(gain == 1.0F); + + // --- read --- + send(io, MessageType::READ, {2, 0}); + settle(now, 4); + msgs = drain(io); + CHECK(f32(only(msgs, MessageType::VALUE).payload, 2) == 1.0F); + + // --- commands are edges, acted on once --- + send(io, MessageType::COMMAND, {42, 7, 0, 0, 0}); + settle(now, 8); + msgs = drain(io); + CHECK(commands.seen.size() == 1); + CHECK(commands.seen[0].first == 42 && commands.seen[0].second == 7); + CHECK(only(msgs, MessageType::COMMAND_ACK).payload[1] == uint8_t(CommandResult::OK)); + + // --- a closed credit window drops samples and the sequence shows the gap --- + drain(io); + send(io, MessageType::HELLO, {}); // resets credit to the initial window + settle(now, 4); + drain(io); + send(io, MessageType::GROUP_DEFINE, {0, 1, 0, 2, 0, 0, 1, 0}); + send(io, MessageType::GROUP_ENABLE, {0, 1}); + settle(now, 4); + drain(io); + { + core::TVariablePool<8> probe; + link.register_variables(probe, "link/"); + + uint8_t baseline_buf[4]; + uint32_t baseline = 0; + probe.read(*probe.find("link/dropped_samples"), baseline_buf); + std::memcpy(&baseline, baseline_buf, 4); + + settle(now, 400); // far more than the window allows + msgs = drain(io); + msgs.erase( + std::remove_if(msgs.begin(), msgs.end(), [](const Msg& m) { return m.type != MessageType::SAMPLE; }), + msgs.end() + ); + CHECK(!msgs.empty() && msgs.size() < 40); // the window, not the loop, set the rate + + uint8_t buf[4]; + uint32_t dropped = 0; + probe.read(*probe.find("link/dropped_samples"), buf); + std::memcpy(&dropped, buf, 4); + CHECK(dropped - baseline + msgs.size() == 400); // every attempt is either sent or counted + + const uint16_t last_sent = u16(msgs.back().payload, 1); + + // returning credit starts it again, and the sequence shows what was missed + send(io, MessageType::CREDIT, {0x00, 0x01}); + settle(now, 8); + msgs = drain(io); + msgs.erase( + std::remove_if(msgs.begin(), msgs.end(), [](const Msg& m) { return m.type != MessageType::SAMPLE; }), + msgs.end() + ); + CHECK(!msgs.empty()); + CHECK(u16(msgs.front().payload, 1) > last_sent + 1); + } + + send(io, MessageType::GROUP_ENABLE, {0, 0}); + settle(now, 8); + drain(io); + + // --- a corrupted frame costs only itself --- + drain(io); + const std::size_t corrupt_at = io.to_robot.size() + 2; + send(io, MessageType::READ, {0, 0}); + io.to_robot[corrupt_at] ^= 0xFF; // flip a byte inside the first frame + send(io, MessageType::READ, {1, 0}); + settle(now, 8); + msgs = drain(io); + auto values = std::count_if(msgs.begin(), msgs.end(), [](const Msg& m) { return m.type == MessageType::VALUE; }); + CHECK(values == 1); + for (auto& m : msgs) + if (m.type == MessageType::VALUE) + CHECK(u16(m.payload, 0) == 1); + + // --- trace: full rate capture with pre-trigger, read out afterwards --- + drain(io); + send(io, MessageType::GROUP_DEFINE, {1, 1, 0, 1, 0, 0}); // group 1, period 1, just cmd/linear + settle(now, 4); + drain(io); + + // trigger when cmd/linear rises through 10 + V arm{1, 50, uint8_t(TriggerType::ABOVE), 0, 0, 0, 0, 0x20, 0x41}; // threshold 10.0F + linear = 0.0F; + send(io, MessageType::TRACE_ARM, arm); + settle(now, 4); + msgs = drain(io); + CHECK(only(msgs, MessageType::TRACE_STATUS).payload[0] == uint8_t(TraceState::ARMED)); + + const std::size_t capacity = ring.size() / 4; + const std::size_t pre = capacity / 2; + + for (std::size_t i = 0; i < capacity; i++) { + linear = -float(i); + step(now += 125); + } + CHECK(trace.state() == TraceState::ARMED); // nothing crossed the threshold yet + + for (std::size_t i = 0; i < pre; i++) { + linear = 50.0F + float(i); + step(now += 125); + } + CHECK(trace.state() == TraceState::FULL); + CHECK(trace.held() == capacity); + CHECK(trace.pre_trigger() == pre); + CHECK(trace.sample_size() == 4); + CHECK(trace.period() == 1); + + drain(io); + send(io, MessageType::TRACE_READ, {0, 0, 0, 0}); + + // the readout runs at whatever rate the application returns credit at, which is the point + V dump; + for (int round = 0; round < 200 && dump.size() < capacity * 4; round++) { + settle(now, 8); + msgs = drain(io); + + for (auto& m : msgs) { + if (m.type != MessageType::TRACE_DATA) + continue; + CHECK(u32(m.payload, 0) == dump.size()); + dump.insert(dump.end(), m.payload.begin() + 4, m.payload.end()); + } + } + CHECK(dump.size() == capacity * 4); + + // the oldest half is what preceded the trigger, the newest half what followed it + for (std::size_t i = 0; i < pre; i++) { + CHECK(f32(dump, i * 4) == -float(pre + i)); + } + for (std::size_t i = 0; i < capacity - pre; i++) { + CHECK(f32(dump, (pre + i) * 4) == 50.0F + float(i)); + } + + std::puts("link ok"); +} diff --git a/tests/test_storage.cpp b/tests/test_storage.cpp new file mode 100644 index 0000000..0ef4ff6 --- /dev/null +++ b/tests/test_storage.cpp @@ -0,0 +1,114 @@ +/** + * @file + */ + +#include + +#include "micras/hal/flash.hpp" +#include "micras/proxy/storage.hpp" +#include "test_host.hpp" + +using namespace micras; + +struct Blob : core::ISerializable { + std::vector data; + + std::vector serialize() const override { return data; } + + void deserialize(const uint8_t* p, uint16_t n) override { data.assign(p, p + n); } +}; + +static const proxy::Storage::Config cfg{.start_sector = 0, .number_of_sectors = 1}; + +int main() { + // --- round trip --- + { + core::TVariablePool<8> pool; + float kv = 3.25F; + uint8_t profile = 7; + bool flag = true; + float transient = 1.0F; + Blob maze; + maze.data = {9, 8, 7, 6, 5}; + pool.add("model/", "kv", kv, {.persist = true}); + pool.add("", "run_profile", profile, {.persist = true}); + pool.add("", "flag", flag, {.persist = true}); + pool.add("", "transient", transient, {.stream = true}); + pool.add("", "maze", maze, {.persist = true}); + + proxy::Storage s{cfg}; + CHECK(!s.is_valid()); + CHECK(s.restore(pool) == 0); + CHECK(s.save(pool)); + CHECK(s.is_valid()); + } + { + core::TVariablePool<8> pool; + float kv = 0.0F; + uint8_t profile = 0; + bool flag = false; + float transient = 42.0F; + Blob maze; + pool.add("model/", "kv", kv, {.persist = true}); + pool.add("", "run_profile", profile, {.persist = true}); + pool.add("", "flag", flag, {.persist = true}); + pool.add("", "transient", transient, {.stream = true}); + pool.add("", "maze", maze, {.persist = true}); + + proxy::Storage s{cfg}; + CHECK(s.is_valid()); + CHECK(s.restore(pool) == 4); + CHECK(kv == 3.25F && profile == 7 && flag == true); + CHECK((maze.data == std::vector{9, 8, 7, 6, 5})); + CHECK(transient == 42.0F); + } + // --- a renamed / reordered pool still finds what it knows --- + { + core::TVariablePool<8> pool; + uint8_t profile = 0; + float kv = 0.0F; + float fresh = 1.0F; + pool.add("", "fresh", fresh, {.persist = true}); + pool.add("", "run_profile", profile, {.persist = true}); + pool.add("model/", "kv", kv, {.persist = true}); + proxy::Storage s{cfg}; + CHECK(s.restore(pool) == 2); + CHECK(profile == 7 && kv == 3.25F && fresh == 1.0F); + } + // --- a type change is refused, not misread --- + { + core::TVariablePool<8> pool; + uint32_t kv = 0; + pool.add("model/", "kv", kv, {.persist = true}); + proxy::Storage s{cfg}; + CHECK(s.restore(pool) == 0); + CHECK(kv == 0); + } + // --- a torn write leaves the previous image, not a half one --- + { + core::TVariablePool<8> pool; + float kv = 99.0F; + pool.add("model/", "kv", kv, {.persist = true}); + proxy::Storage s{cfg}; + hal::Flash::write_budget = 0; + CHECK(!s.save(pool)); + hal::Flash::write_budget = UINT32_MAX; + proxy::Storage after{cfg}; + CHECK(!after.is_valid()); + CHECK(after.restore(pool) == 0); + } + // --- writing the body but dying before the header is the same as never saving --- + { + hal::Flash::erase_sectors(0, 1); + core::TVariablePool<8> pool; + float kv = 1.0F; + pool.add("model/", "kv", kv, {.persist = true}); + proxy::Storage s{cfg}; + hal::Flash::write_budget = 1; // one flash word: the body goes in, the header does not + CHECK(!s.save(pool)); + hal::Flash::write_budget = UINT32_MAX; + proxy::Storage after{cfg}; + CHECK(!after.is_valid()); + } + std::puts("storage ok"); +} diff --git a/tests/test_variable_pool.cpp b/tests/test_variable_pool.cpp new file mode 100644 index 0000000..5fffac1 --- /dev/null +++ b/tests/test_variable_pool.cpp @@ -0,0 +1,85 @@ +/** + * @file + */ + +#include +#include + +#include "micras/core/variable_pool.hpp" +#include "test_host.hpp" + +using namespace micras::core; + +struct Blob : ISerializable { + std::vector data{1, 2, 3}; + + std::vector serialize() const override { return data; } + + void deserialize(const uint8_t* p, uint16_t n) override { data.assign(p, p + n); } +}; + +enum class Profile : uint8_t { + A, + B +}; + +int main() { + TVariablePool<8> pool; + float speed = 1.5F; + const float k = 2.0F; + Profile profile = Profile::A; + Blob blob; + + auto id_speed = pool.add("speed/", "linear", speed, {.stream = true, .write = true}); + auto id_k = pool.add("model/", "kv", k, {.stream = true, .write = true, .idle = true, .persist = true}); + auto id_prof = pool.add("", "run_profile", profile, {.write = true, .idle = true, .persist = true}); + auto id_blob = pool.add("", "maze", blob, {.persist = true}); + + CHECK(id_speed == 0 && id_k == 1 && id_prof == 2 && id_blob == 3); + CHECK(pool.all().size() == 4); + CHECK(pool.at(id_k).access.write == false); + CHECK(pool.at(id_prof).type == TypeCode::U8); + CHECK(pool.at(id_blob).type == TypeCode::BLOB); + CHECK(pool.at(id_blob).access.stream == false); + + CHECK(pool.find("speed/linear").value() == 0); + CHECK(pool.find("model/kv").value() == 1); + CHECK(pool.find("run_profile").value() == 2); + CHECK(!pool.find("speed/linea").has_value()); + CHECK(!pool.find("speed/linearr").has_value()); + + uint8_t buf[8]; + CHECK(pool.read(id_speed, buf) == 4); + float out; + std::memcpy(&out, buf, 4); + CHECK(out == 1.5F); + CHECK(pool.read(id_blob, buf) == 0); + + float nv = 9.0F; + CHECK(pool.write(id_speed, {reinterpret_cast(&nv), 4}, false) == VariablePool::WriteStatus::OK); + CHECK(speed == 9.0F); + CHECK(pool.write(id_k, {reinterpret_cast(&nv), 4}, true) == VariablePool::WriteStatus::READ_ONLY); + uint8_t one = 1; + CHECK(pool.write(id_prof, {&one, 1}, false) == VariablePool::WriteStatus::NEEDS_IDLE); + CHECK(pool.write(id_prof, {&one, 1}, true) == VariablePool::WriteStatus::OK); + CHECK(profile == Profile::B); + CHECK(pool.write(id_speed, {&one, 1}, true) == VariablePool::WriteStatus::WRONG_SIZE); + CHECK(pool.write(99, {&one, 1}, true) == VariablePool::WriteStatus::NO_SUCH_ID); + + uint32_t h0 = pool.schema_hash(); + TVariablePool<8> other; + other.add("speed/", "linear", speed, {.stream = true, .write = true}); + other.add("model/", "kv", k, {.stream = true, .write = true, .idle = true, .persist = true}); + other.add("", "run_profile", profile, {.write = true, .idle = true, .persist = true}); + other.add("", "maze", blob, {.persist = true}); + CHECK(other.schema_hash() == h0); + other.add("", "extra", speed, {}); + CHECK(other.schema_hash() != h0); + + TVariablePool<1> tiny; + tiny.add("", "a", speed, {}); + CHECK(tiny.add("", "b", speed, {}) == VariablePool::invalid_id); + CHECK(tiny.all().size() == 1); + + std::printf("pool ok, sizeof(Variable)=%zu hash=%08X\n", sizeof(Variable), h0); +} From b7da80f673da49a8f9271a5f522db324b5611a04 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 22 Sep 2026 02:34:20 -0300 Subject: [PATCH 482/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Leave=20the=20choi?= =?UTF-8?q?ce=20of=20what=20to=20publish=20to=20whoever=20is=20publishing?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The sensors and the filter had gained a method that registered their readings into a variable pool. That put a telemetry mechanism inside a device wrapper: anyone using these as a library would have been compiling the pool whether or not they wanted a link, and a class whose job is to talk to a part would have been deciding what is worth watching. They now know nothing about it. The application copies what it publishes into a struct of its own once per iteration and registers that, which is also the only thing that could have worked for the values that are computed on the way out rather than stored: the yaw rate has its calibration subtracted, the battery is scaled into volts, and neither has an address to point at. --- .../include/micras/core/butterworth_filter.hpp | 15 --------------- micras_core/src/butterworth_filter.cpp | 6 ------ .../include/micras/proxy/impl/wall_sensors.tpp | 13 ------------- micras_proxy/include/micras/proxy/imu.hpp | 10 ---------- .../include/micras/proxy/wall_sensors.hpp | 10 ---------- micras_proxy/src/imu.cpp | 10 ---------- 6 files changed, 64 deletions(-) diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp index 17b36be..56add89 100644 --- a/micras_core/include/micras/core/butterworth_filter.hpp +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -8,9 +8,6 @@ #include #include #include -#include - -#include "micras/core/variable_pool.hpp" namespace micras::core { /** @@ -119,18 +116,6 @@ class ButterworthFilter { */ float get_last() const; - /** - * @brief Expose the filtered value for monitoring. - * - * @note The value is registered where it already lives, so sampling it costs a copy of four - * bytes and the filter gains nothing to keep up to date. - * - * @param pool Pool to register into. - * @param prefix Prefix of the name, identifying the owner of the filter. - * @param name Name of the filtered value inside its owner. - */ - void register_variables(VariablePool& pool, std::string_view prefix, std::string_view name); - private: /** * @brief Last two input values of the filter, most recent first. diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index 2d015f9..ba6b03d 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -4,10 +4,8 @@ #include #include -#include #include "micras/core/butterworth_filter.hpp" -#include "micras/core/variable_pool.hpp" namespace micras::core { ButterworthFilter::Coefficients ButterworthFilter::compute_coefficients(const Config& config) { @@ -50,8 +48,4 @@ float ButterworthFilter::update(float x0) { float ButterworthFilter::get_last() const { return this->output; } - -void ButterworthFilter::register_variables(VariablePool& pool, std::string_view prefix, std::string_view name) { - pool.add(prefix, name, this->output, {.stream = true}); -} } // namespace micras::core diff --git a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp index 849f435..6ba12e8 100644 --- a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp +++ b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp @@ -6,15 +6,12 @@ #define MICRAS_PROXY_WALL_SENSORS_TPP #include -#include #include #include #include -#include #include "micras/core/butterworth_filter.hpp" #include "micras/core/utils.hpp" -#include "micras/core/variable_pool.hpp" #include "micras/hal/pwm.hpp" namespace micras::proxy { @@ -99,16 +96,6 @@ void TWallSensors::calibrate_sensor(uint8_t sensor_index) { this->base_readings.at(sensor_index) = this->get_reading(sensor_index); } -template -void TWallSensors::register_variables(core::VariablePool& pool, std::string_view prefix) { - static constexpr std::array names{"0", "1", "2", "3", "4", "5", "6", "7"}; - static_assert(num_of_sensors <= names.size(), "Every sensor needs a name literal to register under"); - - for (uint8_t i = 0; i < num_of_sensors; i++) { - this->filters.at(i).register_variables(pool, prefix, names.at(i)); - } -} - template bool TWallSensors::was_initialized() const { return this->initialized; diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index e85fc23..85cf376 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -9,10 +9,8 @@ #include #include #include -#include #include "micras/core/butterworth_filter.hpp" -#include "micras/core/variable_pool.hpp" #include "micras/hal/spi.hpp" namespace micras::proxy { @@ -110,14 +108,6 @@ class Imu { * * @return True if the device was successfully initialized, false otherwise. */ - /** - * @brief Register the angular velocity and the linear acceleration for monitoring. - * - * @param pool Pool to register into. - * @param prefix Prefix of the names, identifying this sensor. - */ - void register_variables(core::VariablePool& pool, std::string_view prefix); - bool was_initialized() const; private: diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index cbd9989..52e2425 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -7,10 +7,8 @@ #include #include -#include #include "micras/core/butterworth_filter.hpp" -#include "micras/core/variable_pool.hpp" #include "micras/hal/adc_dma.hpp" #include "micras/hal/pwm.hpp" @@ -111,14 +109,6 @@ class TWallSensors { * * @return True if the initialization was successful, false otherwise. */ - /** - * @brief Register the readings of the sensors for monitoring. - * - * @param pool Pool to register into. - * @param prefix Prefix of the names, identifying this set of sensors. - */ - void register_variables(core::VariablePool& pool, std::string_view prefix); - bool was_initialized() const; private: diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 0e12a77..357198a 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -6,10 +6,8 @@ #include #include #include -#include #include "lsm6dsv_reg.h" -#include "micras/core/variable_pool.hpp" #include "micras/proxy/imu.hpp" #include "micras/proxy/stopwatch.hpp" @@ -188,12 +186,4 @@ bool Imu::was_initialized() const { return this->initialized; } -void Imu::register_variables(core::VariablePool& pool, std::string_view prefix) { - pool.add(prefix, "gyro_x", this->angular_velocity.at(0), {.stream = true}); - pool.add(prefix, "gyro_y", this->angular_velocity.at(1), {.stream = true}); - pool.add(prefix, "gyro_z", this->angular_velocity.at(2), {.stream = true}); - pool.add(prefix, "accel_x", this->linear_acceleration.at(0), {.stream = true}); - pool.add(prefix, "accel_y", this->linear_acceleration.at(1), {.stream = true}); - pool.add(prefix, "accel_z", this->linear_acceleration.at(2), {.stream = true}); -} } // namespace micras::proxy From 61ddf5876357694312a7ff8cff0b75530319365b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Tue, 22 Sep 2026 02:34:20 -0300 Subject: [PATCH 483/561] =?UTF-8?q?=F0=9F=94=A5=20Drop=20what=20the=20rest?= =?UTF-8?q?=20of=20it=20already=20answers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A second CRC existed for one caller. The schema hash only has to change when the schema does; nothing is being corrected and the schema it stands for arrives over a link that checks its own frames, so it is four lines of FNV-1a and the CRC-32 is gone from both sides. `writable()` asked whether a frame would fit, and then `write()` answered the same question by taking all of the frame or none of it. The interface a transport has to implement is one method shorter. An error code nothing ever sent, and a translation unit holding a single function with no state. --- micras_comm/include/micras/comm/group.hpp | 14 +++++++- micras_comm/include/micras/comm/protocol.hpp | 3 +- micras_comm/src/group.cpp | 26 -------------- micras_comm/src/link.cpp | 14 ++++---- .../include/micras/core/byte_stream.hpp | 10 ++---- micras_core/include/micras/core/crc.hpp | 11 ------ .../include/micras/core/variable_pool.hpp | 4 +++ micras_core/src/crc.cpp | 14 -------- micras_core/src/variable_pool.cpp | 25 +++++++------ .../include/micras/proxy/bluetooth_serial.hpp | 8 +---- micras_proxy/src/bluetooth_serial.cpp | 6 +--- tests/CMakeLists.txt | 1 - tests/test_crc.cpp | 35 ++++++++++++++----- tests/test_link.cpp | 4 +-- 14 files changed, 72 insertions(+), 103 deletions(-) delete mode 100644 micras_comm/src/group.cpp diff --git a/micras_comm/include/micras/comm/group.hpp b/micras_comm/include/micras/comm/group.hpp index 9a9ff77..147736f 100644 --- a/micras_comm/include/micras/comm/group.hpp +++ b/micras_comm/include/micras/comm/group.hpp @@ -64,7 +64,19 @@ struct Group { * @param into Buffer to copy into. * @return Number of bytes copied, zero if the buffer is too small. */ - std::size_t sample(const core::VariablePool& pool, std::span into) const; + std::size_t sample(const core::VariablePool& pool, std::span into) const { + if (into.size() < this->sample_size) { + return 0; + } + + std::size_t offset = 0; + + for (uint8_t index = 0; index < this->count; index++) { + offset += pool.read(this->ids.at(index), into.subspan(offset)); + } + + return offset; + } }; } // namespace micras::comm diff --git a/micras_comm/include/micras/comm/protocol.hpp b/micras_comm/include/micras/comm/protocol.hpp index 4588130..7b22a0c 100644 --- a/micras_comm/include/micras/comm/protocol.hpp +++ b/micras_comm/include/micras/comm/protocol.hpp @@ -89,8 +89,7 @@ enum class ErrorCode : uint8_t { NO_SUCH_GROUP = 2, GROUP_TOO_LARGE = 3, NOT_STREAMABLE = 4, - NO_SUCH_VARIABLE = 5, - BUSY = 6 + NO_SUCH_VARIABLE = 5 }; /** diff --git a/micras_comm/src/group.cpp b/micras_comm/src/group.cpp deleted file mode 100644 index ae1a366..0000000 --- a/micras_comm/src/group.cpp +++ /dev/null @@ -1,26 +0,0 @@ -/** - * @file - */ - -#include -#include -#include - -#include "micras/comm/group.hpp" -#include "micras/core/variable_pool.hpp" - -namespace micras::comm { -std::size_t Group::sample(const core::VariablePool& pool, std::span into) const { - if (into.size() < this->sample_size) { - return 0; - } - - std::size_t offset = 0; - - for (uint8_t index = 0; index < this->count; index++) { - offset += pool.read(this->ids.at(index), into.subspan(offset)); - } - - return offset; -} -} // namespace micras::comm diff --git a/micras_comm/src/link.cpp b/micras_comm/src/link.cpp index 87e8814..83f350b 100644 --- a/micras_comm/src/link.cpp +++ b/micras_comm/src/link.cpp @@ -352,22 +352,20 @@ void Link::log(Severity severity, std::string_view text) { bool Link::send(MessageType type, std::span payload) { const std::size_t size = encode_frame(type, payload, this->frame); - if (size == 0 or this->stream.writable() < size) { - return false; - } - - this->stream.write(std::span{this->frame}.first(size)); - return true; + return size > 0 and this->stream.write(std::span{this->frame}.first(size)) == size; } bool Link::send_metered(MessageType type, std::span payload) { const std::size_t size = encode_frame(type, payload, this->frame); - if (size == 0 or std::cmp_less(this->credit, size) or this->stream.writable() < size) { + if (size == 0 or std::cmp_less(this->credit, size)) { + return false; + } + + if (this->stream.write(std::span{this->frame}.first(size)) != size) { return false; } - this->stream.write(std::span{this->frame}.first(size)); this->credit -= static_cast(size); return true; } diff --git a/micras_core/include/micras/core/byte_stream.hpp b/micras_core/include/micras/core/byte_stream.hpp index bbab2e5..035ffc7 100644 --- a/micras_core/include/micras/core/byte_stream.hpp +++ b/micras_core/include/micras/core/byte_stream.hpp @@ -35,18 +35,14 @@ class IByteStream { /** * @brief Queue data to be sent. * + * @note All of the data is taken or none of it is, so a frame is never cut in half and asking + * beforehand whether it would fit would only be the same question twice. + * * @param from Data to send. * @return Number of bytes accepted, which is either all of them or none. */ virtual std::size_t write(std::span from) = 0; - /** - * @brief Get how much can be written before the stream refuses data. - * - * @return Number of bytes that can be written right now. - */ - virtual std::size_t writable() const = 0; - protected: /** * @brief Special member functions declared as default. diff --git a/micras_core/include/micras/core/crc.hpp b/micras_core/include/micras/core/crc.hpp index c2dbcfb..e552750 100644 --- a/micras_core/include/micras/core/crc.hpp +++ b/micras_core/include/micras/core/crc.hpp @@ -22,17 +22,6 @@ namespace micras::core { * @return CRC of the buffer. */ uint16_t crc16(std::span data, uint16_t seed = 0xFFFF); - -/** - * @brief Compute the CRC-32 of a buffer. - * - * @note Computed bit by bit, with no lookup table, because the only caller runs once at startup. - * - * @param data Buffer to compute the CRC of. - * @param seed Initial value of the register, so that a CRC can be computed in several calls. - * @return CRC of the buffer. - */ -uint32_t crc32(std::span data, uint32_t seed = 0xFFFFFFFF); } // namespace micras::core #endif // MICRAS_CORE_CRC_HPP diff --git a/micras_core/include/micras/core/variable_pool.hpp b/micras_core/include/micras/core/variable_pool.hpp index 8d629b2..ae8d966 100644 --- a/micras_core/include/micras/core/variable_pool.hpp +++ b/micras_core/include/micras/core/variable_pool.hpp @@ -302,6 +302,10 @@ class VariablePool { * application holding a cached schema would then plot the wrong signal with nothing to warn it. * Comparing this hash against the one the schema was fetched with is that warning. * + * @note Only has to change whenever the schema does, so it is an FNV-1a hash rather than + * anything with error detection properties. Nothing is being corrected here, and the schema it + * stands for arrives over a link that checks its own frames. + * * @return Hash over the name, type and access flags of every variable, in order. */ uint32_t schema_hash() const; diff --git a/micras_core/src/crc.cpp b/micras_core/src/crc.cpp index f8301ec..332b7c3 100644 --- a/micras_core/src/crc.cpp +++ b/micras_core/src/crc.cpp @@ -24,18 +24,4 @@ uint16_t crc16(std::span data, uint16_t seed) { return crc; } - -uint32_t crc32(std::span data, uint32_t seed) { - uint32_t crc = seed; - - for (const uint8_t byte : data) { - crc ^= static_cast(byte) << 24; - - for (uint8_t bit = 0; bit < 8; bit++) { - crc = (crc & 0x80000000) != 0 ? (crc << 1) ^ 0x04C11DB7 : crc << 1; - } - } - - return crc; -} } // namespace micras::core diff --git a/micras_core/src/variable_pool.cpp b/micras_core/src/variable_pool.cpp index e77a43b..092859f 100644 --- a/micras_core/src/variable_pool.cpp +++ b/micras_core/src/variable_pool.cpp @@ -2,7 +2,6 @@ * @file */ -#include #include #include #include @@ -11,13 +10,20 @@ #include #include -#include "micras/core/crc.hpp" #include "micras/core/serializable.hpp" #include "micras/core/variable_pool.hpp" namespace micras::core { -static std::span as_bytes(std::string_view text) { - return {std::bit_cast(text.data()), text.size()}; +static uint32_t mix(uint32_t hash, uint8_t byte) { + return (hash ^ byte) * 16777619U; +} + +static uint32_t mix(uint32_t hash, std::string_view text) { + for (const char character : text) { + hash = mix(hash, static_cast(character)); + } + + return hash; } VariablePool::VariablePool(std::span storage) : storage{storage} { } @@ -119,14 +125,13 @@ VariablePool::WriteStatus VariablePool::write(VariableId id, std::spanall()) { - const std::array tail{std::to_underlying(variable.type), std::bit_cast(variable.access)}; - - hash = crc32(as_bytes(variable.prefix), hash); - hash = crc32(as_bytes(variable.name), hash); - hash = crc32(tail, hash); + hash = mix(hash, variable.prefix); + hash = mix(hash, variable.name); + hash = mix(hash, std::to_underlying(variable.type)); + hash = mix(hash, std::bit_cast(variable.access)); } return hash; diff --git a/micras_proxy/include/micras/proxy/bluetooth_serial.hpp b/micras_proxy/include/micras/proxy/bluetooth_serial.hpp index 6da544d..ad885e8 100644 --- a/micras_proxy/include/micras/proxy/bluetooth_serial.hpp +++ b/micras_proxy/include/micras/proxy/bluetooth_serial.hpp @@ -60,19 +60,13 @@ class BluetoothSerial : public core::IByteStream { * @brief Queue data to be sent. * * @note Either all of the data is queued or none of it is, so a frame is never cut in half. + * A frame that does not fit is refused, and the session drops it and counts it. * * @param from Data to send. * @return Number of bytes queued. */ std::size_t write(std::span from) override; - /** - * @brief Get how much can be queued right now. - * - * @return Number of bytes that fit in the ring. - */ - std::size_t writable() const override; - /** * @brief Check if the peripheral was initialized and is receiving. * diff --git a/micras_proxy/src/bluetooth_serial.cpp b/micras_proxy/src/bluetooth_serial.cpp index e5f5ac5..2eadf3e 100644 --- a/micras_proxy/src/bluetooth_serial.cpp +++ b/micras_proxy/src/bluetooth_serial.cpp @@ -41,7 +41,7 @@ std::size_t BluetoothSerial::read(std::span into) { } std::size_t BluetoothSerial::write(std::span from) { - if (from.size() > this->writable()) { + if (from.size() > this->tx_buffer.size() - this->stored) { return 0; } @@ -61,10 +61,6 @@ std::size_t BluetoothSerial::write(std::span from) { return from.size(); } -std::size_t BluetoothSerial::writable() const { - return this->tx_buffer.size() - this->stored; -} - bool BluetoothSerial::was_initialized() const { return this->uart.was_initialized(); } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index bbbd7d9..b596e57 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -27,7 +27,6 @@ target_include_directories(micras_host_core PUBLIC ${MICRAS_ROOT}/micras_core/in add_library(micras_host_comm STATIC ${MICRAS_ROOT}/micras_comm/src/frame.cpp - ${MICRAS_ROOT}/micras_comm/src/group.cpp ${MICRAS_ROOT}/micras_comm/src/link.cpp ${MICRAS_ROOT}/micras_comm/src/trace.cpp ) diff --git a/tests/test_crc.cpp b/tests/test_crc.cpp index 238d485..66f4ebe 100644 --- a/tests/test_crc.cpp +++ b/tests/test_crc.cpp @@ -2,8 +2,13 @@ * @file */ +#include +#include #include +#include +#include #include +#include #include #include "micras/core/crc.hpp" @@ -12,12 +17,26 @@ using namespace micras::core; int main() { - std::string_view s = "123456789"; - std::span b{std::bit_cast(s.data()), s.size()}; - printf("crc16=%04X crc32=%08X\n", crc16(b), crc32(b)); - CHECK(crc16(b) == 0x29B1); - CHECK(crc32(b) == 0x0376E6E7); - // incremental must equal one shot - CHECK(crc16(b.subspan(4), crc16(b.first(4))) == crc16(b)); - puts("crc ok"); + const std::string_view text{"123456789"}; + const std::span bytes{std::bit_cast(text.data()), text.size()}; + + // The published check value of CRC-16/CCITT-FALSE, so this is pinned to the standard rather + // than to whatever the other implementation of it happens to agree with + CHECK(crc16(bytes) == 0x29B1); + + // Computing it in two calls has to give the same answer as computing it in one + CHECK(crc16(bytes.subspan(4), crc16(bytes.first(4))) == crc16(bytes)); + + // A single flipped bit anywhere has to change it + for (std::size_t index = 0; index < text.size(); index++) { + for (uint8_t bit = 0; bit < 8; bit++) { + std::array flipped{}; + std::ranges::copy(bytes, flipped.begin()); + flipped.at(index) ^= static_cast(1U << bit); + + CHECK(crc16(flipped) != crc16(bytes)); + } + } + + std::puts("crc ok"); } diff --git a/tests/test_link.cpp b/tests/test_link.cpp index 44c79d4..7c8a73e 100644 --- a/tests/test_link.cpp +++ b/tests/test_link.cpp @@ -36,14 +36,12 @@ struct Loopback : core::IByteStream { } std::size_t write(std::span from) override { - if (writable() < from.size()) + if (capacity - from_robot.size() < from.size()) return 0; for (uint8_t b : from) from_robot.push_back(b); return from.size(); } - - std::size_t writable() const override { return capacity - from_robot.size(); } }; struct Commands : ICommandHandler { From 81a54e38f11fd550ddacc675f3773ab26217807a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 23 Sep 2026 00:03:51 -0300 Subject: [PATCH 484/561] =?UTF-8?q?=F0=9F=94=A5=20Take=20the=20scope=20out?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The trace recorded a group into a 32 KiB ring at loop rate and read it back out afterwards, so that the 8 kHz loop could be seen over a link that carries a hundredth of it. It cost a capture state machine with four states and four trigger kinds, four message types, a second reader of the pool that walked it at loop rate, a block-by-block readout metered through the credit window, and an application that never grew a button for any of it. What is left answers the question the link was built for: a group is defined with a period in loop iterations and streams at a rate the link can actually deliver. The 32 KiB the ring reserved is back, and a sample is now packed straight into the outgoing payload instead of through a shared helper that existed so the capture could reuse it. `Group` was its own header for the same reason and is now a detail of the session. Message types are contiguous again, and `send_schema_page` returns nothing, because nothing was choosing between a schema page and a capture block any more. --- micras_comm/README.md | 6 +- micras_comm/include/micras/comm/group.hpp | 83 -------- micras_comm/include/micras/comm/link.hpp | 81 ++++---- micras_comm/include/micras/comm/protocol.hpp | 30 +-- micras_comm/include/micras/comm/trace.hpp | 190 ------------------ micras_comm/src/link.cpp | 123 ++---------- micras_comm/src/trace.cpp | 163 --------------- .../include/micras/core/variable_pool.hpp | 2 +- tests/CMakeLists.txt | 1 - tests/test_link.cpp | 69 +------ 10 files changed, 69 insertions(+), 679 deletions(-) delete mode 100644 micras_comm/include/micras/comm/group.hpp delete mode 100644 micras_comm/include/micras/comm/trace.hpp delete mode 100644 micras_comm/src/trace.cpp diff --git a/micras_comm/README.md b/micras_comm/README.md index 6c9e719..773632b 100644 --- a/micras_comm/README.md +++ b/micras_comm/README.md @@ -79,6 +79,6 @@ what both the microcontroller and `DataView` in the browser already are. - **Writes are levels and commands are edges.** `WRITE` sets a gain or a flag and is acknowledged with a result; the `idle` flag on a variable refuses the dangerous ones while the robot is moving. `COMMAND` happens once, when it arrives. -- **The trace is a scope.** The link is between twenty and a hundred times too slow to carry the - 8 kHz control loop, so `TRACE_ARM` records a group into a RAM ring at loop rate, keeping a share - of the capture from before the trigger, and `TRACE_READ` pulls it out afterwards. +- **The link cannot carry the control loop.** It is between twenty and a hundred times too slow for + 8 kHz, so a group is defined with a period in loop iterations and only every period-th iteration + is sent. The rate the application asks for is a rate it can actually receive. diff --git a/micras_comm/include/micras/comm/group.hpp b/micras_comm/include/micras/comm/group.hpp deleted file mode 100644 index 147736f..0000000 --- a/micras_comm/include/micras/comm/group.hpp +++ /dev/null @@ -1,83 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_COMM_GROUP_HPP -#define MICRAS_COMM_GROUP_HPP - -#include -#include -#include -#include - -#include "micras/comm/protocol.hpp" -#include "micras/core/variable_pool.hpp" - -namespace micras::comm { -/** - * @brief A set of variables captured in the same loop iteration. - * - * @note Sampling several variables under one header and one timestamp is not only cheaper on a link - * this slow, it is the only way the samples mean anything together. A response plotted against a - * setpoint captured two iterations later is a plot of the loop plus an unknown delay. - */ -struct Group { - /** - * @brief Variables of the group, in the order their values are packed. - */ - std::array ids{}; - - /** - * @brief Number of variables in the group. - */ - uint8_t count{}; - - /** - * @brief Number of loop iterations between two samples. - */ - uint16_t period{1}; - - /** - * @brief Number of bytes of the values of one sample. - */ - uint16_t sample_size{}; - - /** - * @brief Iterations left until the next sample. - */ - uint16_t counter{}; - - /** - * @brief Number of samples taken, which lets the application see the ones that were dropped. - */ - uint16_t sequence{}; - - /** - * @brief Whether the group is being sent. - */ - bool enabled{}; - - /** - * @brief Copy the current value of every variable of the group. - * - * @param pool Pool the variables belong to. - * @param into Buffer to copy into. - * @return Number of bytes copied, zero if the buffer is too small. - */ - std::size_t sample(const core::VariablePool& pool, std::span into) const { - if (into.size() < this->sample_size) { - return 0; - } - - std::size_t offset = 0; - - for (uint8_t index = 0; index < this->count; index++) { - offset += pool.read(this->ids.at(index), into.subspan(offset)); - } - - return offset; - } -}; -} // namespace micras::comm - -#endif // MICRAS_COMM_GROUP_HPP diff --git a/micras_comm/include/micras/comm/link.hpp b/micras_comm/include/micras/comm/link.hpp index ede0aaf..f4bb788 100644 --- a/micras_comm/include/micras/comm/link.hpp +++ b/micras_comm/include/micras/comm/link.hpp @@ -12,9 +12,7 @@ #include #include "micras/comm/frame.hpp" -#include "micras/comm/group.hpp" #include "micras/comm/protocol.hpp" -#include "micras/comm/trace.hpp" #include "micras/core/byte_stream.hpp" #include "micras/core/variable_pool.hpp" @@ -83,14 +81,10 @@ class Link { * * @param stream Transport the session runs over. * @param pool Variables the session exposes. - * @param trace Capture buffer the session arms and reads out. * @param commands Handler the commands are given to. * @param config Configuration for the link. */ - Link( - core::IByteStream& stream, core::VariablePool& pool, Trace& trace, ICommandHandler& commands, - const Config& config - ); + Link(core::IByteStream& stream, core::VariablePool& pool, ICommandHandler& commands, const Config& config); /** * @brief Take whatever arrived and act on at most one message. @@ -132,6 +126,50 @@ class Link { void register_variables(core::VariablePool& pool, std::string_view prefix); private: + /** + * @brief A set of variables sampled in the same loop iteration and sent under one timestamp. + * + * @note Sampling several variables under one header and one timestamp is not only cheaper on a + * link this slow, it is the only way the samples mean anything together. A response plotted + * against a setpoint captured two iterations later is a plot of the loop plus an unknown delay. + */ + struct Group { + /** + * @brief Variables of the group, in the order their values are packed. + */ + std::array ids{}; + + /** + * @brief Number of variables in the group. + */ + uint8_t count{}; + + /** + * @brief Number of loop iterations between two samples. + */ + uint16_t period{1}; + + /** + * @brief Number of bytes of the values of one sample. + */ + uint16_t sample_size{}; + + /** + * @brief Iterations left until the next sample. + */ + uint16_t counter{}; + + /** + * @brief Number of samples taken, which lets the application see the ones that were dropped. + */ + uint16_t sequence{}; + + /** + * @brief Whether the group is being sent. + */ + bool enabled{}; + }; + /** * @brief Act on one decoded message. * @@ -151,28 +189,12 @@ class Link { void on_write(Reader& reader, bool robot_is_idle); void on_read(Reader& reader); void on_command(Reader& reader); - void on_trace_arm(Reader& reader); - void on_trace_read(Reader& reader); ///@} /** * @brief Send one page of the schema, if one was asked for. - * - * @return True if a frame was sent, false otherwise. - */ - bool send_schema_page(); - - /** - * @brief Send one block of the capture, if one was asked for. - * - * @return True if a frame was sent, false otherwise. */ - bool send_trace_block(); - - /** - * @brief Send the state of the capture. - */ - void send_trace_status(); + void send_schema_page(); /** * @brief Send an error for a message that could not be acted on. @@ -204,7 +226,6 @@ class Link { // NOLINTBEGIN(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot core::IByteStream& stream; core::VariablePool& pool; - Trace& trace; ICommandHandler& commands; // NOLINTEND(*-avoid-const-or-ref-data-members) @@ -241,16 +262,6 @@ class Link { */ uint16_t schema_index{}; - /** - * @brief Offset of the next block of the capture to send, past the end when none is due. - */ - uint32_t trace_offset{}; - - /** - * @brief Whether a capture is being read out. - */ - bool trace_dumping{}; - /** * @brief Counters worth watching from the application. */ diff --git a/micras_comm/include/micras/comm/protocol.hpp b/micras_comm/include/micras/comm/protocol.hpp index 7b22a0c..58cdfd8 100644 --- a/micras_comm/include/micras/comm/protocol.hpp +++ b/micras_comm/include/micras/comm/protocol.hpp @@ -62,9 +62,7 @@ enum class MessageType : uint8_t { WRITE = 0x06, READ = 0x07, COMMAND = 0x08, - TRACE_ARM = 0x09, - TRACE_READ = 0x0A, - PING = 0x0B, + PING = 0x09, HELLO_ACK = 0x81, SCHEMA_PAGE = 0x82, @@ -73,10 +71,8 @@ enum class MessageType : uint8_t { WRITE_ACK = 0x86, VALUE = 0x87, COMMAND_ACK = 0x88, - LOG = 0x89, - TRACE_STATUS = 0x8A, - TRACE_DATA = 0x8B, - PONG = 0x8C, + PONG = 0x89, + LOG = 0x8A, ERROR = 0x8F }; @@ -101,26 +97,6 @@ enum class Severity : uint8_t { WARNING = 2, ERROR = 3 }; - -/** - * @brief What starts a trace capture. - */ -enum class TriggerType : uint8_t { - IMMEDIATE = 0, - COMMAND = 1, - ABOVE = 2, - BELOW = 3 -}; - -/** - * @brief State of the trace capture. - */ -enum class TraceState : uint8_t { - IDLE = 0, - ARMED = 1, - TRIGGERED = 2, - FULL = 3 -}; } // namespace micras::comm #endif // MICRAS_COMM_PROTOCOL_HPP diff --git a/micras_comm/include/micras/comm/trace.hpp b/micras_comm/include/micras/comm/trace.hpp deleted file mode 100644 index fd51392..0000000 --- a/micras_comm/include/micras/comm/trace.hpp +++ /dev/null @@ -1,190 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_COMM_TRACE_HPP -#define MICRAS_COMM_TRACE_HPP - -#include -#include -#include - -#include "micras/comm/group.hpp" -#include "micras/comm/protocol.hpp" -#include "micras/core/variable_pool.hpp" - -namespace micras::comm { -/** - * @brief Full rate capture of a group into a ring in RAM. - * - * @note The link is between twenty and a hundred times too slow to carry the control loop, so the - * only way to see what happened inside a fast run is to record it at loop rate and read it out - * afterwards. Capturing is a copy into a ring and nothing else, and nothing goes on the wire until - * the capture has stopped. - * - * @note Samples carry no timestamp of their own. The period is fixed at arming, so the instant of - * sample i is the trigger timestamp plus i times the period, which is four bytes per sample saved - * on the only path where they would be paid at loop rate. - */ -class Trace { -public: - /** - * @brief Construct a new Trace object. - * - * @param pool Pool the captured variables belong to. - * @param ring Memory to capture into, which bounds how long a capture can be. - */ - Trace(const core::VariablePool& pool, std::span ring); - - /** - * @brief Arm a capture. - * - * @param group Group to capture, which must already be defined. - * @param pre_trigger Share of the capture kept from before the trigger, in percent. - * @param trigger What starts the capture. - * @param id Variable watched by a threshold trigger. - * @param threshold Value the watched variable has to cross. - * @return True if the capture was armed, false if the group does not fit or the trigger makes - * no sense for the variable it watches. - */ - bool arm(const Group& group, uint8_t pre_trigger, TriggerType trigger, core::VariableId id, float threshold); - - /** - * @brief Start a capture waiting on a command trigger. - */ - void fire(); - - /** - * @brief Abandon the current capture. - */ - void stop(); - - /** - * @brief Take one sample, if a capture is running. - * - * @note Called every loop iteration. It costs a compare when nothing is armed. - * - * @param timestamp_us Current time in microseconds, recorded at the trigger. - */ - void sample(uint32_t timestamp_us); - - /** - * @brief Get the state of the capture. - * - * @return Current state. - */ - TraceState state() const; - - /** - * @brief Get how many samples are held. - * - * @return Number of samples that can be read. - */ - uint32_t held() const; - - /** - * @brief Get how many of the held samples precede the trigger. - * - * @return Number of samples taken before the trigger fired. - */ - uint32_t pre_trigger() const; - - /** - * @brief Get the number of bytes of one sample. - * - * @return Size of a sample in bytes. - */ - uint16_t sample_size() const; - - /** - * @brief Get the period of the capture. - * - * @return Number of loop iterations between two samples. - */ - uint16_t period() const; - - /** - * @brief Get the time the trigger fired. - * - * @return Timestamp of the first sample after the trigger, in microseconds. - */ - uint32_t timestamp() const; - - /** - * @brief Copy captured bytes, oldest first. - * - * @param offset Byte offset into the capture, counting from the oldest sample held. - * @param into Buffer to copy into. - * @return Number of bytes copied. - */ - std::size_t read(uint32_t offset, std::span into) const; - -private: - /** - * @brief Check whether a threshold trigger has just been crossed. - * - * @return True if the capture should start now, false otherwise. - */ - bool triggered_by_threshold(); - - /** - * @brief Write the current values of the group at the head of the ring. - */ - void store(); - - // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot - const core::VariablePool& pool; - - std::span ring; - - Group group; - - /** - * @brief Number of samples the ring can hold. - */ - uint32_t capacity{}; - - /** - * @brief Number of samples written since the capture was armed, saturated at the capacity. - */ - uint32_t filled{}; - - /** - * @brief Index of the sample that will be written next. - */ - uint32_t head{}; - - /** - * @brief Number of samples still to take before the capture is complete. - */ - uint32_t remaining{}; - - /** - * @brief Number of samples requested from before the trigger, and how many were actually kept. - */ - ///@{ - uint32_t requested_pre{}; - uint32_t kept_pre{}; - ///@} - - /** - * @brief Iterations left until the next sample. - */ - uint16_t counter{}; - - core::VariableId watched{}; - float threshold{}; - float last_value{}; - bool has_last_value{}; - - TriggerType trigger{TriggerType::IMMEDIATE}; - TraceState current_state{TraceState::IDLE}; - - /** - * @brief Time of the first sample taken after the trigger. - */ - uint32_t trigger_timestamp{}; -}; -} // namespace micras::comm - -#endif // MICRAS_COMM_TRACE_HPP diff --git a/micras_comm/src/link.cpp b/micras_comm/src/link.cpp index 83f350b..d37f571 100644 --- a/micras_comm/src/link.cpp +++ b/micras_comm/src/link.cpp @@ -11,19 +11,15 @@ #include #include "micras/comm/frame.hpp" -#include "micras/comm/group.hpp" #include "micras/comm/link.hpp" #include "micras/comm/protocol.hpp" -#include "micras/comm/trace.hpp" #include "micras/core/byte_stream.hpp" #include "micras/core/serializable.hpp" #include "micras/core/variable_pool.hpp" namespace micras::comm { -Link::Link( - core::IByteStream& stream, core::VariablePool& pool, Trace& trace, ICommandHandler& commands, const Config& config -) : - stream{stream}, pool{pool}, trace{trace}, commands{commands}, config{config} { +Link::Link(core::IByteStream& stream, core::VariablePool& pool, ICommandHandler& commands, const Config& config) : + stream{stream}, pool{pool}, commands{commands}, config{config} { this->schema_index = pool.all().size(); } @@ -83,14 +79,6 @@ void Link::execute(bool robot_is_idle) { this->on_command(payload_reader); return; - case MessageType::TRACE_ARM: - this->on_trace_arm(payload_reader); - return; - - case MessageType::TRACE_READ: - this->on_trace_read(payload_reader); - return; - case MessageType::PING: this->send(MessageType::PONG, {}); return; @@ -106,8 +94,6 @@ void Link::on_hello() { group = {}; } - this->trace.stop(); - this->trace_dumping = false; this->schema_index = this->pool.all().size(); this->credit = initial_credit; @@ -278,59 +264,6 @@ void Link::on_command(Reader& reader) { this->send(MessageType::COMMAND_ACK, writer.done()); } -void Link::on_trace_arm(Reader& reader) { - const uint8_t index = reader.u8(); - const uint8_t pre_trigger = reader.u8(); - const auto trigger = static_cast(reader.u8()); - const core::VariableId watched = reader.u16(); - const uint32_t raw_threshold = reader.u32(); - - if (not reader.valid() or index >= max_groups) { - this->send_error(ErrorCode::NO_SUCH_GROUP, index); - return; - } - - this->trace_dumping = false; - - if (not this->trace.arm( - this->groups.at(index), pre_trigger, trigger, watched, std::bit_cast(raw_threshold) - )) { - this->send_error(ErrorCode::MALFORMED, index); - return; - } - - this->send_trace_status(); -} - -void Link::on_trace_read(Reader& reader) { - const uint32_t offset = reader.u32(); - - if (not reader.valid()) { - this->send_error(ErrorCode::MALFORMED, 0); - return; - } - - if (this->trace.state() != TraceState::FULL) { - this->send_trace_status(); - return; - } - - this->trace_offset = offset; - this->trace_dumping = true; -} - -void Link::send_trace_status() { - Writer writer{this->payload}; - writer.u8(std::to_underlying(this->trace.state())); - writer.u32(this->trace.held()); - writer.u32(this->trace.pre_trigger()); - writer.u16(this->trace.sample_size()); - writer.u16(this->trace.period()); - writer.u32(this->trace.timestamp()); - - this->send(MessageType::TRACE_STATUS, writer.done()); -} - void Link::send_error(ErrorCode code, uint16_t context) { Writer writer{this->payload}; writer.u8(std::to_underlying(code)); @@ -370,11 +303,11 @@ bool Link::send_metered(MessageType type, std::span payload) { return true; } -bool Link::send_schema_page() { +void Link::send_schema_page() { const std::span variables = this->pool.all(); if (this->schema_index >= variables.size()) { - return false; + return; } constexpr std::size_t count_offset{8}; @@ -409,42 +342,14 @@ bool Link::send_schema_page() { if (count == 0) { this->schema_index = variables.size(); - return false; + return; } this->payload.at(count_offset) = count; - if (not this->send_metered(MessageType::SCHEMA_PAGE, writer.done())) { - return false; + if (this->send_metered(MessageType::SCHEMA_PAGE, writer.done())) { + this->schema_index = index; } - - this->schema_index = index; - return true; -} - -bool Link::send_trace_block() { - if (not this->trace_dumping) { - return false; - } - - constexpr std::size_t header_size{4}; - - const std::size_t taken = this->trace.read(this->trace_offset, std::span{this->payload}.subspan(header_size)); - - if (taken == 0) { - this->trace_dumping = false; - return false; - } - - Writer writer{this->payload}; - writer.u32(this->trace_offset); - - if (not this->send_metered(MessageType::TRACE_DATA, std::span{this->payload}.first(header_size + taken))) { - return false; - } - - this->trace_offset += taken; - return true; } void Link::pump(uint32_t timestamp_us) { @@ -462,24 +367,22 @@ void Link::pump(uint32_t timestamp_us) { group.counter = group.period - 1; - constexpr std::size_t header_size{7}; - Writer writer{this->payload}; writer.u8(index); writer.u16(group.sequence++); writer.u32(timestamp_us); - group.sample(this->pool, std::span{this->payload}.subspan(header_size, group.sample_size)); + std::size_t offset = writer.done().size(); - const std::span message = std::span{this->payload}.first(header_size + group.sample_size); + for (uint8_t position = 0; position < group.count; position++) { + offset += this->pool.read(group.ids.at(position), std::span{this->payload}.subspan(offset)); + } - if (not this->send_metered(MessageType::SAMPLE, message)) { + if (not this->send_metered(MessageType::SAMPLE, std::span{this->payload}.first(offset))) { this->dropped_samples++; } } - if (not this->send_schema_page()) { - this->send_trace_block(); - } + this->send_schema_page(); } } // namespace micras::comm diff --git a/micras_comm/src/trace.cpp b/micras_comm/src/trace.cpp deleted file mode 100644 index fd0b3bc..0000000 --- a/micras_comm/src/trace.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/** - * @file - */ - -#include -#include -#include -#include -#include - -#include "micras/comm/group.hpp" -#include "micras/comm/protocol.hpp" -#include "micras/comm/trace.hpp" -#include "micras/core/variable_pool.hpp" - -namespace micras::comm { -Trace::Trace(const core::VariablePool& pool, std::span ring) : pool{pool}, ring{ring} { } - -bool Trace::arm(const Group& group, uint8_t pre_trigger, TriggerType trigger, core::VariableId id, float threshold) { - if (group.count == 0 or group.sample_size == 0 or group.sample_size > this->ring.size()) { - return false; - } - - if (trigger == TriggerType::ABOVE or trigger == TriggerType::BELOW) { - if (id >= this->pool.all().size() or this->pool.at(id).type != core::TypeCode::F32) { - return false; - } - } - - this->group = group; - this->capacity = this->ring.size() / group.sample_size; - this->requested_pre = std::min(this->capacity * std::min(pre_trigger, 100) / 100, this->capacity - 1); - this->trigger = trigger; - this->watched = id; - this->threshold = threshold; - - this->filled = 0; - this->head = 0; - this->kept_pre = 0; - this->remaining = 0; - this->counter = 0; - this->has_last_value = false; - this->current_state = TraceState::ARMED; - - return true; -} - -void Trace::fire() { - if (this->current_state == TraceState::ARMED and this->trigger == TriggerType::COMMAND) { - this->kept_pre = std::min(this->filled, this->requested_pre); - this->remaining = this->capacity - this->kept_pre; - this->current_state = TraceState::TRIGGERED; - } -} - -void Trace::stop() { - this->current_state = TraceState::IDLE; -} - -bool Trace::triggered_by_threshold() { - float value = 0.0F; - std::memcpy(&value, this->pool.at(this->watched).address, sizeof(float)); - - const bool crossed = this->has_last_value and (this->trigger == TriggerType::ABOVE ? - this->last_value <= this->threshold and value > this->threshold : - this->last_value >= this->threshold and value < this->threshold); - - this->last_value = value; - this->has_last_value = true; - - return crossed; -} - -void Trace::store() { - const std::size_t offset = static_cast(this->head) * this->group.sample_size; - this->group.sample(this->pool, this->ring.subspan(offset, this->group.sample_size)); - - this->head = (this->head + 1) % this->capacity; - this->filled = std::min(this->filled + 1, this->capacity); -} - -void Trace::sample(uint32_t timestamp_us) { - if (this->current_state != TraceState::ARMED and this->current_state != TraceState::TRIGGERED) { - return; - } - - if (this->current_state == TraceState::ARMED) { - if (this->trigger == TriggerType::IMMEDIATE or - ((this->trigger == TriggerType::ABOVE or this->trigger == TriggerType::BELOW) and - this->triggered_by_threshold())) { - this->kept_pre = std::min(this->filled, this->requested_pre); - this->remaining = this->capacity - this->kept_pre; - this->current_state = TraceState::TRIGGERED; - } - } - - if (this->counter > 0) { - this->counter--; - return; - } - - this->counter = this->group.period - 1; - - if (this->current_state == TraceState::TRIGGERED and this->remaining == this->capacity - this->kept_pre) { - this->trigger_timestamp = timestamp_us; - } - - this->store(); - - if (this->current_state == TraceState::TRIGGERED and --this->remaining == 0) { - this->current_state = TraceState::FULL; - } -} - -TraceState Trace::state() const { - return this->current_state; -} - -uint32_t Trace::held() const { - return this->current_state == TraceState::FULL ? this->filled : 0; -} - -uint32_t Trace::pre_trigger() const { - return this->kept_pre; -} - -uint16_t Trace::sample_size() const { - return this->group.sample_size; -} - -uint16_t Trace::period() const { - return this->group.period; -} - -uint32_t Trace::timestamp() const { - return this->trigger_timestamp; -} - -std::size_t Trace::read(uint32_t offset, std::span into) const { - const std::size_t usable = static_cast(this->capacity) * this->group.sample_size; - const std::size_t total = static_cast(this->held()) * this->group.sample_size; - const std::size_t from = offset; - - if (from >= total) { - return 0; - } - - const std::size_t oldest = static_cast(this->head) * this->group.sample_size; - const std::size_t size = std::min(into.size(), total - from); - const std::size_t start = (oldest + from) % usable; - const std::size_t first = std::min(size, usable - start); - - // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) bounded just above - std::memcpy(into.data(), &this->ring[start], first); - - if (first < size) { - std::memcpy(&into[first], this->ring.data(), size - first); - } - // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) - - return size; -} -} // namespace micras::comm diff --git a/micras_core/include/micras/core/variable_pool.hpp b/micras_core/include/micras/core/variable_pool.hpp index ae8d966..8bc45b2 100644 --- a/micras_core/include/micras/core/variable_pool.hpp +++ b/micras_core/include/micras/core/variable_pool.hpp @@ -122,7 +122,7 @@ concept Registrable = std::is_trivially_copyable_v and requires { TypeCodeOf< */ struct Access { /** - * @brief Whether the variable can be part of a stream or of a trace group. + * @brief Whether the variable can be part of a stream. */ bool stream : 1 {}; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index b596e57..0b3f05d 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -28,7 +28,6 @@ target_include_directories(micras_host_core PUBLIC ${MICRAS_ROOT}/micras_core/in add_library(micras_host_comm STATIC ${MICRAS_ROOT}/micras_comm/src/frame.cpp ${MICRAS_ROOT}/micras_comm/src/link.cpp - ${MICRAS_ROOT}/micras_comm/src/trace.cpp ) target_include_directories(micras_host_comm PUBLIC ${MICRAS_ROOT}/micras_comm/include) diff --git a/tests/test_link.cpp b/tests/test_link.cpp index 7c8a73e..353dde9 100644 --- a/tests/test_link.cpp +++ b/tests/test_link.cpp @@ -12,7 +12,6 @@ #include "micras/comm/frame.hpp" #include "micras/comm/link.hpp" -#include "micras/comm/trace.hpp" #include "micras/core/byte_stream.hpp" #include "micras/core/variable_pool.hpp" #include "test_host.hpp" @@ -120,16 +119,13 @@ int main() { pool.add("model/", "gain", gain, {.stream = true, .write = true, .idle = true, .persist = true}); pool.add("", "run_profile", profile, {.stream = true, .write = true}); - std::vector ring(1024); - Trace trace{pool, ring}; - Commands commands; - Loopback io; - Link link{io, pool, trace, commands, {.loop_time_us = 125}}; + Commands commands; + Loopback io; + Link link{io, pool, commands, {.loop_time_us = 125}}; auto step = [&](uint32_t t, bool idle = true) { link.poll(idle); link.pump(t); - trace.sample(t); }; auto settle = [&](uint32_t& t, int n = 40, bool idle = true) { for (int i = 0; i < n; i++) @@ -284,64 +280,5 @@ int main() { if (m.type == MessageType::VALUE) CHECK(u16(m.payload, 0) == 1); - // --- trace: full rate capture with pre-trigger, read out afterwards --- - drain(io); - send(io, MessageType::GROUP_DEFINE, {1, 1, 0, 1, 0, 0}); // group 1, period 1, just cmd/linear - settle(now, 4); - drain(io); - - // trigger when cmd/linear rises through 10 - V arm{1, 50, uint8_t(TriggerType::ABOVE), 0, 0, 0, 0, 0x20, 0x41}; // threshold 10.0F - linear = 0.0F; - send(io, MessageType::TRACE_ARM, arm); - settle(now, 4); - msgs = drain(io); - CHECK(only(msgs, MessageType::TRACE_STATUS).payload[0] == uint8_t(TraceState::ARMED)); - - const std::size_t capacity = ring.size() / 4; - const std::size_t pre = capacity / 2; - - for (std::size_t i = 0; i < capacity; i++) { - linear = -float(i); - step(now += 125); - } - CHECK(trace.state() == TraceState::ARMED); // nothing crossed the threshold yet - - for (std::size_t i = 0; i < pre; i++) { - linear = 50.0F + float(i); - step(now += 125); - } - CHECK(trace.state() == TraceState::FULL); - CHECK(trace.held() == capacity); - CHECK(trace.pre_trigger() == pre); - CHECK(trace.sample_size() == 4); - CHECK(trace.period() == 1); - - drain(io); - send(io, MessageType::TRACE_READ, {0, 0, 0, 0}); - - // the readout runs at whatever rate the application returns credit at, which is the point - V dump; - for (int round = 0; round < 200 && dump.size() < capacity * 4; round++) { - settle(now, 8); - msgs = drain(io); - - for (auto& m : msgs) { - if (m.type != MessageType::TRACE_DATA) - continue; - CHECK(u32(m.payload, 0) == dump.size()); - dump.insert(dump.end(), m.payload.begin() + 4, m.payload.end()); - } - } - CHECK(dump.size() == capacity * 4); - - // the oldest half is what preceded the trigger, the newest half what followed it - for (std::size_t i = 0; i < pre; i++) { - CHECK(f32(dump, i * 4) == -float(pre + i)); - } - for (std::size_t i = 0; i < capacity - pre; i++) { - CHECK(f32(dump, (pre + i) * 4) == 50.0F + float(i)); - } - std::puts("link ok"); } From bc69df802429fd815fc79ad46219d17a55b9300e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 23 Sep 2026 00:04:20 -0300 Subject: [PATCH 485/561] =?UTF-8?q?=F0=9F=93=9D=20Say=20what=20the=20recep?= =?UTF-8?q?tacle=20actually=20is?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The wiring note claimed the board has no USB device mode. It has one. `D-` and `D+` reach `PA11` and `PA12`, which the device database gives as `USB_OTG_HS_DM` and `USB_OTG_HS_DP`; `CC1` and `CC2` terminate in 5.1 kฮฉ to ground, which advertises a sink, and `D+` carries a 1.5 kฮฉ pull-up to 3V3, which is the full speed device signature. The receptacle is a USB device port that the radio module is currently occupying. Only one of the two can have it at a time, so nothing about the link changes. What changes is that the next person reading this will not conclude the option does not exist. --- micras_comm/README.md | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/micras_comm/README.md b/micras_comm/README.md index 773632b..fcd55e1 100644 --- a/micras_comm/README.md +++ b/micras_comm/README.md @@ -10,13 +10,14 @@ The module's serial port *is* the USB-C receptacle. `D-` reaches `PA11` and `D+` each through a 33 ฮฉ series resistor, and `SBU1`/`SBU2` supply 3V3 through a 300 mA resettable fuse. `CC1` and `CC2` terminate in 5.1 kฮฉ resistors and never reach the microcontroller. -Two consequences follow from that, and both shape the protocol: - -- **There is no hardware flow control.** The CC2640R2F inside the module has `RTS`/`CTS`, but the - board cannot reach them, and every other conductor of the receptacle is already used. The module - therefore drops bytes silently when its buffer fills, which is what the credit window in the - session layer exists to prevent. -- **USB device mode does not exist on this board.** The receptacle is a serial port, not a USB one. +One consequence of that shapes the protocol: **there is no hardware flow control.** The CC2640R2F +inside the module has `RTS`/`CTS`, but the board cannot reach them, and every other conductor of the +receptacle is already used. The module therefore drops bytes silently when its buffer fills, which +is what the credit window in the session layer exists to prevent. + +`PA11` and `PA12` are also `USB_OTG_HS_DM` and `USB_OTG_HS_DP`, and the board carries the 5.1 kฮฉ +`CC` terminations and the 1.5 kฮฉ `D+` pull-up of a full speed USB device, so the same receptacle +can be a USB port instead. It cannot be both at once, and the radio owns it. ## Configuring the module From 5285e2d3dac1b30d65d78f285204aac26f27a0ac Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 23 Sep 2026 00:10:22 -0300 Subject: [PATCH 486/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Compute=20the=20fr?= =?UTF-8?q?ame=20check=20from=20the=20polynomial?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The check was a nibble table: sixteen constants nothing in the file derives, and a translation unit of its own to hold them. The table buys speed, and speed is not what this path is short of โ€” it runs a hundred times a second over a few hundred bytes, against a serial port that takes eighty microseconds to carry each of them. Computed a bit at a time it is the same check value, the same bytes on the wire, and the polynomial is readable in the code instead of precomputed into it. Small enough now to be an inline function, so `crc.cpp` is gone with the table. --- micras_core/include/micras/core/crc.hpp | 29 ++++++++++++++++++++----- micras_core/src/crc.cpp | 27 ----------------------- tests/CMakeLists.txt | 1 - 3 files changed, 24 insertions(+), 33 deletions(-) delete mode 100644 micras_core/src/crc.cpp diff --git a/micras_core/include/micras/core/crc.hpp b/micras_core/include/micras/core/crc.hpp index e552750..1138780 100644 --- a/micras_core/include/micras/core/crc.hpp +++ b/micras_core/include/micras/core/crc.hpp @@ -12,16 +12,35 @@ namespace micras::core { /** * @brief Compute the CRC-16/CCITT-FALSE of a buffer. * - * @note Implemented in software, over a nibble table, instead of through the CRC peripheral. The - * peripheral is configured for the polynomial of the rotary sensors and is stateful, so sharing it - * with the 8 kHz sensor path would couple two unrelated things to save a few cycles on a path that - * runs a hundred times a second. + * @note Computed a bit at a time, from the polynomial itself, rather than through a table or the + * CRC peripheral. The peripheral is configured for the polynomial of the rotary sensors and is + * stateful, so sharing it would couple the 8 kHz sensor path to this one; a table would buy speed + * on a path that runs a hundred times a second and carries a few hundred bytes each time. * * @param data Buffer to compute the CRC of. * @param seed Initial value of the register, so that a CRC can be computed in several calls. * @return CRC of the buffer. */ -uint16_t crc16(std::span data, uint16_t seed = 0xFFFF); +constexpr uint16_t crc16(std::span data, uint16_t seed = 0xFFFF) { + constexpr uint16_t polynomial{0x1021}; + + uint16_t crc = seed; + + for (const uint8_t byte : data) { + crc ^= static_cast(byte << 8U); + + for (uint8_t bit = 0; bit < 8; bit++) { + const bool overflow = (crc & 0x8000U) != 0; + crc = static_cast(crc << 1U); + + if (overflow) { + crc ^= polynomial; + } + } + } + + return crc; +} } // namespace micras::core #endif // MICRAS_CORE_CRC_HPP diff --git a/micras_core/src/crc.cpp b/micras_core/src/crc.cpp deleted file mode 100644 index 332b7c3..0000000 --- a/micras_core/src/crc.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @file - */ - -#include -#include -#include - -#include "micras/core/crc.hpp" - -namespace micras::core { -static constexpr std::array crc16_table{ - 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, - 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, -}; - -uint16_t crc16(std::span data, uint16_t seed) { - uint16_t crc = seed; - - for (const uint8_t byte : data) { - crc = static_cast(crc << 4) ^ crc16_table.at((crc >> 12) ^ (byte >> 4)); - crc = static_cast(crc << 4) ^ crc16_table.at((crc >> 12) ^ (byte & 0x0F)); - } - - return crc; -} -} // namespace micras::core diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 0b3f05d..34bbfb8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -19,7 +19,6 @@ enable_testing() add_library(micras_host_core STATIC ${MICRAS_ROOT}/micras_core/src/cobs.cpp - ${MICRAS_ROOT}/micras_core/src/crc.cpp ${MICRAS_ROOT}/micras_core/src/variable_pool.cpp ) From 246da9ca032edcad5cad1e9dfe00a9534fd0ce15 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Wed, 23 Sep 2026 00:32:22 -0300 Subject: [PATCH 487/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Check=20frames=20w?= =?UTF-8?q?ith=20two=20running=20sums?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Fletcher-16 replaces the CRC. Two sums modulo 255, four lines, and they live in `frame.cpp` beside the encoder and the reader that are the only things that ever wanted them, so `core::crc` and its test are gone. The odds a spliced frame passes are one in sixty five thousand either way. What is given up is the CRC's guarantee on long bursts, which the radio's own CRC-24 and retransmission already cover on the only hop long enough to have them. The frame vectors are regenerated from the new codec and still match micras-monitor byte for byte. --- micras_comm/README.md | 17 +++++---- micras_comm/src/frame.cpp | 17 +++++++-- micras_core/include/micras/core/crc.hpp | 46 ------------------------- micras_proxy/src/imu.cpp | 1 - tests/CMakeLists.txt | 3 +- tests/test_crc.cpp | 42 ---------------------- tests/test_frame.cpp | 18 +++++----- 7 files changed, 34 insertions(+), 110 deletions(-) delete mode 100644 micras_core/include/micras/core/crc.hpp delete mode 100644 tests/test_crc.cpp diff --git a/micras_comm/README.md b/micras_comm/README.md index fcd55e1..56d9a17 100644 --- a/micras_comm/README.md +++ b/micras_comm/README.md @@ -50,15 +50,18 @@ Frames are [COBS](https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffin delimited by a zero byte: ```text -COBS( type | payload | crc16 ) 0x00 +COBS( type | payload | fletcher16 ) 0x00 ``` -The CRC-16/CCITT-FALSE covers the type and the payload, before encoding. It is not protecting -against the radio, which has a CRC-24 and retransmits until acknowledged. It covers the two hops -BLE never sees: the 8N1 serial port with no parity between the microcontroller and the module, and -the module's buffer, which drops runs of bytes with no indication that it did. A dropped run can -join the head of one frame to the tail of the next into something with a plausible shape, which a -sum passes far too often and a CRC does not. +The frame check is Fletcher-16 over the type and the payload, before encoding, little endian. It is +not protecting against the radio, which has a CRC-24 and retransmits until acknowledged. It covers +the two hops BLE never sees: the 8N1 serial port with no parity between the microcontroller and the +module, and the module's buffer, which drops runs of bytes with no indication that it did. A +dropped run can join the head of one frame to the tail of the next into something with a plausible +shape, and the two running sums make that about as unlikely as a sixteen bit check can: one over +sixty five thousand, against one in two hundred and fifty five for a single sum of bytes. It is +weaker than a CRC on long bursts, which is what a CRC is uniquely good at, and it is four lines +that live beside the codec they belong to. Message types, their payloads and the meaning of every field are in `micras_comm/include/micras/comm/protocol.hpp`. Everything on the wire is little endian, which is diff --git a/micras_comm/src/frame.cpp b/micras_comm/src/frame.cpp index 3aa04c6..ccaff5e 100644 --- a/micras_comm/src/frame.cpp +++ b/micras_comm/src/frame.cpp @@ -14,7 +14,6 @@ #include "micras/comm/frame.hpp" #include "micras/comm/protocol.hpp" #include "micras/core/cobs.hpp" -#include "micras/core/crc.hpp" namespace micras::comm { // The buffers here are all fixed size arrays and spans the caller owns, indexed right after the @@ -94,6 +93,18 @@ bool Reader::valid() const { return not this->underflowed; } +static uint16_t fletcher16(std::span data) { + uint16_t low = 0; + uint16_t high = 0; + + for (const uint8_t byte : data) { + low = static_cast((low + byte) % 255); + high = static_cast((high + low) % 255); + } + + return static_cast(high << 8U | low); +} + std::size_t encode_frame(MessageType type, std::span payload, std::span into) { if (payload.size() > max_payload_size or into.empty()) { return 0; @@ -107,7 +118,7 @@ std::size_t encode_frame(MessageType type, std::span payload, std } const std::size_t checked_size = payload.size() + 1; - const uint16_t check = core::crc16(std::span{plain}.first(checked_size)); + const uint16_t check = fletcher16(std::span{plain}.first(checked_size)); plain[checked_size] = check; plain[checked_size + 1] = check >> 8; @@ -155,7 +166,7 @@ bool FrameReader::finish() { const std::span checked = std::span{this->decoded}.first(size - 2); const auto expected = static_cast(this->decoded[size - 2] | this->decoded[size - 1] << 8); - if (core::crc16(checked) != expected) { + if (fletcher16(checked) != expected) { return false; } diff --git a/micras_core/include/micras/core/crc.hpp b/micras_core/include/micras/core/crc.hpp deleted file mode 100644 index 1138780..0000000 --- a/micras_core/include/micras/core/crc.hpp +++ /dev/null @@ -1,46 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_CORE_CRC_HPP -#define MICRAS_CORE_CRC_HPP - -#include -#include - -namespace micras::core { -/** - * @brief Compute the CRC-16/CCITT-FALSE of a buffer. - * - * @note Computed a bit at a time, from the polynomial itself, rather than through a table or the - * CRC peripheral. The peripheral is configured for the polynomial of the rotary sensors and is - * stateful, so sharing it would couple the 8 kHz sensor path to this one; a table would buy speed - * on a path that runs a hundred times a second and carries a few hundred bytes each time. - * - * @param data Buffer to compute the CRC of. - * @param seed Initial value of the register, so that a CRC can be computed in several calls. - * @return CRC of the buffer. - */ -constexpr uint16_t crc16(std::span data, uint16_t seed = 0xFFFF) { - constexpr uint16_t polynomial{0x1021}; - - uint16_t crc = seed; - - for (const uint8_t byte : data) { - crc ^= static_cast(byte << 8U); - - for (uint8_t bit = 0; bit < 8; bit++) { - const bool overflow = (crc & 0x8000U) != 0; - crc = static_cast(crc << 1U); - - if (overflow) { - crc ^= polynomial; - } - } - } - - return crc; -} -} // namespace micras::core - -#endif // MICRAS_CORE_CRC_HPP diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 357198a..5aae772 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -185,5 +185,4 @@ void Imu::calibrate() { bool Imu::was_initialized() const { return this->initialized; } - } // namespace micras::proxy diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 34bbfb8..8023f50 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -45,7 +45,7 @@ target_include_directories(micras_host_proxy PUBLIC target_link_libraries(micras_host_proxy PUBLIC micras_host_core) -foreach(TEST_NAME variable_pool crc cobs frame storage link) +foreach(TEST_NAME variable_pool cobs frame storage link) add_executable(test_${TEST_NAME} test_${TEST_NAME}.cpp) target_include_directories(test_${TEST_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) target_compile_options(test_${TEST_NAME} PRIVATE -Wall -Wextra -fsanitize=address,undefined) @@ -54,7 +54,6 @@ foreach(TEST_NAME variable_pool crc cobs frame storage link) endforeach() target_link_libraries(test_variable_pool PRIVATE micras_host_core) -target_link_libraries(test_crc PRIVATE micras_host_core) target_link_libraries(test_cobs PRIVATE micras_host_core) target_link_libraries(test_frame PRIVATE micras_host_comm) target_link_libraries(test_storage PRIVATE micras_host_proxy) diff --git a/tests/test_crc.cpp b/tests/test_crc.cpp deleted file mode 100644 index 66f4ebe..0000000 --- a/tests/test_crc.cpp +++ /dev/null @@ -1,42 +0,0 @@ -/** - * @file - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "micras/core/crc.hpp" -#include "test_host.hpp" - -using namespace micras::core; - -int main() { - const std::string_view text{"123456789"}; - const std::span bytes{std::bit_cast(text.data()), text.size()}; - - // The published check value of CRC-16/CCITT-FALSE, so this is pinned to the standard rather - // than to whatever the other implementation of it happens to agree with - CHECK(crc16(bytes) == 0x29B1); - - // Computing it in two calls has to give the same answer as computing it in one - CHECK(crc16(bytes.subspan(4), crc16(bytes.first(4))) == crc16(bytes)); - - // A single flipped bit anywhere has to change it - for (std::size_t index = 0; index < text.size(); index++) { - for (uint8_t bit = 0; bit < 8; bit++) { - std::array flipped{}; - std::ranges::copy(bytes, flipped.begin()); - flipped.at(index) ^= static_cast(1U << bit); - - CHECK(crc16(flipped) != crc16(bytes)); - } - } - - std::puts("crc ok"); -} diff --git a/tests/test_frame.cpp b/tests/test_frame.cpp index 049f090..edae639 100644 --- a/tests/test_frame.cpp +++ b/tests/test_frame.cpp @@ -26,18 +26,18 @@ struct Vector { // and reader disagreement this project has had was invisible until something was on the wire, so // the two implementations are pinned to the same bytes rather than to each other's good intentions. const std::vector vectors{ - {"hello", MessageType{0x01}, {}, {4, 1, 209, 241, 0}}, + {"hello", MessageType{0x01}, {}, {4, 1, 1, 1, 0}}, {"hello_ack", MessageType{0x81}, {1, 133, 114, 115, 247, 4, 0, 125, 0, 0, 0, 0, 1}, - {8, 129, 1, 133, 114, 115, 247, 4, 2, 125, 1, 1, 1, 4, 1, 40, 160, 0}}, - {"schema_request", MessageType{0x02}, {0, 0}, {2, 2, 1, 3, 252, 162, 0}}, - {"group_define", MessageType{0x03}, {0, 80, 0, 2, 0, 0, 1, 0}, {2, 3, 2, 80, 2, 2, 1, 2, 1, 3, 19, 168, 0}}, - {"credit", MessageType{0x05}, {0, 1}, {2, 5, 4, 1, 77, 55, 0}}, - {"write", MessageType{0x06}, {2, 0, 0, 0, 128, 63}, {3, 6, 2, 1, 1, 5, 128, 63, 143, 7, 0}}, + {8, 129, 1, 133, 114, 115, 247, 4, 2, 125, 1, 1, 1, 4, 1, 104, 155, 0}}, + {"schema_request", MessageType{0x02}, {0, 0}, {2, 2, 1, 3, 2, 6, 0}}, + {"group_define", MessageType{0x03}, {0, 80, 0, 2, 0, 0, 1, 0}, {2, 3, 2, 80, 2, 2, 1, 2, 1, 3, 86, 89, 0}}, + {"credit", MessageType{0x05}, {0, 1}, {2, 5, 4, 1, 6, 16, 0}}, + {"write", MessageType{0x06}, {2, 0, 0, 0, 128, 63}, {3, 6, 2, 1, 1, 5, 128, 63, 199, 118, 0}}, {"zeros", MessageType{0x85}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {2, 133, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, - 1, 1, 1, 3, 142, 100, 0}}, + 1, 1, 1, 3, 133, 221, 0}}, {"ramp", MessageType{0x8B}, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, @@ -57,13 +57,13 @@ const std::vector vectors{ 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, - 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 115, 21, 0}}, + 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 149, 49, 0}}, {"high", MessageType{0x89}, {200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239}, {44, 137, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, - 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 42, 80, 0}}, + 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 247, 247, 0}}, }; } // namespace From dadd8e728f58a28b5dc4cd35f3cdcf0865252d39 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 03:23:52 -0300 Subject: [PATCH 488/561] =?UTF-8?q?=E2=9C=A8=20Describe=20the=20robot=20ph?= =?UTF-8?q?ysically=20and=20design=20the=20turns=20from=20it?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The robot model holds what can be measured on the robot and on the maze: masses, dimensions, friction, the constants of the drive train, the mounting of the wall sensors and the noise of every sensor. Limits and gains are derived from it instead of being tuned, so another robot reuses the navigation by describing itself, and a wrong number has a unit and a plausible range to be checked against. The turn table is the first consumer. A slalom turn is two clothoids around an arc, and with the lateral and the angular accelerations both bounded by the tires the length of the clothoids is a property of the chassis alone, which leaves the size of the turn as the only thing to choose. Each turn is made as wide as it can be while it fits between its two nodes of the planning lattice and keeps the outline of the robot a safety margin away from every post and from every wall that might exist, measured on the real path with the Fresnel series rather than on an equivalent arc. Only the path is stored, so a turn can be driven at any speed by scaling its angular speed and acceleration. The table is a constant expression: it costs under a second per translation unit, and a turn that does not fit is a static_assert away from failing the build. clang needs its limit of evaluation steps raised for it, which the lint script now does. The lattice is where turns live for the planner: the midpoints of the walls, crossed with eight headings, which is where every slalom turn starts and ends. --- cmake/templates/run_clang_tidy.sh.in | 3 + micras_nav/include/micras/nav/lattice.hpp | 390 +++++++++ micras_nav/include/micras/nav/robot_model.hpp | 207 +++++ micras_nav/include/micras/nav/turn_table.hpp | 793 ++++++++++++++++++ 4 files changed, 1393 insertions(+) create mode 100644 micras_nav/include/micras/nav/lattice.hpp create mode 100644 micras_nav/include/micras/nav/robot_model.hpp create mode 100644 micras_nav/include/micras/nav/turn_table.hpp diff --git a/cmake/templates/run_clang_tidy.sh.in b/cmake/templates/run_clang_tidy.sh.in index 97a8b92..49e77b8 100755 --- a/cmake/templates/run_clang_tidy.sh.in +++ b/cmake/templates/run_clang_tidy.sh.in @@ -25,6 +25,8 @@ echo "Running clang-tidy on all source files..." # The filtered lines are the echoed clang-tidy invocations and the per-file diagnostic # counters, which include the warnings suppressed in the Cube headers and are always noise. # Real diagnostics name their own file, so nothing useful is hidden. +# The shape of every turn is computed when the firmware is compiled, which takes more evaluation +# steps than clang allows a constant expression by default. run-clang-tidy \ "${RUN_FLAGS[@]}" \ -quiet \ @@ -35,6 +37,7 @@ run-clang-tidy \ -extra-arg-before=--sysroot=@SYSROOT@/ \ -extra-arg-before=-I@CXX_INCLUDE_DIR@/ \ -extra-arg-before=-I@CXX_TRIPLE_INCLUDE_DIR@/ \ + -extra-arg=-fconstexpr-steps=1000000000 \ "$@" 2>&1 | grep -Ev '^(\[ *[0-9]+/[0-9]+\]|[0-9]+ (warning|error)s? generated\.|clang-tidy(-[0-9.]+)? )' exit "${PIPESTATUS[0]}" diff --git a/micras_nav/include/micras/nav/lattice.hpp b/micras_nav/include/micras/nav/lattice.hpp new file mode 100644 index 0000000..7628802 --- /dev/null +++ b/micras_nav/include/micras/nav/lattice.hpp @@ -0,0 +1,390 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_LATTICE_HPP +#define MICRAS_NAV_LATTICE_HPP + +#include +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Slalom turns the robot can execute, named by where they start and end. + * + * @details S is a straight along the grid and D a diagonal, followed by the angle turned. SS90S is + * the short turn that fits inside one cell, which is also the turn used while searching, and SS90L + * the long one that needs a cell of straight on both sides. + */ +enum class TurnId : uint8_t { + SS90S = 0, + SS90L = 1, + SS180 = 2, + SD45 = 3, + SD135 = 4, + DS45 = 5, + DS135 = 6, + DD90 = 7, + NUMBER_OF_TURNS = 8, +}; + +/** + * @brief Number of different turns. + */ +inline constexpr uint8_t number_of_turns{std::to_underlying(TurnId::NUMBER_OF_TURNS)}; + +/** + * @brief Side a turn is made to. + */ +enum class TurnSide : uint8_t { + LEFT = 0, + RIGHT = 1, +}; + +/** + * @brief Point of the lattice the routes are planned on, in units of half a cell. + * + * @details Both coordinates even is a post, both odd is the center of a cell, and one of each is + * the midpoint of a wall: a vertical wall when x is even and a horizontal one when y is even. Every + * slalom turn starts and ends at the midpoint of a wall, which is why those are the nodes. + */ +struct LatticePoint { + /** + * @brief Check if the point is the midpoint of a wall that runs along y. + * + * @return True if the point is on a vertical wall. + */ + constexpr bool on_vertical_wall() const { return this->x % 2 == 0 and this->y % 2 != 0; } + + /** + * @brief Check if the point is the midpoint of a wall that runs along x. + * + * @return True if the point is on a horizontal wall. + */ + constexpr bool on_horizontal_wall() const { return this->x % 2 != 0 and this->y % 2 == 0; } + + /** + * @brief Add two points. + * + * @param other The point to add. + * @return The sum of the points. + */ + constexpr LatticePoint operator+(const LatticePoint& other) const { + return {.x = static_cast(this->x + other.x), .y = static_cast(this->y + other.y)}; + } + + /** + * @brief Compare two points for equality. + * + * @param other The other point to compare. + * @return True if the points are equal, false otherwise. + */ + constexpr bool operator==(const LatticePoint& other) const = default; + + int8_t x; + int8_t y; +}; + +/** + * @brief Node of the lattice: the midpoint of a wall, crossed with one of eight headings. + * + * @note The heading counts steps of 45 degrees from the positive x axis, counterclockwise, so the + * even ones run along the grid and the odd ones are diagonal. + */ +struct LatticePose { + /** + * @brief Number of headings. + */ + static constexpr uint8_t number_of_headings{8}; + + /** + * @brief Check if the heading is diagonal. + * + * @return True if the heading is diagonal, false if it runs along the grid. + */ + constexpr bool is_diagonal() const { return this->heading % 2 != 0; } + + /** + * @brief Check if the node exists, which takes a wall that the heading crosses. + * + * @return True if the node is the midpoint of a wall and the heading is not parallel to it. + */ + constexpr bool is_valid() const { + if (this->point.on_vertical_wall()) { + return this->heading % 4 != 2; + } + + return this->point.on_horizontal_wall() and this->heading % 4 != 0; + } + + /** + * @brief Get the displacement of one step along the heading. + * + * @note A step goes from one wall to the next, so it is a whole cell along the grid and half of + * the diagonal of a cell along a diagonal. + * + * @return The displacement in lattice units. + */ + constexpr LatticePoint step() const { + constexpr std::array steps{{ + {.x = 2, .y = 0}, + {.x = 1, .y = 1}, + {.x = 0, .y = 2}, + {.x = -1, .y = 1}, + {.x = -2, .y = 0}, + {.x = -1, .y = -1}, + {.x = 0, .y = -2}, + {.x = 1, .y = -1}, + }}; + + return steps.at(this->heading); + } + + /** + * @brief Get the node one step ahead. + * + * @return The node after moving one step along the heading. + */ + constexpr LatticePose advanced() const { return {.point = this->point + this->step(), .heading = this->heading}; } + + /** + * @brief Get the heading as an angle. + * + * @return The heading in radians, in [0, 2*pi). + */ + constexpr float angle() const { return static_cast(this->heading) * std::numbers::pi_v / 4.0F; } + + /** + * @brief Get the side a diagonal node can turn to. + * + * @details A robot that crosses a wall diagonally is veering to one side of the perpendicular + * of that wall, and every turn that starts there continues to that side: the ones to the other + * side only become possible one step later, at the next wall. + * + * @return The side of the turns that can start at this node. + */ + constexpr TurnSide diagonal_turn_side() const { + const bool even_quadrant = (this->heading / 2) % 2 == 0; + return this->point.on_vertical_wall() == even_quadrant ? TurnSide::LEFT : TurnSide::RIGHT; + } + + /** + * @brief Get the wall the node is on, as a side of a cell. + * + * @note The cell is the one behind the wall, except for the walls of the left and bottom + * borders, which only have a cell ahead. + * + * @return The cell and the side of it where the wall is. + */ + constexpr GridPose wall() const { + if (this->point.on_vertical_wall()) { + const auto row = static_cast((this->point.y - 1) / 2); + + if (this->point.x < 2) { + return {.position = {.x = 0, .y = row}, .orientation = Side::LEFT}; + } + + return { + .position = {.x = static_cast(this->point.x / 2 - 1), .y = row}, .orientation = Side::RIGHT + }; + } + + const auto column = static_cast((this->point.x - 1) / 2); + + if (this->point.y < 2) { + return {.position = {.x = column, .y = 0}, .orientation = Side::DOWN}; + } + + return {.position = {.x = column, .y = static_cast(this->point.y / 2 - 1)}, .orientation = Side::UP}; + } + + /** + * @brief Get the cell the robot is entering when it crosses the node. + * + * @note The coordinates are signed so that a node on the border, heading out, gives a cell + * outside of the maze instead of wrapping around. + * + * @return The coordinates of the cell ahead of the wall. + */ + constexpr LatticePoint cell_ahead() const { + const LatticePoint step = this->step(); + + if (this->point.on_vertical_wall()) { + return { + .x = static_cast(this->point.x / 2 - (step.x > 0 ? 0 : 1)), + .y = static_cast((this->point.y - 1) / 2), + }; + } + + return { + .x = static_cast((this->point.x - 1) / 2), + .y = static_cast(this->point.y / 2 - (step.y > 0 ? 0 : 1)), + }; + } + + /** + * @brief Get the pose of the node in the maze frame. + * + * @param cell_size The size of the cells in meters. + * @return The pose in meters and radians. + */ + constexpr Pose to_pose(float cell_size) const { + return { + .position = + {.x = static_cast(this->point.x) * cell_size / 2.0F, + .y = static_cast(this->point.y) * cell_size / 2.0F}, + .orientation = this->angle(), + }; + } + + /** + * @brief Compare two nodes for equality. + * + * @param other The other node to compare. + * @return True if the nodes are equal, false otherwise. + */ + constexpr bool operator==(const LatticePose& other) const = default; + + LatticePoint point; + uint8_t heading; +}; + +/** + * @brief Place of a turn in the lattice, for the turn to the left in its canonical frame. + * + * @details The canonical frame has the entry node at the origin, on a vertical wall, heading along + * +x for the turns that start on a straight and along the diagonal between +x and +y for the ones + * that start on a diagonal. The gates are the walls crossed between the entry and the exit nodes, + * in the order they are crossed. + */ +struct TurnPrimitive { + /** + * @brief Largest number of walls a turn crosses between its entry and exit nodes. + */ + static constexpr uint8_t max_gates{2}; + + bool diagonal_entry; + uint8_t rotation; + LatticePoint exit; + std::array gates; + uint8_t number_of_gates; +}; + +/** + * @brief Place of every turn in the lattice, indexed by TurnId. + */ +inline constexpr std::array turn_primitives{{ + {.diagonal_entry = false, .rotation = 2, .exit = {.x = 1, .y = 1}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = false, + .rotation = 2, + .exit = {.x = 3, .y = 3}, + .gates = {{{.x = 2, .y = 0}, {.x = 3, .y = 1}}}, + .number_of_gates = 2}, + {.diagonal_entry = false, + .rotation = 4, + .exit = {.x = 0, .y = 2}, + .gates = {{{.x = 1, .y = 1}, {}}}, + .number_of_gates = 1}, + {.diagonal_entry = false, + .rotation = 1, + .exit = {.x = 3, .y = 1}, + .gates = {{{.x = 2, .y = 0}, {}}}, + .number_of_gates = 1}, + {.diagonal_entry = false, + .rotation = 3, + .exit = {.x = 2, .y = 2}, + .gates = {{{.x = 2, .y = 0}, {.x = 3, .y = 1}}}, + .number_of_gates = 2}, + {.diagonal_entry = true, + .rotation = 1, + .exit = {.x = 1, .y = 3}, + .gates = {{{.x = 1, .y = 1}, {}}}, + .number_of_gates = 1}, + {.diagonal_entry = true, + .rotation = 3, + .exit = {.x = -2, .y = 2}, + .gates = {{{.x = 1, .y = 1}, {.x = 0, .y = 2}}}, + .number_of_gates = 2}, + {.diagonal_entry = true, + .rotation = 2, + .exit = {.x = 0, .y = 2}, + .gates = {{{.x = 1, .y = 1}, {}}}, + .number_of_gates = 1}, +}}; + +/** + * @brief Get the place of a turn in the lattice. + * + * @param turn The turn. + * @return The primitive of the turn, for the turn to the left in its canonical frame. + */ +constexpr const TurnPrimitive& get_primitive(TurnId turn) { + return turn_primitives.at(std::to_underlying(turn)); +} + +/** + * @brief Move a displacement from the canonical frame of a turn to the frame of an entry node. + * + * @param canonical The displacement in the canonical frame of the turn to the left. + * @param diagonal_entry Whether the canonical heading is diagonal. + * @param heading The heading of the entry node. + * @param side The side the turn is made to. + * @return The displacement in the lattice, relative to the entry node. + */ +constexpr LatticePoint + from_canonical(const LatticePoint& canonical, bool diagonal_entry, uint8_t heading, TurnSide side) { + LatticePoint point = canonical; + + if (side == TurnSide::RIGHT) { + point = diagonal_entry ? LatticePoint{.x = canonical.y, .y = canonical.x} : + LatticePoint{.x = canonical.x, .y = static_cast(-canonical.y)}; + } + + for (uint8_t quarter = 0; quarter < heading / 2; quarter++) { + point = {.x = static_cast(-point.y), .y = point.x}; + } + + return point; +} + +/** + * @brief Get the node where a turn ends. + * + * @param entry The node where the turn starts. + * @param turn The turn. + * @param side The side the turn is made to. + * @return The exit node of the turn. + */ +constexpr LatticePose get_turn_exit(const LatticePose& entry, TurnId turn, TurnSide side) { + const TurnPrimitive& primitive = get_primitive(turn); + const uint8_t rotation = + side == TurnSide::LEFT ? primitive.rotation : LatticePose::number_of_headings - primitive.rotation; + + return { + .point = entry.point + from_canonical(primitive.exit, primitive.diagonal_entry, entry.heading, side), + .heading = static_cast((entry.heading + rotation) % LatticePose::number_of_headings), + }; +} + +/** + * @brief Get one of the walls a turn crosses between its entry and exit nodes. + * + * @param entry The node where the turn starts. + * @param turn The turn. + * @param side The side the turn is made to. + * @param index The index of the gate, in the order the gates are crossed. + * @return The midpoint of the wall. + */ +constexpr LatticePoint get_turn_gate(const LatticePose& entry, TurnId turn, TurnSide side, uint8_t index) { + const TurnPrimitive& primitive = get_primitive(turn); + + return entry.point + from_canonical(primitive.gates.at(index), primitive.diagonal_entry, entry.heading, side); +} +} // namespace micras::nav + +#endif // MICRAS_NAV_LATTICE_HPP diff --git a/micras_nav/include/micras/nav/robot_model.hpp b/micras_nav/include/micras/nav/robot_model.hpp new file mode 100644 index 0000000..db6fd61 --- /dev/null +++ b/micras_nav/include/micras/nav/robot_model.hpp @@ -0,0 +1,207 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_ROBOT_MODEL_HPP +#define MICRAS_NAV_ROBOT_MODEL_HPP + +#include +#include + +#include "micras/core/vector.hpp" + +namespace micras::nav { +/** + * @brief Number of wall sensors the navigation is written for. + */ +inline constexpr uint8_t number_of_wall_sensors{4}; + +/** + * @brief Physical description of a robot and of the maze it runs in. + * + * @details Every limit, gain and geometric window of the navigation is derived from these constants, + * so tuning the robot is a matter of measuring them. All of them have a unit and a plausible range, + * which makes a wrong one visible, and another robot reuses the navigation by describing itself here. + * + * @note The body frame has its origin at the midpoint of the axle, x forward and y to the left. + */ +struct RobotModel { + /** + * @brief Standard acceleration of gravity, in m/s^2. + */ + static constexpr float gravity{9.80665F}; + + /** + * @brief Dimensions of the maze, in meters. + */ + struct Maze { + float cell_size; + float wall_thickness; + }; + + /** + * @brief Mass properties and outline of the robot. + * + * @note The outline is the rectangle that encloses everything that can touch a wall, given by + * its half width and by how far it extends ahead of and behind the axle. + */ + struct Chassis { + float mass; + float yaw_inertia; + float wheel_radius; + float track_width; + float half_width; + float front_length; + float rear_length; + }; + + /** + * @brief What limits the force the tires can transmit. + * + * @note The fan downforce is the extra normal force, in newtons, with the fan at the speed it + * is run at. + */ + struct Traction { + float friction_coefficient; + float fan_downforce; + }; + + /** + * @brief Electrical and mechanical constants of one side of the drive train. + * + * @note The torque constant, in N*m/A, is numerically equal to the back EMF constant in + * V*s/rad. The gear ratio is motor turns per wheel turn. The static friction voltage is what it + * takes to keep one side turning slowly, and the supply voltage is what a command of 100 % + * applies to a motor. + */ + struct Drive { + float torque_constant; + float resistance; + float gear_ratio; + float supply_voltage; + float static_friction_voltage; + }; + + /** + * @brief Mounting of one wall sensor. + * + * @note The angle of the optical axis is measured from the forward direction, positive to the + * left. The half angle is that of the cone the emitter illuminates, plus the mounting tolerance. + */ + struct WallSensor { + core::Vector position; + float angle; + float half_angle; + }; + + /** + * @brief Noise of the sensors, which is what the pose estimator weighs them by. + * + * @note The gyroscope noise is its rate noise density, in rad/s per square root of hertz, which + * unlike the deviation of a sample does not depend on how often it is sampled. Its bias walk + * is how much the bias may drift per square root of second. The wheel angle noise is the + * resolution of the encoders, in radians of wheel. The slip allowances are the + * standard deviation of the traveled distance and of the sideways motion per square root of + * meter traveled, the first of them growing with the acceleration. The range noise of a wall + * sensor is a constant part plus a part proportional to the range, for a reading that shares + * its noise with no other, which is for the estimator to account for when the readings come + * faster than their filter lets them change. + */ + struct Noise { + float gyroscope; + float gyroscope_bias_walk; + float wheel_angle; + float longitudinal_slip; + float longitudinal_slip_per_acceleration; + float lateral_slip; + float wall_range; + float wall_range_per_meter; + }; + + /** + * @brief Get the largest acceleration the tires can transmit, in any direction. + * + * @param fan_on Whether the fan is adding downforce. + * @return The traction limited acceleration in m/s^2. + */ + constexpr float traction_acceleration(bool fan_on) const { + return this->traction.friction_coefficient * + (gravity + (fan_on ? this->traction.fan_downforce / this->chassis.mass : 0.0F)); + } + + /** + * @brief Get the largest angular acceleration the tires can transmit. + * + * @param fan_on Whether the fan is adding downforce. + * @return The traction limited angular acceleration in rad/s^2. + */ + constexpr float traction_angular_acceleration(bool fan_on) const { + return this->traction_acceleration(fan_on) * this->chassis.mass * this->chassis.track_width / + (2.0F * this->chassis.yaw_inertia); + } + + /** + * @brief Get the distance over which a turn ramps its curvature up. + * + * @details With the lateral and the angular accelerations both limited by the tires, the ratio + * between them is a property of the chassis alone, and it fixes the length of the clothoid that + * takes the robot from a straight into the tightest arc it can hold. It does not depend on the + * friction, on the fan or on the speed. + * + * @return The length of the curvature ramp in meters. + */ + constexpr float turn_ramp_length() const { + return 2.0F * this->chassis.yaw_inertia / (this->chassis.mass * this->chassis.track_width); + } + + /** + * @brief Get the voltage per unit of linear speed, applied to both motors. + * + * @return The speed constant in V*s/m. + */ + constexpr float speed_constant() const { + return this->drive.torque_constant * this->drive.gear_ratio / this->chassis.wheel_radius; + } + + /** + * @brief Get the voltage per unit of linear acceleration, applied to both motors. + * + * @return The acceleration constant in V*s^2/m. + */ + constexpr float acceleration_constant() const { + return this->chassis.wheel_radius * this->drive.resistance * this->chassis.mass / + (2.0F * this->drive.gear_ratio * this->drive.torque_constant); + } + + /** + * @brief Get the differential voltage per unit of angular speed. + * + * @return The angular speed constant in V*s/rad. + */ + constexpr float angular_speed_constant() const { return this->speed_constant() * this->chassis.track_width / 2.0F; } + + /** + * @brief Get the differential voltage per unit of angular acceleration. + * + * @return The angular acceleration constant in V*s^2/rad. + */ + constexpr float angular_acceleration_constant() const { + return this->chassis.yaw_inertia * this->chassis.wheel_radius * this->drive.resistance / + (this->drive.gear_ratio * this->drive.torque_constant * this->chassis.track_width); + } + + Maze maze; + Chassis chassis; + Traction traction; + Drive drive; + std::array wall_sensors; + Noise noise; + + /** + * @brief Factor that corrects the sensitivity of the gyroscope, from the scale calibration. + */ + float gyroscope_scale; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_ROBOT_MODEL_HPP diff --git a/micras_nav/include/micras/nav/turn_table.hpp b/micras_nav/include/micras/nav/turn_table.hpp new file mode 100644 index 0000000..063541f --- /dev/null +++ b/micras_nav/include/micras/nav/turn_table.hpp @@ -0,0 +1,793 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_TURN_TABLE_HPP +#define MICRAS_NAV_TURN_TABLE_HPP + +#include +#include +#include +#include +#include +#include + +#include "micras/core/math.hpp" +#include "micras/nav/lattice.hpp" +#include "micras/nav/robot_model.hpp" + +namespace micras::nav { +/** + * @brief Geometry of a slalom turn. + * + * @details A turn is a clothoid that ramps the curvature up, an arc at the peak curvature and the + * mirrored clothoid that ramps it down, which is the path of a robot that holds its linear speed + * while its angular speed follows a trapezoid. Only the path is stored: the speed it is driven at is + * chosen when a run is planned, and scaling the angular speed with it and the angular acceleration + * with its square keeps the robot on this same path at any speed. + * + * @note The straights before and after the curve place it between its entry and exit nodes. All + * lengths are in meters and the angle, in radians, is that of the turn to the left. + */ +struct TurnShape { + /** + * @brief Point of the curve, in the frame of its start, for the turn to the left. + * + * @tparam T Floating point type. + */ + template + struct Sample { + T x; + T y; + T heading; + T curvature; + T sharpness; + }; + + /** + * @brief Evaluate the curve at a distance from its start. + * + * @tparam T Floating point type. + * @param distance Distance traveled along the curve, clamped to its length. + * @return The point of the curve, with the curvature and its rate of change there. + */ + template + constexpr Sample sample(T distance) const { + const T ramp = static_cast(this->ramp_length); + const T arc = static_cast(this->arc_length); + const T rate = static_cast(this->sharpness); + const T peak = static_cast(this->curvature); + const T total = T{2} * ramp + arc; + + if (distance < T{0}) { + distance = T{0}; + } else if (distance > total) { + distance = total; + } + + if (distance <= ramp) { + const auto point = core::math::clothoid(distance, rate); + + return { + .x = point.x, + .y = point.y, + .heading = rate * distance * distance / T{2}, + .curvature = rate * distance, + .sharpness = rate, + }; + } + + const auto ramp_end = core::math::clothoid(ramp, rate); + const T ramp_heading = rate * ramp * ramp / T{2}; + + if (distance <= ramp + arc) { + const T heading = ramp_heading + peak * (distance - ramp); + + return { + .x = ramp_end.x + (core::math::sin(heading) - core::math::sin(ramp_heading)) / peak, + .y = ramp_end.y - (core::math::cos(heading) - core::math::cos(ramp_heading)) / peak, + .heading = heading, + .curvature = peak, + .sharpness = T{0}, + }; + } + + const T angle = T{2} * ramp_heading + peak * arc; + const T cosine = core::math::cos(angle); + const T sine = core::math::sin(angle); + + const T arc_heading = ramp_heading + peak * arc; + const T arc_end_x = arc > T{0} ? + ramp_end.x + (core::math::sin(arc_heading) - core::math::sin(ramp_heading)) / peak : + ramp_end.x; + const T arc_end_y = arc > T{0} ? + ramp_end.y - (core::math::cos(arc_heading) - core::math::cos(ramp_heading)) / peak : + ramp_end.y; + + const T end_x = arc_end_x + cosine * ramp_end.x + sine * ramp_end.y; + const T end_y = arc_end_y + sine * ramp_end.x - cosine * ramp_end.y; + + const T remaining = total - distance; + const auto mirrored = core::math::clothoid(remaining, rate); + + return { + .x = end_x - (cosine * mirrored.x + sine * mirrored.y), + .y = end_y - (sine * mirrored.x - cosine * mirrored.y), + .heading = angle - rate * remaining * remaining / T{2}, + .curvature = rate * remaining, + .sharpness = -rate, + }; + } + + /** + * @brief Get the length of the curve. + * + * @return The length of the two ramps and the arc in meters. + */ + constexpr float length() const { return 2.0F * this->ramp_length + this->arc_length; } + + /** + * @brief Get the length of the whole turn, from its entry node to its exit node. + * + * @return The length of the curve and of the straights around it in meters. + */ + constexpr float total_length() const { return this->pre + this->length() + this->post; } + + float angle{}; + float curvature{}; + float sharpness{}; + float ramp_length{}; + float arc_length{}; + float pre{}; + float post{}; + + /** + * @brief Distance from the entry node at which each wall between the nodes is crossed. + */ + std::array gates{}; + + /** + * @brief Whether a curve that fits between the nodes with the required clearance exists. + */ + bool valid{}; +}; + +/** + * @brief Shape of every turn, computed from the geometry of the maze and of the robot. + * + * @details Each turn is made as wide as it can be while it still fits between its two nodes and + * keeps the outline of the robot a safety margin away from every post and from every wall that is + * not known to be absent, since a wider turn is a faster one. The clearance is measured on the path + * itself, rather than on a circular arc of equivalent displacement. + * + * @note Everything here is a constant expression, so a table is normally built when the firmware is + * compiled, where a turn that does not fit can be rejected by a static_assert on is_valid(). + */ +class TurnTable { +public: + /** + * @brief Compute the shape of every turn. + * + * @param model Physical description of the robot and of the maze. + * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + */ + constexpr TurnTable(const RobotModel& model, float margin) { + const Designer designer{model, margin}; + + for (uint8_t i = 0; i < number_of_turns; i++) { + this->shapes.at(i) = designer.design(static_cast(i)); + } + } + + /** + * @brief Get the shape of a turn. + * + * @param turn The turn. + * @return The shape of the turn. + */ + constexpr const TurnShape& get(TurnId turn) const { return this->shapes.at(std::to_underlying(turn)); } + + /** + * @brief Check if every turn fits. + * + * @return True if a shape was found for every turn, false otherwise. + */ + constexpr bool is_valid() const { + return std::ranges::all_of(this->shapes, [](const TurnShape& shape) { return shape.valid; }); + } + +private: + /** + * @brief Solver for the shape of one turn, in double precision. + */ + class Designer { + public: + /** + * @brief Construct a new Designer object. + * + * @param model Physical description of the robot and of the maze. + * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + */ + constexpr Designer(const RobotModel& model, float margin) : + half_cell{static_cast(model.maze.cell_size) / 2.0}, + half_wall{static_cast(model.maze.wall_thickness) / 2.0}, + ramp_length{static_cast(model.turn_ramp_length())}, + half_width{static_cast(model.chassis.half_width)}, + front_length{static_cast(model.chassis.front_length)}, + rear_length{static_cast(model.chassis.rear_length)}, + margin{static_cast(margin)} { } + + /** + * @brief Find the widest shape of a turn that fits. + * + * @param turn The turn. + * @return The shape, with its valid flag cleared if none fits. + */ + constexpr TurnShape design(TurnId turn) const { + const TurnPrimitive& primitive = get_primitive(turn); + const Layout layout = this->get_layout(primitive); + + if (primitive.rotation == 4) { + return this->design_half_turn(primitive, layout); + } + + double infeasible = 1.0 / (16.0 * this->half_cell); + double feasible = 0.0; + double curvature = infeasible; + + for (uint8_t i = 0; i < coarse_steps; i++) { + if (this->fits(this->place(layout, curvature), layout)) { + feasible = curvature; + break; + } + + infeasible = curvature; + curvature *= coarse_ratio; + } + + if (feasible == 0.0) { + return {}; + } + + for (uint8_t i = 0; i < 14; i++) { + const double middle = (infeasible + feasible) / 2.0; + + if (this->fits(this->place(layout, middle), layout)) { + feasible = middle; + } else { + infeasible = middle; + } + } + + return finish(this->place(layout, feasible), layout); + } + + private: + /** + * @brief Number of sizes tried, from the widest down, before refining the first that fits. + */ + static constexpr uint8_t coarse_steps{48}; + + /** + * @brief Number of lengths tried for the straights around a turn of half a revolution. + */ + static constexpr uint8_t straight_steps{40}; + + /** + * @brief Ratio between the curvatures of two consecutive sizes tried. + */ + static constexpr double coarse_ratio{1.12}; + + /** + * @brief Largest number of obstacles considered around a turn. + */ + static constexpr uint8_t max_obstacles{48}; + + /** + * @brief Number of points of the curve where the clearance is checked. + */ + static constexpr uint8_t clearance_samples{32}; + + /** + * @brief Axis aligned rectangle in the canonical frame of a turn, in meters. + */ + struct Rectangle { + double center_x; + double center_y; + double half_x; + double half_y; + }; + + /** + * @brief Where a turn sits in the canonical frame, in meters and radians. + */ + struct Layout { + double entry_heading{}; + double angle{}; + double exit_x{}; + double exit_y{}; + double tangent_in{}; + double tangent_out{}; + std::array obstacles{}; + uint8_t number_of_obstacles{}; + std::array gates{}; + uint8_t number_of_gates{}; + }; + + /** + * @brief Candidate shape, in double precision. + */ + struct Candidate { + double curvature{}; + double sharpness{}; + double ramp{}; + double arc{}; + double pre{}; + double post{}; + }; + + /** + * @brief Pose of the robot in the canonical frame. + */ + struct Placement { + double x; + double y; + double heading; + }; + + /** + * @brief Build the curve of a candidate as a shape, to evaluate it. + * + * @param candidate The candidate. + * @param angle The angle of the turn. + * @return The shape with the fields of the curve filled in. + */ + static constexpr TurnShape to_shape(const Candidate& candidate, double angle) { + TurnShape shape{}; + shape.angle = static_cast(angle); + shape.curvature = static_cast(candidate.curvature); + shape.sharpness = static_cast(candidate.sharpness); + shape.ramp_length = static_cast(candidate.ramp); + shape.arc_length = static_cast(candidate.arc); + shape.pre = static_cast(candidate.pre); + shape.post = static_cast(candidate.post); + return shape; + } + + /** + * @brief Build the curve that turns a given angle with a given peak curvature. + * + * @note When the ramps alone would turn more than the angle, they are shortened and the arc + * disappears, which keeps the rate of change of the curvature. + * + * @param angle The angle of the turn. + * @param curvature The peak curvature requested. + * @return The candidate, without the straights. + */ + constexpr Candidate make_curve(double angle, double curvature) const { + Candidate candidate{}; + candidate.sharpness = curvature / this->ramp_length; + candidate.ramp = this->ramp_length; + candidate.curvature = curvature; + + if (curvature * this->ramp_length > angle) { + candidate.ramp = core::math::sqrt(angle / candidate.sharpness); + candidate.curvature = candidate.sharpness * candidate.ramp; + } + + candidate.arc = (angle - candidate.curvature * candidate.ramp) / candidate.curvature; + + return candidate; + } + + /** + * @brief Get the pose of the robot at a distance from the entry node. + * + * @param candidate The candidate shape. + * @param layout The place of the turn. + * @param distance Distance traveled from the entry node. + * @return The pose in the canonical frame. + */ + static constexpr Placement get_placement(const Candidate& candidate, const Layout& layout, double distance) { + const double cosine = core::math::cos(layout.entry_heading); + const double sine = core::math::sin(layout.entry_heading); + const double length = 2.0 * candidate.ramp + candidate.arc; + + double local_x = distance; + double local_y = 0.0; + double heading = 0.0; + + if (distance > candidate.pre) { + const auto sample = to_shape(candidate, layout.angle).template sample(distance - candidate.pre); + + local_x = candidate.pre + sample.x; + local_y = sample.y; + heading = sample.heading; + + if (distance > candidate.pre + length) { + const double extra = distance - candidate.pre - length; + local_x += extra * core::math::cos(layout.angle); + local_y += extra * core::math::sin(layout.angle); + } + } + + return { + .x = cosine * local_x - sine * local_y, + .y = sine * local_x + cosine * local_y, + .heading = layout.entry_heading + heading, + }; + } + + /** + * @brief Collect the geometry of a turn in its canonical frame. + * + * @param primitive The place of the turn in the lattice. + * @return The layout, with every post and every wall that may exist around the turn. + */ + constexpr Layout get_layout(const TurnPrimitive& primitive) const { + Layout layout{}; + layout.entry_heading = primitive.diagonal_entry ? std::numbers::pi / 4.0 : 0.0; + layout.angle = primitive.rotation * std::numbers::pi / 4.0; + layout.exit_x = primitive.exit.x * this->half_cell; + layout.exit_y = primitive.exit.y * this->half_cell; + layout.number_of_gates = primitive.number_of_gates; + + const double cosine = core::math::cos(layout.entry_heading); + const double sine = core::math::sin(layout.entry_heading); + const double forward = cosine * layout.exit_x + sine * layout.exit_y; + const double lateral = -sine * layout.exit_x + cosine * layout.exit_y; + + if (primitive.rotation != 4) { + layout.tangent_out = lateral / core::math::sin(layout.angle); + layout.tangent_in = forward - layout.tangent_out * core::math::cos(layout.angle); + } + + for (uint8_t i = 0; i < primitive.number_of_gates; i++) { + layout.gates.at(i) = this->get_wall(primitive.gates.at(i)); + } + + const double reach = 3.0 * this->half_cell; + const double low_x = (layout.exit_x < 0.0 ? layout.exit_x : 0.0) - reach; + const double high_x = (layout.exit_x > 0.0 ? layout.exit_x : 0.0) + reach; + const double low_y = (layout.exit_y < 0.0 ? layout.exit_y : 0.0) - reach; + const double high_y = (layout.exit_y > 0.0 ? layout.exit_y : 0.0) + reach; + + for (int8_t column = -6; column <= 8; column++) { + for (int8_t row = -7; row <= 7; row++) { + const LatticePoint point{.x = column, .y = row}; + + if (column % 2 != 0 and row % 2 == 0) { + continue; + } + + if (is_open(primitive, point) or layout.number_of_obstacles == max_obstacles) { + continue; + } + + const Rectangle obstacle = this->get_wall(point); + + if (obstacle.center_x < low_x or obstacle.center_x > high_x or obstacle.center_y < low_y or + obstacle.center_y > high_y) { + continue; + } + + layout.obstacles.at(layout.number_of_obstacles) = obstacle; + layout.number_of_obstacles++; + } + } + + return layout; + } + + /** + * @brief Check if a point of the canonical frame is a wall the turn needs to be absent. + * + * @note In the canonical frame the entry node is on a vertical wall, so the posts are the + * points with an even x and an odd y, the vertical walls have both coordinates even and + * the horizontal ones have both odd. + * + * @param primitive The place of the turn in the lattice. + * @param point The point, relative to the entry node. + * @return True if the robot crosses the wall at that point. + */ + static constexpr bool is_open(const TurnPrimitive& primitive, const LatticePoint& point) { + if (point == LatticePoint{.x = 0, .y = 0} or point == primitive.exit) { + return true; + } + + for (uint8_t i = 0; i < primitive.number_of_gates; i++) { + if (point == primitive.gates.at(i)) { + return true; + } + } + + return false; + } + + /** + * @brief Get the rectangle occupied by a post or by a wall. + * + * @param point The post or the midpoint of the wall, relative to the entry node. + * @return The rectangle in the canonical frame. + */ + constexpr Rectangle get_wall(const LatticePoint& point) const { + const bool is_post = point.x % 2 == 0 and point.y % 2 != 0; + const bool is_vertical = point.x % 2 == 0; + + return { + .center_x = point.x * this->half_cell, + .center_y = point.y * this->half_cell, + .half_x = (is_post or is_vertical) ? this->half_wall : this->half_cell, + .half_y = (is_post or not is_vertical) ? this->half_wall : this->half_cell, + }; + } + + /** + * @brief Place a curve between the nodes of a turn. + * + * @param layout The place of the turn. + * @param curvature The peak curvature requested. + * @return The candidate, whose straights are negative when the curve does not fit. + */ + constexpr Candidate place(const Layout& layout, double curvature) const { + Candidate candidate = this->make_curve(layout.angle, curvature); + const auto end = to_shape(candidate, layout.angle).template sample(1.0e3); + const double tangent = end.y / core::math::sin(layout.angle); + + candidate.pre = layout.tangent_in - tangent; + candidate.post = layout.tangent_out - tangent; + + return candidate; + } + + /** + * @brief Check if a candidate fits between its nodes and clears every obstacle. + * + * @param candidate The candidate shape. + * @param layout The place of the turn. + * @return True if the candidate can be driven. + */ + constexpr bool fits(const Candidate& candidate, const Layout& layout) const { + if (candidate.pre < 0.0 or candidate.post < 0.0) { + return false; + } + + const double length = 2.0 * candidate.ramp + candidate.arc; + const double longest = this->front_length > this->rear_length ? this->front_length : this->rear_length; + const double reach = + core::math::sqrt(this->half_width * this->half_width + longest * longest) + this->margin; + + for (uint8_t i = 0; i <= clearance_samples + 1; i++) { + double distance = candidate.pre + length * (i - 1) / (clearance_samples - 1); + + if (i == 0) { + distance = 0.0; + } else if (i == clearance_samples + 1) { + distance = candidate.pre + length + candidate.post; + } + + const Placement placement = get_placement(candidate, layout, distance); + + for (uint8_t j = 0; j < layout.number_of_obstacles; j++) { + const Rectangle& obstacle = layout.obstacles.at(j); + + if (core::math::abs(obstacle.center_x - placement.x) > reach + obstacle.half_x or + core::math::abs(obstacle.center_y - placement.y) > reach + obstacle.half_y) { + continue; + } + + if (this->touches(placement, obstacle)) { + return false; + } + } + } + + return true; + } + + /** + * @brief Check if a point is closer than the margin to a rectangle centered at the origin. + * + * @param x The x coordinate of the point in the frame of the rectangle. + * @param y The y coordinate of the point in the frame of the rectangle. + * @param low_x The lowest x coordinate of the rectangle. + * @param high_x The highest x coordinate of the rectangle. + * @param half_y Half of the size of the rectangle along y. + * @return True if the point is within the margin of the rectangle. + */ + constexpr bool is_near(double x, double y, double low_x, double high_x, double half_y) const { + double excess_x = 0.0; + + if (x > high_x) { + excess_x = x - high_x; + } else if (x < low_x) { + excess_x = low_x - x; + } + + const double excess_y = core::math::abs(y) > half_y ? core::math::abs(y) - half_y : 0.0; + + return excess_x * excess_x + excess_y * excess_y < this->margin * this->margin; + } + + /** + * @brief Check if the outline of the robot comes within the margin of an obstacle. + * + * @note Both shapes are convex and none of them can be inside the other, so the smallest + * distance between them is always measured from a corner of one of the two. + * + * @param placement The pose of the robot. + * @param obstacle The obstacle. + * @return True if the robot is closer to the obstacle than the margin. + */ + constexpr bool touches(const Placement& placement, const Rectangle& obstacle) const { + const double cosine = core::math::cos(placement.heading); + const double sine = core::math::sin(placement.heading); + + for (uint8_t corner = 0; corner < 4; corner++) { + const double along = corner < 2 ? this->front_length : -this->rear_length; + const double across = corner % 2 == 0 ? this->half_width : -this->half_width; + + const double corner_x = placement.x + cosine * along - sine * across - obstacle.center_x; + const double corner_y = placement.y + sine * along + cosine * across - obstacle.center_y; + + if (this->is_near(corner_x, corner_y, -obstacle.half_x, obstacle.half_x, obstacle.half_y)) { + return true; + } + + const double offset_x = + obstacle.center_x + (corner < 2 ? obstacle.half_x : -obstacle.half_x) - placement.x; + const double offset_y = + obstacle.center_y + (corner % 2 == 0 ? obstacle.half_y : -obstacle.half_y) - placement.y; + + const double body_x = cosine * offset_x + sine * offset_y; + const double body_y = -sine * offset_x + cosine * offset_y; + + if (this->is_near(body_x, body_y, -this->rear_length, this->front_length, this->half_width)) { + return true; + } + } + + return false; + } + + /** + * @brief Find the shape of a turn of half a revolution. + * + * @note The exit is parallel to the entry, so the size of the curve is fixed by the distance + * between them and what is left to choose is how far into the cell it is made. + * + * @param primitive The place of the turn in the lattice. + * @param layout The place of the turn. + * @return The shape, with its valid flag cleared if none fits. + */ + constexpr TurnShape design_half_turn(const TurnPrimitive& primitive, const Layout& layout) const { + const double lateral = primitive.exit.y * this->half_cell; + + double tight = 1.0 / (0.1 * this->half_cell); + double wide = 1.0 / (16.0 * this->half_cell); + + for (uint8_t i = 0; i < 48; i++) { + const double middle = (tight + wide) / 2.0; + const Candidate candidate = this->make_curve(layout.angle, middle); + const auto end = to_shape(candidate, layout.angle).template sample(1.0e3); + + if (end.y > lateral) { + wide = middle; + } else { + tight = middle; + } + } + + Candidate candidate = this->make_curve(layout.angle, (tight + wide) / 2.0); + double blocked = -1.0; + double clear = -1.0; + + for (uint8_t i = 0; i < straight_steps; i++) { + const double straight = 2.0 * this->half_cell * i / straight_steps; + + candidate.pre = straight; + candidate.post = straight; + + if (this->fits(candidate, layout)) { + clear = straight; + break; + } + + blocked = straight; + } + + if (clear < 0.0) { + return {}; + } + + for (uint8_t i = 0; i < 10 and blocked >= 0.0; i++) { + const double middle = (blocked + clear) / 2.0; + candidate.pre = middle; + candidate.post = middle; + + if (this->fits(candidate, layout)) { + clear = middle; + } else { + blocked = middle; + } + } + + candidate.pre = clear; + candidate.post = clear; + + return finish(candidate, layout); + } + + /** + * @brief Turn a candidate into a shape, locating where it crosses each gate. + * + * @note A gate is looked for from the end of the turn backwards, since a turn that doubles + * back can touch the line of a wall before the place where it actually crosses it. + * + * @param candidate The candidate shape. + * @param layout The place of the turn. + * @return The shape. + */ + static constexpr TurnShape finish(const Candidate& candidate, const Layout& layout) { + TurnShape shape = to_shape(candidate, layout.angle); + shape.valid = true; + + const double total = candidate.pre + 2.0 * candidate.ramp + candidate.arc + candidate.post; + + for (uint8_t i = 0; i < layout.number_of_gates; i++) { + const Rectangle& gate = layout.gates.at(i); + const bool vertical = gate.half_x < gate.half_y; + + const auto side = [&](double distance) { + const Placement placement = get_placement(candidate, layout, distance); + return vertical ? placement.x - gate.center_x : placement.y - gate.center_y; + }; + + const double final_side = side(total); + double after = total; + double before = 0.0; + + for (uint8_t j = 1; j <= 64; j++) { + const double distance = total * (64 - j) / 64.0; + + if ((side(distance) > 0.0) != (final_side > 0.0)) { + before = distance; + break; + } + + after = distance; + } + + for (uint8_t j = 0; j < 30; j++) { + const double middle = (before + after) / 2.0; + + if ((side(middle) > 0.0) != (final_side > 0.0)) { + before = middle; + } else { + after = middle; + } + } + + shape.gates.at(i) = static_cast((before + after) / 2.0); + } + + return shape; + } + + double half_cell; + double half_wall; + double ramp_length; + double half_width; + double front_length; + double rear_length; + double margin; + }; + + /** + * @brief Shape of each turn, indexed by TurnId. + */ + std::array shapes{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_TURN_TABLE_HPP From 640cbf3e32b73bff4eb0968eead54bcf5f0c2100 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 03:23:52 -0300 Subject: [PATCH 489/561] =?UTF-8?q?=E2=9C=A8=20Generate=20the=20motion=20a?= =?UTF-8?q?s=20segments,=20one=20speed=20evaluator=20and=20a=20reference?= =?UTF-8?q?=20in=20time?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A motion is a plain value: a kind, a length, the speeds at its ends and the pose it nominally starts from. That pose comes from the route on the grid and never from a measurement, so the reference built from it does not move when the pose estimate is corrected, and the error left at the end of a segment shows up at the start of the next one instead of being forgotten. The speed profile is the single evaluator of the navigation. It accelerates as the limits allow, which are the tires up to the speed where the motors take over and the falling torque of a motor fed from a fixed voltage from there on, cruises and brakes at the last moment. The planner prices a straight with it, the velocity planner chains straights with it and the executor plays the very same object back, so the time a route is chosen by is the time it takes. The limits come from the robot model and from the profile of the run, whose utilization is the fraction of the available traction the run may ask for. The velocity planner chooses the speed of every junction with a pass from the end, for what can be braked from, and a pass from the start, for what can be reached. A turn is driven at one speed, so two turns too close together slow each other down. The executor turns the segments into a pose, a twist and their exact derivatives as functions of time alone, evaluating a turn from its closed form, and can play them in slow motion when the motors cannot follow, which keeps the robot on the same path. GridPoint::from_vector converted a possibly negative float to an unsigned type, which is undefined. It now floors and clamps. --- micras_nav/include/micras/nav/executor.hpp | 211 +++++++++++++++ micras_nav/include/micras/nav/grid_pose.hpp | 3 + .../include/micras/nav/motion_limits.hpp | 165 ++++++++++++ micras_nav/include/micras/nav/segment.hpp | 63 +++++ .../include/micras/nav/speed_profile.hpp | 179 ++++++++++++ micras_nav/include/micras/nav/state.hpp | 16 ++ .../include/micras/nav/velocity_planner.hpp | 58 ++++ micras_nav/src/executor.cpp | 221 +++++++++++++++ micras_nav/src/grid_pose.cpp | 10 +- micras_nav/src/motion_limits.cpp | 76 ++++++ micras_nav/src/speed_profile.cpp | 254 ++++++++++++++++++ micras_nav/src/state.cpp | 25 ++ micras_nav/src/velocity_planner.cpp | 108 ++++++++ 13 files changed, 1386 insertions(+), 3 deletions(-) create mode 100644 micras_nav/include/micras/nav/executor.hpp create mode 100644 micras_nav/include/micras/nav/motion_limits.hpp create mode 100644 micras_nav/include/micras/nav/segment.hpp create mode 100644 micras_nav/include/micras/nav/speed_profile.hpp create mode 100644 micras_nav/include/micras/nav/velocity_planner.hpp create mode 100644 micras_nav/src/executor.cpp create mode 100644 micras_nav/src/motion_limits.cpp create mode 100644 micras_nav/src/speed_profile.cpp create mode 100644 micras_nav/src/velocity_planner.cpp diff --git a/micras_nav/include/micras/nav/executor.hpp b/micras_nav/include/micras/nav/executor.hpp new file mode 100644 index 0000000..bda4571 --- /dev/null +++ b/micras_nav/include/micras/nav/executor.hpp @@ -0,0 +1,211 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_EXECUTOR_HPP +#define MICRAS_NAV_EXECUTOR_HPP + +#include +#include +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Playback of a route as a reference in time. + * + * @details The executor owns the clock of the segment in progress and turns it into a pose, a twist + * and an acceleration in the maze frame, by composing the nominal start of the segment with the + * motion inside it. The reference depends on time alone, never on where the robot is, so a + * correction of the pose estimate cannot make it jump, and the time a route takes is exactly the + * time the planner computed for it. + */ +class Executor { +public: + /** + * @brief Configuration struct for the executor. + * + * @note The tolerances tell when the robot has settled against a wall: the pose error and the + * speeds have to stay below them for the settle time. + */ + struct Config { + std::size_t capacity; + float settle_distance; + float settle_angle; + float settle_linear_speed; + float settle_angular_speed; + float settle_time; + }; + + /** + * @brief Construct a new Executor object. + * + * @param dynamics The physical limits of the robot, borrowed for the lifetime of the executor. + * @param config The configuration for the executor. + */ + Executor(const Dynamics& dynamics, const Config& config); + + /** + * @brief Drop every segment and stand still at a pose. + * + * @param pose The pose to hold. + * @param profile The profile of the run about to start. + */ + void reset(const Pose& pose, const RunProfile& profile); + + /** + * @brief Queue segments after the ones already queued. + * + * @note Nothing is allocated as long as the number of segments waiting stays within the + * capacity, which a route planned with the robot stopped may exceed and the few segments + * queued per cell while exploring never do. + * + * @param segments The segments, with their speeds already planned. + */ + void push(std::span segments); + + /** + * @brief Advance the clock and get the reference for this instant. + * + * @note A time scale below one plays the reference in slow motion: the clock advances by less + * than the time that passed and the speeds are scaled to match, so the path stays the same. + * + * @param elapsed_time Time since the last update, in seconds. + * @param time_scale Factor applied to the elapsed time, in (0, 1]. + * @param estimate The estimated state of the robot, which only tells when it has settled. + * @return The reference in the maze frame. + */ + Reference update(float elapsed_time, float time_scale, const State& estimate); + + /** + * @brief Check if there is nothing left to execute. + * + * @return True if every segment has ended. + */ + bool is_finished() const; + + /** + * @brief Check if the last segment queued ends within a time from now. + * + * @note Whoever feeds the executor cell by cell asks this every iteration, so as to decide the + * next move as late as possible and still hand it over before the reference runs out. + * + * @param horizon The time ahead to look, in seconds. + * @return True if everything that is queued ends within the horizon. + */ + bool is_ending(float horizon) const; + + /** + * @brief Replace everything after a point of the segment in progress by other segments. + * + * @note Used to stop short of a wall that was seen late. The segment in progress must be a + * straight, which is cut where the robot is, and the new segments start from there. + * + * @param segments The segments to execute instead, the first starting at the current speed. + */ + void divert(std::span segments); + + /** + * @brief Get the segment in progress. + * + * @return The segment, or a null pointer if every segment has ended. + */ + const Segment* get_current() const; + + /** + * @brief Get the last reference produced. + * + * @return The reference. + */ + const Reference& get_reference() const; + +private: + /** + * @brief Start the next segment, carrying over the time the previous one did not use. + */ + void start_next(); + + /** + * @brief Evaluate the segment in progress at the current clock. + * + * @return The reference in the maze frame. + */ + Reference evaluate() const; + + /** + * @brief Check if the robot has settled at the reference. + * + * @param estimate The estimated state of the robot. + * @param elapsed_time Time since the last update, in seconds. + * @return True if the errors and the speeds have been small for long enough. + */ + bool has_settled(const State& estimate, float elapsed_time); + + /** + * @brief Physical limits of the robot. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const Dynamics& dynamics; + + /** + * @brief Tolerances to consider the robot settled. + */ + Config config; + + /** + * @brief Profile of the run in progress. + */ + RunProfile run_profile{}; + + /** + * @brief Segments queued, the ones before the index being already done. + */ + std::vector segments; + + /** + * @brief Time each of the segments queued takes. + */ + std::vector durations; + + /** + * @brief Index of the segment in progress. + */ + std::size_t index{}; + + /** + * @brief Time the segments queued after the one in progress take. + */ + float queued_time{}; + + /** + * @brief Time since the start of the segment in progress. + */ + float clock{}; + + /** + * @brief Duration of the segment in progress. + */ + float duration{}; + + /** + * @brief Motion along the segment in progress, for the kinds that have one. + */ + SpeedProfile speed_profile; + + /** + * @brief Time the robot has been within the settling tolerances. + */ + float settled_time{}; + + /** + * @brief Last reference produced, which is held when there is nothing to execute. + */ + Reference reference{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_EXECUTOR_HPP diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index d13f15a..4dd2355 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -51,6 +51,9 @@ struct GridPoint { /** * @brief Convert the point to a grid point. * + * @note A point outside of the first quadrant is clamped to it, since a negative coordinate + * cannot be represented and converting one to an unsigned type is undefined. + * * @param point The point to convert. * @param cell_size The size of the grid cells. * @return The grid point corresponding to the point. diff --git a/micras_nav/include/micras/nav/motion_limits.hpp b/micras_nav/include/micras/nav/motion_limits.hpp new file mode 100644 index 0000000..3f9564d --- /dev/null +++ b/micras_nav/include/micras/nav/motion_limits.hpp @@ -0,0 +1,165 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MOTION_LIMITS_HPP +#define MICRAS_NAV_MOTION_LIMITS_HPP + +#include "micras/nav/robot_model.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +/** + * @brief How a run trades time for safety. + * + * @note The utilization is the fraction of the available traction the run may ask for, in (0, 1]. + * Whether the run is risky selects the turn table built with the smaller safety margin. The speed + * limit is what makes the search run slow: a fast run leaves it above what the robot can reach. + */ +struct RunProfile { + bool diagonal; + bool fan; + bool risky; + float utilization; + float max_speed; +}; + +/** + * @brief Limits of a motion along one axis, linear or angular. + * + * @details The acceleration is bounded by the tires up to the speed where the motors take over, and + * from there it falls linearly with the speed, reaching zero at the free running speed, which is + * how a DC motor fed from a fixed voltage behaves. Braking is not limited by the motors, since the + * back EMF helps it. + */ +struct MotionLimits { + /** + * @brief Get the largest acceleration available at a speed. + * + * @param speed The current speed. + * @return The acceleration limit at that speed. + */ + float acceleration_at(float speed) const; + + /** + * @brief Get the speed from which the motors, rather than the tires, limit the acceleration. + * + * @return The crossover speed, which is negative if the motors limit it from rest. + */ + float crossover_speed() const; + + float max_speed; + float acceleration; + float deceleration; + float motor_acceleration; + float motor_speed; +}; + +/** + * @brief Derivation of the limits of a run from the physical model of the robot. + */ +class Dynamics { +public: + /** + * @brief Configuration struct for the dynamics. + * + * @note The speed limits are not physical: they bound what the sensing and the control have + * been proven at, and only matter where the physics would allow more. The voltage reserve is + * the fraction of the motor supply kept for the feedback and for steering, which the planned + * accelerations may not use. + */ + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) no defaults, so that a missing field is a warning + struct Config { + RobotModel model; + TurnTable turns; + TurnTable risky_turns; + float max_linear_speed; + float max_angular_speed; + float voltage_reserve; + }; + + /** + * @brief Construct a new Dynamics object. + * + * @param config The configuration for the dynamics. + */ + explicit Dynamics(const Config& config); + + /** + * @brief Get the limits of the motion along a straight. + * + * @param profile The profile of the run. + * @return The linear limits in m/s and m/s^2. + */ + MotionLimits get_linear_limits(const RunProfile& profile) const; + + /** + * @brief Get the limits of a rotation in place. + * + * @param profile The profile of the run. + * @return The angular limits in rad/s and rad/s^2. + */ + MotionLimits get_angular_limits(const RunProfile& profile) const; + + /** + * @brief Get the shape of a turn for a run. + * + * @param profile The profile of the run. + * @param turn The turn. + * @return The shape of the turn. + */ + const TurnShape& get_turn(const RunProfile& profile, TurnId turn) const; + + /** + * @brief Get the speed a turn is driven at. + * + * @details The largest speed at which neither the lateral acceleration at the peak curvature + * nor the angular acceleration along the ramps exceeds what the tires can give to this run. + * + * @param profile The profile of the run. + * @param turn The turn. + * @return The linear speed along the turn in m/s. + */ + float get_turn_speed(const RunProfile& profile, TurnId turn) const; + + /** + * @brief Get the physical model of the robot. + * + * @return The model. + */ + const RobotModel& get_model() const; + +private: + /** + * @brief Physical description of the robot. + */ + RobotModel model; + + /** + * @brief Shape of the turns with the normal safety margin. + */ + TurnTable turns; + + /** + * @brief Shape of the turns with the reduced safety margin. + */ + TurnTable risky_turns; + + /** + * @brief Largest linear speed planned, in m/s. + */ + float max_linear_speed; + + /** + * @brief Largest angular speed planned for a rotation in place, in rad/s. + */ + float max_angular_speed; + + /** + * @brief Voltage the planned motions may count on. + */ + float available_voltage; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_MOTION_LIMITS_HPP diff --git a/micras_nav/include/micras/nav/segment.hpp b/micras_nav/include/micras/nav/segment.hpp new file mode 100644 index 0000000..15bd272 --- /dev/null +++ b/micras_nav/include/micras/nav/segment.hpp @@ -0,0 +1,63 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_SEGMENT_HPP +#define MICRAS_NAV_SEGMENT_HPP + +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Kinds of motion a route is made of. + */ +enum class SegmentKind : uint8_t { + STRAIGHT = 0, // Straight line along the grid. + DIAGONAL = 1, // Straight line along a diagonal. + TURN = 2, // Slalom turn from the turn table, at a constant linear speed. + SPIN = 3, // Rotation in place. + STOP = 4, // Stand still for a given time. + ATTACH = 5, // Stand still facing a wall until the pose settles, or for a given time at most. +}; + +/** + * @brief One motion of a route, as a plain value. + * + * @details The length is in meters for a straight and negative to drive it backwards, in radians for + * a turn and for a rotation in place, positive to the left, and in seconds for the two kinds that + * stand still. The speeds are those at the ends of the segment and are filled in by the velocity + * planner, as is the largest speed allowed along it. + * + * @note The start is the pose the segment nominally starts from in the maze frame. It comes from the + * route on the grid and never from a measurement, so the references generated from it do not move + * when the pose estimate is corrected, and whatever error is left at the end of a segment shows up + * as an error at the start of the next one instead of being forgotten. + */ +struct Segment { + SegmentKind kind; + TurnId turn; + float length; + float start_speed; + float end_speed; + float max_speed; + Pose start; +}; + +/** + * @brief What the robot should be doing at one instant. + * + * @note The acceleration is the exact derivative of the twist, which is what lets the feed forward + * be computed without differentiating anything numerically. + */ +struct Reference { + Pose pose; + Twist twist; + Twist acceleration; + float distance; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_SEGMENT_HPP diff --git a/micras_nav/include/micras/nav/speed_profile.hpp b/micras_nav/include/micras/nav/speed_profile.hpp new file mode 100644 index 0000000..63defe6 --- /dev/null +++ b/micras_nav/include/micras/nav/speed_profile.hpp @@ -0,0 +1,179 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_SPEED_PROFILE_HPP +#define MICRAS_NAV_SPEED_PROFILE_HPP + +#include +#include + +#include "micras/nav/motion_limits.hpp" + +namespace micras::nav { +/** + * @brief Fastest motion along one axis between two speeds, as a function of time. + * + * @details The motion accelerates as hard as the limits allow, cruises if it reaches the speed + * limit and brakes at the last moment, which is the time optimal way to cover a distance. It is the + * single evaluator of the navigation: the planner prices a straight with duration(), the velocity + * planner chains straights with get_reachable_speed() and get_brakeable_speed(), and the executor + * plays the very same object back with sample(), so that the time a route is chosen by is the time + * it takes. The axis is a distance for a straight and an angle for a rotation in place. + */ +class SpeedProfile { +public: + /** + * @brief State of the motion at one instant. + */ + struct Sample { + float distance; + float speed; + float acceleration; + }; + + /** + * @brief Construct an empty profile, of zero duration. + */ + SpeedProfile() = default; + + /** + * @brief Construct a new Speed Profile object. + * + * @note Speeds that the distance does not allow are not an error. A start speed too high to + * brake from is braked harder than the limit, and an end speed that cannot be reached leaves + * the motion accelerating all the way, so the profile always covers exactly the distance. It is + * the velocity planner that keeps the speeds it asks for feasible. + * + * @param distance The distance to cover, which must not be negative. + * @param start_speed The speed at the start. + * @param end_speed The speed wanted at the end. + * @param limits The limits of the motion. + */ + SpeedProfile(float distance, float start_speed, float end_speed, const MotionLimits& limits); + + /** + * @brief Get the time the motion takes. + * + * @return The duration in seconds. + */ + float duration() const; + + /** + * @brief Get the distance the motion covers. + * + * @return The distance. + */ + float distance() const; + + /** + * @brief Evaluate the motion at an instant. + * + * @param time Time since the start of the motion, clamped to its duration. + * @return The distance covered, the speed and the acceleration at that instant. + */ + Sample sample(float time) const; + + /** + * @brief Get the instant at which a distance has been covered. + * + * @param distance The distance, clamped to the one the motion covers. + * @return The time since the start of the motion in seconds. + */ + float time_at(float distance) const; + + /** + * @brief Get the largest speed that can be reached over a distance. + * + * @param distance The distance available to accelerate. + * @param start_speed The speed at the start. + * @param limits The limits of the motion. + * @return The speed at the end when accelerating all the way, capped at the speed limit. + */ + static float get_reachable_speed(float distance, float start_speed, const MotionLimits& limits); + + /** + * @brief Get the largest speed that can be braked from over a distance. + * + * @param distance The distance available to brake. + * @param end_speed The speed at the end. + * @param limits The limits of the motion. + * @return The speed at the start when braking all the way, capped at the speed limit. + */ + static float get_brakeable_speed(float distance, float end_speed, const MotionLimits& limits); + +private: + /** + * @brief Stretch of the motion with one law for the acceleration. + * + * @note The rate is zero for a constant acceleration. Otherwise the acceleration decays as the + * speed approaches the target, as `rate * (target - speed)`, which is the motor limited stretch. + */ + struct Phase { + float duration{}; + float start_distance{}; + float start_speed{}; + float acceleration{}; + float rate{}; + float target{}; + }; + + /** + * @brief Number of phases: traction limited, motor limited, cruise and braking. + */ + static constexpr uint8_t number_of_phases{4}; + + /** + * @brief Get the distance needed to accelerate between two speeds. + * + * @param start_speed The speed at the start. + * @param end_speed The speed at the end, not lower than the start and below the free speed. + * @param limits The limits of the motion. + * @return The distance covered while accelerating as hard as the limits allow. + */ + static float get_acceleration_distance(float start_speed, float end_speed, const MotionLimits& limits); + + /** + * @brief Append a phase, advancing the state of the motion to its end. + * + * @param phase The phase, with its duration, acceleration, rate and target filled in. + */ + void append(Phase phase); + + /** + * @brief Evaluate a phase at an instant. + * + * @param phase The phase. + * @param time Time since the start of the phase. + * @return The distance covered since the start of the motion, the speed and the acceleration. + */ + static Sample evaluate(const Phase& phase, float time); + + /** + * @brief Phases of the motion, in order, the unused ones with a null duration. + */ + std::array phases{}; + + /** + * @brief Number of phases in use. + */ + uint8_t size{}; + + /** + * @brief Time the motion takes. + */ + float total_duration{}; + + /** + * @brief Distance the motion covers. + */ + float total_distance{}; + + /** + * @brief Speed at the end of the last phase appended. + */ + float last_speed{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_SPEED_PROFILE_HPP diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index 1a8fa21..b62c896 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -29,6 +29,22 @@ struct Pose { */ core::Vector to_cell(float cell_size) const; + /** + * @brief Express in the frame of this pose a pose given relative to it. + * + * @param local The pose in the frame defined by this pose. + * @return The same pose in the frame this pose is expressed in. + */ + Pose compose(const Pose& local) const; + + /** + * @brief Express another pose in the frame defined by this pose. + * + * @param other A pose in the same frame as this pose. + * @return The other pose as seen from this one, with the orientation wrapped to [-pi, pi]. + */ + Pose relative(const Pose& other) const; + /** * @brief The position of the pose. */ diff --git a/micras_nav/include/micras/nav/velocity_planner.hpp b/micras_nav/include/micras/nav/velocity_planner.hpp new file mode 100644 index 0000000..bda9cca --- /dev/null +++ b/micras_nav/include/micras/nav/velocity_planner.hpp @@ -0,0 +1,58 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_VELOCITY_PLANNER_HPP +#define MICRAS_NAV_VELOCITY_PLANNER_HPP + +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/segment.hpp" + +namespace micras::nav { +/** + * @brief Choice of the speed at every junction of a route. + * + * @details Every turn starts at the speed its shape allows and every straight at the speed limit. + * A pass from the end to the start then lowers each junction to what can be braked from before the + * next one, and a pass from the start to the end lowers it to what can be reached from the previous + * one. A turn is driven at a single speed, so lowering one of its ends lowers the other, which is + * how two turns too close together slow each other down. What is left is the fastest set of speeds + * the limits allow. + */ +class VelocityPlanner { +public: + /** + * @brief Deleted constructor for static class. + */ + VelocityPlanner() = delete; + + /** + * @brief Fill in the speeds of a route. + * + * @param route The segments of the route, in order, with their kinds, lengths and turns set. + * @param dynamics The physical limits of the robot. + * @param profile The profile of the run. + * @param start_speed The speed at the start of the first segment. + * @param end_speed The speed at the end of the last segment. + * @return The time the route takes in seconds. + */ + static float plan( + std::span route, const Dynamics& dynamics, const RunProfile& profile, float start_speed, + float end_speed + ); + + /** + * @brief Get the time a segment takes, with the speeds it already has. + * + * @param segment The segment. + * @param dynamics The physical limits of the robot. + * @param profile The profile of the run. + * @return The duration of the segment in seconds. + */ + static float get_duration(const Segment& segment, const Dynamics& dynamics, const RunProfile& profile); +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_VELOCITY_PLANNER_HPP diff --git a/micras_nav/src/executor.cpp b/micras_nav/src/executor.cpp new file mode 100644 index 0000000..9a812fc --- /dev/null +++ b/micras_nav/src/executor.cpp @@ -0,0 +1,221 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/nav/executor.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/turn_table.hpp" +#include "micras/nav/velocity_planner.hpp" + +namespace micras::nav { +Executor::Executor(const Dynamics& dynamics, const Config& config) : dynamics{dynamics}, config{config} { + this->segments.reserve(config.capacity); + this->durations.reserve(config.capacity); +} + +void Executor::reset(const Pose& pose, const RunProfile& profile) { + this->run_profile = profile; + this->segments.clear(); + this->durations.clear(); + this->index = 0; + this->clock = 0.0F; + this->duration = 0.0F; + this->queued_time = 0.0F; + this->settled_time = 0.0F; + this->reference = {.pose = pose, .twist = {}, .acceleration = {}, .distance = 0.0F}; +} + +void Executor::push(std::span segments) { + const bool was_finished = this->is_finished(); + + if (was_finished) { + this->segments.clear(); + this->durations.clear(); + this->index = 0; + this->queued_time = 0.0F; + } + + for (const Segment& segment : segments) { + this->segments.push_back(segment); + this->durations.push_back(VelocityPlanner::get_duration(segment, this->dynamics, this->run_profile)); + this->queued_time += this->durations.back(); + } + + if (was_finished and not this->segments.empty()) { + this->start_next(); + } +} + +Reference Executor::update(float elapsed_time, float time_scale, const State& estimate) { + if (this->is_finished()) { + return this->reference; + } + + this->clock += elapsed_time * time_scale; + + while (not this->is_finished()) { + const bool attaching = this->segments.at(this->index).kind == SegmentKind::ATTACH; + + if (this->clock < this->duration and not(attaching and this->has_settled(estimate, elapsed_time))) { + break; + } + + const float carried = attaching ? 0.0F : this->clock - this->duration; + + this->clock = this->duration; + this->reference = this->evaluate(); + this->index++; + + if (this->is_finished()) { + this->reference.twist = {}; + this->reference.acceleration = {}; + return this->reference; + } + + this->start_next(); + this->clock = carried; + } + + this->reference = this->evaluate(); + this->reference.twist.linear *= time_scale; + this->reference.twist.angular *= time_scale; + this->reference.acceleration.linear *= time_scale * time_scale; + this->reference.acceleration.angular *= time_scale * time_scale; + + return this->reference; +} + +bool Executor::is_finished() const { + return this->index >= this->segments.size(); +} + +bool Executor::is_ending(float horizon) const { + if (this->is_finished()) { + return true; + } + + return this->clock + horizon >= this->duration + this->queued_time; +} + +void Executor::divert(std::span segments) { + this->segments.resize(std::min(this->index, this->segments.size())); + this->durations.resize(this->segments.size()); + this->index = this->segments.size(); + this->duration = 0.0F; + this->clock = 0.0F; + this->queued_time = 0.0F; + this->push(segments); +} + +const Segment* Executor::get_current() const { + return this->is_finished() ? nullptr : &this->segments.at(this->index); +} + +const Reference& Executor::get_reference() const { + return this->reference; +} + +void Executor::start_next() { + const Segment& segment = this->segments.at(this->index); + + this->clock = 0.0F; + this->settled_time = 0.0F; + this->speed_profile = {}; + this->queued_time = std::max(this->queued_time - this->durations.at(this->index), 0.0F); + + switch (segment.kind) { + case SegmentKind::STRAIGHT: + case SegmentKind::DIAGONAL: + this->speed_profile = SpeedProfile{ + std::abs(segment.length), segment.start_speed, segment.end_speed, + this->dynamics.get_linear_limits(this->run_profile) + }; + this->duration = this->speed_profile.duration(); + break; + + case SegmentKind::SPIN: + this->speed_profile = SpeedProfile{ + std::abs(segment.length), 0.0F, 0.0F, this->dynamics.get_angular_limits(this->run_profile) + }; + this->duration = this->speed_profile.duration(); + break; + + default: + this->duration = this->durations.at(this->index); + break; + } +} + +Reference Executor::evaluate() const { + const Segment& segment = this->segments.at(this->index); + const float side = std::copysign(1.0F, segment.length); + + switch (segment.kind) { + case SegmentKind::STRAIGHT: + case SegmentKind::DIAGONAL: { + const SpeedProfile::Sample sample = this->speed_profile.sample(this->clock); + + return { + .pose = + segment.start.compose({.position = {.x = side * sample.distance, .y = 0.0F}, .orientation = 0.0F}), + .twist = {.linear = side * sample.speed, .angular = 0.0F}, + .acceleration = {.linear = side * sample.acceleration, .angular = 0.0F}, + .distance = sample.distance, + }; + } + + case SegmentKind::TURN: { + const TurnShape& shape = this->dynamics.get_turn(this->run_profile, segment.turn); + const float speed = segment.start_speed; + const float distance = std::min(speed * this->clock, shape.length()); + const auto sample = shape.sample(distance); + + return { + .pose = segment.start.compose( + {.position = {.x = sample.x, .y = side * sample.y}, .orientation = side * sample.heading} + ), + .twist = {.linear = speed, .angular = side * sample.curvature * speed}, + .acceleration = {.linear = 0.0F, .angular = side * sample.sharpness * speed * speed}, + .distance = distance, + }; + } + + case SegmentKind::SPIN: { + const SpeedProfile::Sample sample = this->speed_profile.sample(this->clock); + + return { + .pose = segment.start.compose({.position = {}, .orientation = side * sample.distance}), + .twist = {.linear = 0.0F, .angular = side * sample.speed}, + .acceleration = {.linear = 0.0F, .angular = side * sample.acceleration}, + .distance = sample.distance, + }; + } + + case SegmentKind::STOP: + case SegmentKind::ATTACH: + break; + } + + return {.pose = segment.start, .twist = {}, .acceleration = {}, .distance = 0.0F}; +} + +bool Executor::has_settled(const State& estimate, float elapsed_time) { + const Pose error = this->segments.at(this->index).start.relative(estimate.pose); + + const bool within_tolerance = error.position.magnitude() < this->config.settle_distance and + std::abs(error.orientation) < this->config.settle_angle and + std::abs(estimate.velocity.linear) < this->config.settle_linear_speed and + std::abs(estimate.velocity.angular) < this->config.settle_angular_speed; + + this->settled_time = within_tolerance ? this->settled_time + elapsed_time : 0.0F; + + return this->settled_time >= this->config.settle_time; +} +} // namespace micras::nav diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index cec2f23..ff8501f 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -2,11 +2,11 @@ * @file */ -#include - +#include #include #include #include +#include #include "micras/core/vector.hpp" #include "micras/nav/grid_pose.hpp" @@ -38,7 +38,11 @@ Side GridPoint::direction(const GridPoint& next) const { } GridPoint GridPoint::from_vector(const core::Vector& point, float cell_size) { - return {.x = static_cast(point.x / cell_size), .y = static_cast(point.y / cell_size)}; + const auto to_index = [cell_size](float coordinate) { + return static_cast(std::clamp(std::floor(coordinate / cell_size), 0.0F, 255.0F)); + }; + + return {.x = to_index(point.x), .y = to_index(point.y)}; } core::Vector GridPoint::to_vector(float cell_size) const { diff --git a/micras_nav/src/motion_limits.cpp b/micras_nav/src/motion_limits.cpp new file mode 100644 index 0000000..81fc099 --- /dev/null +++ b/micras_nav/src/motion_limits.cpp @@ -0,0 +1,76 @@ +/** + * @file + */ + +#include +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +float MotionLimits::acceleration_at(float speed) const { + return std::min(this->acceleration, this->motor_acceleration * (1.0F - speed / this->motor_speed)); +} + +float MotionLimits::crossover_speed() const { + return this->motor_speed * (1.0F - this->acceleration / this->motor_acceleration); +} + +Dynamics::Dynamics(const Config& config) : + model{config.model}, + turns{config.turns}, + risky_turns{config.risky_turns}, + max_linear_speed{config.max_linear_speed}, + max_angular_speed{config.max_angular_speed}, + available_voltage{ + config.model.drive.supply_voltage * (1.0F - config.voltage_reserve) - config.model.drive.static_friction_voltage + } { } + +MotionLimits Dynamics::get_linear_limits(const RunProfile& profile) const { + const float traction = profile.utilization * this->model.traction_acceleration(profile.fan); + const float free_speed = this->available_voltage / this->model.speed_constant(); + + return { + .max_speed = std::min({profile.max_speed, this->max_linear_speed, 0.95F * free_speed}), + .acceleration = traction, + .deceleration = traction, + .motor_acceleration = this->available_voltage / this->model.acceleration_constant(), + .motor_speed = free_speed, + }; +} + +MotionLimits Dynamics::get_angular_limits(const RunProfile& profile) const { + const float traction = profile.utilization * this->model.traction_angular_acceleration(profile.fan); + const float free_speed = this->available_voltage / this->model.angular_speed_constant(); + + return { + .max_speed = std::min(this->max_angular_speed, 0.95F * free_speed), + .acceleration = traction, + .deceleration = traction, + .motor_acceleration = this->available_voltage / this->model.angular_acceleration_constant(), + .motor_speed = free_speed, + }; +} + +const TurnShape& Dynamics::get_turn(const RunProfile& profile, TurnId turn) const { + return profile.risky ? this->risky_turns.get(turn) : this->turns.get(turn); +} + +float Dynamics::get_turn_speed(const RunProfile& profile, TurnId turn) const { + const TurnShape& shape = this->get_turn(profile, turn); + + const float lateral = profile.utilization * this->model.traction_acceleration(profile.fan); + const float angular = profile.utilization * this->model.traction_angular_acceleration(profile.fan); + + return std::min( + {std::sqrt(lateral / shape.curvature), std::sqrt(angular / shape.sharpness), + this->get_linear_limits(profile).max_speed} + ); +} + +const RobotModel& Dynamics::get_model() const { + return this->model; +} +} // namespace micras::nav diff --git a/micras_nav/src/speed_profile.cpp b/micras_nav/src/speed_profile.cpp new file mode 100644 index 0000000..0b2a71b --- /dev/null +++ b/micras_nav/src/speed_profile.cpp @@ -0,0 +1,254 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/speed_profile.hpp" + +namespace micras::nav { +SpeedProfile::SpeedProfile(float distance, float start_speed, float end_speed, const MotionLimits& limits) : + total_distance{std::max(distance, 0.0F)}, last_speed{start_speed} { + if (this->total_distance <= 0.0F) { + return; + } + + const float braking_distance = (start_speed * start_speed - end_speed * end_speed) / (2.0F * limits.deceleration); + + if (braking_distance >= this->total_distance) { + const float deceleration = (start_speed * start_speed - end_speed * end_speed) / (2.0F * this->total_distance); + + this->append({.duration = (start_speed - end_speed) / deceleration, .acceleration = -deceleration}); + return; + } + + const float cap = std::max(limits.max_speed, start_speed); + float low = std::max(start_speed, end_speed); + float peak = cap; + + const auto excess = [&](float speed) { + return get_acceleration_distance(start_speed, speed, limits) + + (speed * speed - end_speed * end_speed) / (2.0F * limits.deceleration) - this->total_distance; + }; + + if (excess(low) >= 0.0F) { + peak = get_reachable_speed(this->total_distance, start_speed, limits); + } else if (excess(cap) > 0.0F) { + float high = cap; + + for (uint8_t i = 0; i < 32; i++) { + const float middle = (low + high) / 2.0F; + + if (excess(middle) > 0.0F) { + high = middle; + } else { + low = middle; + } + } + + peak = low; + } + + const float crossover = limits.crossover_speed(); + const float traction_end = std::clamp(crossover, start_speed, peak); + + if (traction_end > start_speed) { + this->append( + {.duration = (traction_end - start_speed) / limits.acceleration, .acceleration = limits.acceleration} + ); + } + + if (peak > traction_end) { + const float rate = limits.motor_acceleration / limits.motor_speed; + + this->append({ + .duration = std::log((limits.motor_speed - traction_end) / (limits.motor_speed - peak)) / rate, + .rate = rate, + .target = limits.motor_speed, + }); + } + + const float final_speed = std::min(peak, end_speed); + const float braking = (peak * peak - final_speed * final_speed) / (2.0F * limits.deceleration); + + float accelerated = 0.0F; + + if (this->size > 0) { + const Phase& last = this->phases.at(this->size - 1); + accelerated = evaluate(last, last.duration).distance; + } + + const float cruise = this->total_distance - accelerated - braking; + + if (cruise > 0.0F and peak > 0.0F) { + this->append({.duration = cruise / peak}); + } + + if (peak > final_speed) { + this->append({.duration = (peak - final_speed) / limits.deceleration, .acceleration = -limits.deceleration}); + } +} + +float SpeedProfile::duration() const { + return this->total_duration; +} + +float SpeedProfile::distance() const { + return this->total_distance; +} + +SpeedProfile::Sample SpeedProfile::sample(float time) const { + if (this->size == 0) { + return {.distance = this->total_distance, .speed = this->last_speed, .acceleration = 0.0F}; + } + + time = std::clamp(time, 0.0F, this->total_duration); + + for (uint8_t i = 0; i < this->size; i++) { + const Phase& phase = this->phases.at(i); + + if (time <= phase.duration or i + 1 == this->size) { + Sample result = evaluate(phase, std::min(time, phase.duration)); + result.distance = std::min(result.distance, this->total_distance); + return result; + } + + time -= phase.duration; + } + + return {.distance = this->total_distance, .speed = this->last_speed, .acceleration = 0.0F}; +} + +float SpeedProfile::time_at(float distance) const { + distance = std::clamp(distance, 0.0F, this->total_distance); + float elapsed = 0.0F; + + for (uint8_t i = 0; i < this->size; i++) { + const Phase& phase = this->phases.at(i); + const float end_distance = evaluate(phase, phase.duration).distance; + + if (distance > end_distance and i + 1 < this->size) { + elapsed += phase.duration; + continue; + } + + float low = 0.0F; + float high = phase.duration; + + for (uint8_t j = 0; j < 32; j++) { + const float middle = (low + high) / 2.0F; + + if (evaluate(phase, middle).distance < distance) { + low = middle; + } else { + high = middle; + } + } + + return elapsed + (low + high) / 2.0F; + } + + return elapsed; +} + +float SpeedProfile::get_reachable_speed(float distance, float start_speed, const MotionLimits& limits) { + if (distance <= 0.0F or start_speed >= limits.max_speed) { + return start_speed; + } + + if (get_acceleration_distance(start_speed, limits.max_speed, limits) <= distance) { + return limits.max_speed; + } + + const float crossover = limits.crossover_speed(); + const float unlimited = std::sqrt(start_speed * start_speed + 2.0F * limits.acceleration * distance); + + if (unlimited <= crossover) { + return unlimited; + } + + float low = start_speed; + float high = limits.max_speed; + + for (uint8_t i = 0; i < 32; i++) { + const float middle = (low + high) / 2.0F; + + if (get_acceleration_distance(start_speed, middle, limits) > distance) { + high = middle; + } else { + low = middle; + } + } + + return low; +} + +float SpeedProfile::get_brakeable_speed(float distance, float end_speed, const MotionLimits& limits) { + if (distance <= 0.0F) { + return end_speed; + } + + return std::min(limits.max_speed, std::sqrt(end_speed * end_speed + 2.0F * limits.deceleration * distance)); +} + +float SpeedProfile::get_acceleration_distance(float start_speed, float end_speed, const MotionLimits& limits) { + if (end_speed <= start_speed) { + return 0.0F; + } + + const float traction_end = std::clamp(limits.crossover_speed(), start_speed, end_speed); + + float distance = (traction_end * traction_end - start_speed * start_speed) / (2.0F * limits.acceleration); + + if (end_speed > traction_end) { + const float rate = limits.motor_acceleration / limits.motor_speed; + + distance += + (limits.motor_speed * std::log((limits.motor_speed - traction_end) / (limits.motor_speed - end_speed)) - + (end_speed - traction_end)) / + rate; + } + + return distance; +} + +void SpeedProfile::append(Phase phase) { + if (phase.duration <= 0.0F or this->size == number_of_phases) { + return; + } + + if (this->size > 0) { + const Phase& last = this->phases.at(this->size - 1); + phase.start_distance = evaluate(last, last.duration).distance; + } + + phase.start_speed = this->last_speed; + + this->last_speed = evaluate(phase, phase.duration).speed; + this->total_duration += phase.duration; + this->phases.at(this->size) = phase; + this->size++; +} + +SpeedProfile::Sample SpeedProfile::evaluate(const Phase& phase, float time) { + if (phase.rate > 0.0F) { + const float gap = phase.target - phase.start_speed; + const float decay = std::exp(-phase.rate * time); + + return { + .distance = phase.start_distance + phase.target * time - gap * (1.0F - decay) / phase.rate, + .speed = phase.target - gap * decay, + .acceleration = phase.rate * gap * decay, + }; + } + + return { + .distance = phase.start_distance + phase.start_speed * time + phase.acceleration * time * time / 2.0F, + .speed = phase.start_speed + phase.acceleration * time, + .acceleration = phase.acceleration, + }; +} +} // namespace micras::nav diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 4083eee..2cb22de 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -4,6 +4,7 @@ #include +#include "micras/core/utils.hpp" #include "micras/core/vector.hpp" #include "micras/nav/grid_pose.hpp" #include "micras/nav/state.hpp" @@ -43,6 +44,30 @@ core::Vector Pose::to_cell(float cell_size) const { return {.x = cell_alignment, .y = cell_advance}; } +Pose Pose::compose(const Pose& local) const { + const float cosine = std::cos(this->orientation); + const float sine = std::sin(this->orientation); + + return { + .position = + {.x = this->position.x + cosine * local.position.x - sine * local.position.y, + .y = this->position.y + sine * local.position.x + cosine * local.position.y}, + .orientation = this->orientation + local.orientation, + }; +} + +Pose Pose::relative(const Pose& other) const { + const float cosine = std::cos(this->orientation); + const float sine = std::sin(this->orientation); + + const core::Vector offset = other.position - this->position; + + return { + .position = {.x = cosine * offset.x + sine * offset.y, .y = -sine * offset.x + cosine * offset.y}, + .orientation = core::assert_angle(other.orientation - this->orientation), + }; +} + RelativePose::RelativePose(const Pose& absolute_pose) : absolute_pose{&absolute_pose} { } Pose RelativePose::get() const { diff --git a/micras_nav/src/velocity_planner.cpp b/micras_nav/src/velocity_planner.cpp new file mode 100644 index 0000000..27e8947 --- /dev/null +++ b/micras_nav/src/velocity_planner.cpp @@ -0,0 +1,108 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/velocity_planner.hpp" + +namespace micras::nav { +float VelocityPlanner::plan( + std::span route, const Dynamics& dynamics, const RunProfile& profile, float start_speed, float end_speed +) { + if (route.empty()) { + return 0.0F; + } + + const MotionLimits linear_limits = dynamics.get_linear_limits(profile); + + const auto is_straight = [](const Segment& segment) { + return segment.kind == SegmentKind::STRAIGHT or segment.kind == SegmentKind::DIAGONAL; + }; + + for (Segment& segment : route) { + float limit = 0.0F; + + if (is_straight(segment)) { + limit = linear_limits.max_speed; + } else if (segment.kind == SegmentKind::TURN) { + limit = dynamics.get_turn_speed(profile, segment.turn); + } + + segment.max_speed = limit; + segment.start_speed = limit; + segment.end_speed = limit; + } + + float next_speed = end_speed; + + for (Segment& segment : route | std::views::reverse) { + segment.end_speed = std::min(segment.end_speed, next_speed); + + if (is_straight(segment)) { + segment.start_speed = std::min( + segment.start_speed, + SpeedProfile::get_brakeable_speed(std::abs(segment.length), segment.end_speed, linear_limits) + ); + } else { + segment.start_speed = std::min(segment.start_speed, segment.end_speed); + } + + next_speed = segment.start_speed; + } + + float previous_speed = start_speed; + + for (Segment& segment : route) { + segment.start_speed = std::min(segment.start_speed, previous_speed); + + if (is_straight(segment)) { + segment.end_speed = std::min( + segment.end_speed, + SpeedProfile::get_reachable_speed(std::abs(segment.length), segment.start_speed, linear_limits) + ); + } else { + segment.end_speed = std::min(segment.end_speed, segment.start_speed); + } + + previous_speed = segment.end_speed; + } + + float total_time = 0.0F; + + for (const Segment& segment : route) { + total_time += get_duration(segment, dynamics, profile); + } + + return total_time; +} + +float VelocityPlanner::get_duration(const Segment& segment, const Dynamics& dynamics, const RunProfile& profile) { + switch (segment.kind) { + case SegmentKind::STRAIGHT: + case SegmentKind::DIAGONAL: + return SpeedProfile{ + std::abs(segment.length), segment.start_speed, segment.end_speed, dynamics.get_linear_limits(profile) + } + .duration(); + + case SegmentKind::TURN: + return dynamics.get_turn(profile, segment.turn).length() / segment.start_speed; + + case SegmentKind::SPIN: + return SpeedProfile{std::abs(segment.length), 0.0F, 0.0F, dynamics.get_angular_limits(profile)}.duration(); + + case SegmentKind::STOP: + case SegmentKind::ATTACH: + return segment.length; + } + + return 0.0F; +} +} // namespace micras::nav From c33f1761dd77de1ce5560f73260a433f03e47f83 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 03:23:52 -0300 Subject: [PATCH 490/561] =?UTF-8?q?=E2=9C=A8=20Control=20the=20position=20?= =?UTF-8?q?with=20a=20feed=20forward=20and=20measure=20its=20constants=20o?= =?UTF-8?q?n=20the=20robot?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The controller has two axes, forward and rotation. On each, the model of the drive train fed with the speed and the exact acceleration of the reference supplies almost all of the voltage, and the feedback closes the loop on the integrated quantities, the position along the path and the orientation, with a derivative term on the speed. There is no integrator to wind up or to reset between segments, turns are closed loop in orientation, and the gains are not tuned: they follow from the model and from the natural frequency and damping asked of each axis. The error across the path becomes a small and bounded offset of the orientation, which steers the robot back at a rate set by the distance traveled, wherever the pose is known, walls or not. The navigation takes its inputs as plain measurements, sampled once per iteration by whoever owns the hardware, instead of holding devices. The drive identification replaces the feed forward test program, whose running maxima were never cleared between its four drive patterns and gave the angular acceleration constant the same sign on both motors. It drives two level steps forward, backward and to each side, and fits the friction, the speed constant and the acceleration constant of each axis to the whole response by least squares on the integrated model, which never differentiates a noisy signal. A single level cannot tell the friction from the speed constant, hence the two. The gyroscope calibration turns the robot a whole number of times in front of a wall and compares what the gyroscope integrated with what the two sensors that look forward say about the wall before and after. --- micras_nav/include/micras/nav/controller.hpp | 168 ++++++++++++ .../micras/nav/drive_identification.hpp | 257 ++++++++++++++++++ .../micras/nav/gyroscope_calibration.hpp | 176 ++++++++++++ .../include/micras/nav/measurements.hpp | 53 ++++ micras_nav/src/controller.cpp | 95 +++++++ micras_nav/src/drive_identification.cpp | 207 ++++++++++++++ micras_nav/src/gyroscope_calibration.cpp | 129 +++++++++ 7 files changed, 1085 insertions(+) create mode 100644 micras_nav/include/micras/nav/controller.hpp create mode 100644 micras_nav/include/micras/nav/drive_identification.hpp create mode 100644 micras_nav/include/micras/nav/gyroscope_calibration.hpp create mode 100644 micras_nav/include/micras/nav/measurements.hpp create mode 100644 micras_nav/src/controller.cpp create mode 100644 micras_nav/src/drive_identification.cpp create mode 100644 micras_nav/src/gyroscope_calibration.cpp diff --git a/micras_nav/include/micras/nav/controller.hpp b/micras_nav/include/micras/nav/controller.hpp new file mode 100644 index 0000000..9ec648e --- /dev/null +++ b/micras_nav/include/micras/nav/controller.hpp @@ -0,0 +1,168 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_CONTROLLER_HPP +#define MICRAS_NAV_CONTROLLER_HPP + +#include "micras/nav/robot_model.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Computation of the motor commands that make the robot follow a reference. + * + * @details The command has two axes, forward and rotation. On each of them a feed forward, which is + * the model of the drive train fed with the speed and the acceleration of the reference, supplies + * almost all of the voltage, and a feedback closes the loop on the integrated quantities, the + * position along the path and the orientation, with a derivative term on the speed: + * + * forward = feed_forward(v, a) + K_s * e_s + D_s * (v - v_measured) + * rotation = feed_forward(omega, alpha) + K_o * (e_o + steering(e_y)) + D_o * (omega - omega_measured) + * + * The errors are those of the estimated pose as seen from the pose of the reference: e_s along the + * path, e_y across it and e_o in orientation. There is no integrator, so there is nothing to wind up + * or to reset between segments. The steering is a small and bounded offset of the orientation, + * proportional to the error across the path, which turns the robot back towards it at a rate set by + * the distance traveled rather than by time. It works wherever the pose is known, walls or not. + * + * @note The gains are not tuned: they follow from the model of the drive train and from the natural + * frequency and damping asked of each axis. + */ +class Controller { +public: + /** + * @brief Closed loop behavior asked of one axis. + * + * @note The natural frequency is in rad/s. The error is clamped before it is used, which bounds + * what the feedback can ask for when the robot is held or has crashed. + */ + struct Axis { + float natural_frequency; + float damping; + float max_error; + }; + + /** + * @brief Configuration struct for the controller. + * + * @note The steering gain is in radians of offset per meter of error and the offset is limited + * to the largest steering. Below the blend speed the steering fades out, since turning in place + * cannot reduce an error across the path. The friction speed is the wheel speed over which the + * static friction compensation goes from nothing to all of it. + */ + struct Config { + RobotModel model; + Axis linear; + Axis angular; + float steering_gain; + float max_steering; + float steering_blend_speed; + float friction_speed; + float voltage_reserve; + }; + + /** + * @brief Command for the locomotion, in percent of the motor supply. + */ + struct Command { + float forward; + float rotation; + }; + + /** + * @brief Errors and terms of the last update, for a monitor. + */ + struct Status { + float along_error; + float across_error; + float orientation_error; + float forward_feed_forward; + float rotation_feed_forward; + float forward_feedback; + float rotation_feedback; + }; + + /** + * @brief Construct a new Controller object. + * + * @param config The configuration for the controller. + */ + explicit Controller(const Config& config); + + /** + * @brief Compute the command for this instant. + * + * @param reference What the robot should be doing, in the maze frame. + * @param estimate What the robot is doing, as far as it is known. + * @return The command for the locomotion. + */ + Command update(const Reference& reference, const State& estimate); + + /** + * @brief Get how much slower the reference has to be played for the motors to follow it. + * + * @note The feed forward alone may ask for more voltage than there is. Slowing the clock of + * the reference scales its linear and angular speeds together, so the robot stays on the same + * path and only takes longer, instead of cutting the turn it is in. With the limits of a run + * derived from the same model this should stay at one. + * + * @return The factor to apply to the elapsed time of the reference, in (0, 1]. + */ + float get_time_scale() const; + + /** + * @brief Get the errors and terms of the last update. + * + * @return The status. + */ + const Status& get_status() const; + +private: + /** + * @brief Feedback gains of one axis. + */ + struct Gains { + float proportional; + float derivative; + }; + + /** + * @brief Compute the feedback gains of an axis from the model of the drive train. + * + * @param axis The behavior asked of the axis. + * @param speed_constant The voltage per unit of speed of the axis. + * @param acceleration_constant The voltage per unit of acceleration of the axis. + * @return The gains, in volts per unit of position and of speed. + */ + static Gains compute_gains(const Axis& axis, float speed_constant, float acceleration_constant); + + /** + * @brief Parameters of the controller, with the physical description of the robot. + */ + Config config; + + /** + * @brief Feedback gains of the forward axis. + */ + Gains linear_gains; + + /** + * @brief Feedback gains of the rotation axis. + */ + Gains angular_gains; + + /** + * @brief Factor to apply to the elapsed time of the reference. + */ + float time_scale{1.0F}; + + /** + * @brief Errors and terms of the last update. + */ + Status status{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_CONTROLLER_HPP diff --git a/micras_nav/include/micras/nav/drive_identification.hpp b/micras_nav/include/micras/nav/drive_identification.hpp new file mode 100644 index 0000000..0d5a44a --- /dev/null +++ b/micras_nav/include/micras/nav/drive_identification.hpp @@ -0,0 +1,257 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_DRIVE_IDENTIFICATION_HPP +#define MICRAS_NAV_DRIVE_IDENTIFICATION_HPP + +#include +#include + +#include "micras/nav/controller.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/robot_model.hpp" + +namespace micras::nav { +/** + * @brief Procedure that measures the constants of the drive train on the robot itself. + * + * @details The robot is driven open loop through a fixed sequence: a slow ramp until the wheels + * start to turn, a step forward and the same step backward, then a step of rotation to each side. + * Every step has two levels, the full command and then a fraction of it, because the response to a + * single level cannot tell the friction from the speed constant. + * Each axis is modeled as `u = k_s * sign(v) + k_v * v + k_a * dv/dt`, and the three constants are + * fitted to the whole response by least squares. The model is fitted in its integrated form, + * `integral(u) = k_s * integral(sign(v)) + k_v * x + k_a * (v - v0)`, which uses the position and + * the speed as they are measured and never differentiates a noisy signal. + * + * The constants come out in volts, and get_model() turns them into the physical constants of the + * robot model, so the result of a run is a set of numbers to type into the configuration. + * + * @note The robot travels forward by about the distance limit and comes back, so it needs that much + * free floor ahead of it. It should run with the fan as it is used in a fast run. + */ +class DriveIdentification { +public: + /** + * @brief Configuration struct for the identification. + * + * @note The commands are in percent of the motor supply. The ramp rate is in percent per + * second, and the wheels count as turning once they are faster than the breakaway speed. Each + * level of a step lasts for the step time, a step forward or backward also ends when the + * distance limit is reached, and the robot rests for the rest time between two steps. + */ + struct Config { + RobotModel model; + float ramp_rate; + float breakaway_speed; + float linear_command; + float angular_command; + float step_time; + float rest_time; + float max_distance; + }; + + /** + * @brief Constants of one axis, in volts per unit of speed and of acceleration. + */ + struct Axis { + float static_friction; + float speed_constant; + float acceleration_constant; + }; + + /** + * @brief Construct a new Drive Identification object. + * + * @param config The configuration for the identification. + */ + explicit DriveIdentification(const Config& config); + + /** + * @brief Start the procedure. + * + * @param measurements The current measurements. + */ + void start(const Measurements& measurements); + + /** + * @brief Advance the procedure by one iteration. + * + * @param measurements The current measurements. + * @param elapsed_time Time since the last iteration, in seconds. + * @return The command to apply to the locomotion. + */ + Controller::Command update(const Measurements& measurements, float elapsed_time); + + /** + * @brief Check if the procedure has ended. + * + * @return True once every step was driven and the constants were fitted. + */ + bool is_finished() const; + + /** + * @brief Check if the fit of both axes was well conditioned. + * + * @return True if the result can be trusted. + */ + bool is_valid() const; + + /** + * @brief Get the voltage at which the wheels started to turn. + * + * @return The breakaway voltage, per motor. + */ + float get_breakaway_voltage() const; + + /** + * @brief Get the constants fitted for the forward axis. + * + * @return The constants, in V, V*s/m and V*s^2/m. + */ + const Axis& get_linear() const; + + /** + * @brief Get the constants fitted for the rotation axis. + * + * @return The constants, in V, V*s/rad and V*s^2/rad. + */ + const Axis& get_angular() const; + + /** + * @brief Get the robot model with the drive constants replaced by the ones that were measured. + * + * @note The torque constant follows from the speed constant, the resistance from the + * acceleration constant and the mass, and the yaw inertia from the angular acceleration + * constant. The mass, the wheel radius, the track width and the gear ratio are taken as given. + * + * @return The model. + */ + RobotModel get_model() const; + +private: + /** + * @brief Command of the second level of a step, as a fraction of the first. + */ + static constexpr float second_level{0.5F}; + + /** + * @brief Steps of the procedure. + */ + enum class Phase : uint8_t { + RAMP = 0, + FORWARD = 1, + BACKWARD = 2, + LEFT = 3, + RIGHT = 4, + FINISHED = 5, + }; + + /** + * @brief Accumulator of a least squares fit with three unknowns. + */ + struct Fit { + /** + * @brief Add one sample of the integrated model. + * + * @param regressors The integral of the sign of the speed, the position and the speed. + * @param response The integral of the voltage. + */ + void add(const std::array& regressors, float response); + + /** + * @brief Solve for the constants. + * + * @param axis The constants, which are only written if the problem is well conditioned. + * @return True if the constants were found. + */ + bool solve(Axis& axis) const; + + std::array, 3> normal{}; + std::array moment{}; + }; + + /** + * @brief Move on to the next phase, after a rest. + * + * @param measurements The current measurements. + */ + void advance(const Measurements& measurements); + + /** + * @brief Parameters of the procedure, with the physical description of the robot. + */ + Config config; + + /** + * @brief Current step of the procedure. + */ + Phase phase{Phase::FINISHED}; + + /** + * @brief Time since the current step started. + */ + float phase_time{}; + + /** + * @brief Time left to rest before the current step drives the motors. + */ + float rest_left{}; + + /** + * @brief Wheel angles at the last iteration. + */ + ///@{ + float last_left_angle{}; + float last_right_angle{}; + ///@} + + /** + * @brief Integrals of the current step: voltage, sign of the speed and position. + */ + ///@{ + float voltage_integral{}; + float sign_integral{}; + float position{}; + ///@} + + /** + * @brief Speed at the start of the current step. + */ + float start_speed{}; + + /** + * @brief Distance traveled forward since the procedure started. + */ + float distance{}; + + /** + * @brief Voltage at which the wheels started to turn. + */ + float breakaway_voltage{}; + + /** + * @brief Fit of each axis. + */ + ///@{ + Fit linear_fit{}; + Fit angular_fit{}; + ///@} + + /** + * @brief Constants fitted for each axis. + */ + ///@{ + Axis linear{}; + Axis angular{}; + ///@} + + /** + * @brief Whether both fits were well conditioned. + */ + bool valid{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_DRIVE_IDENTIFICATION_HPP diff --git a/micras_nav/include/micras/nav/gyroscope_calibration.hpp b/micras_nav/include/micras/nav/gyroscope_calibration.hpp new file mode 100644 index 0000000..aee7081 --- /dev/null +++ b/micras_nav/include/micras/nav/gyroscope_calibration.hpp @@ -0,0 +1,176 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_GYROSCOPE_CALIBRATION_HPP +#define MICRAS_NAV_GYROSCOPE_CALIBRATION_HPP + +#include + +#include "micras/nav/measurements.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief Procedure that measures the scale factor of the gyroscope. + * + * @details The robot is placed facing a wall and turns a whole number of times in place. Its + * orientation relative to the wall is measured before and after with the two sensors that look + * forward, from the line through the two points they see, so the angle it really turned is known + * to a fraction of a degree however many turns it made. Dividing it by what the gyroscope + * integrated over the same time gives the factor that corrects the sensitivity of the gyroscope, + * which is the constant to type into the robot model. + * + * @note More turns divide the error of the two wall measurements by a larger angle, so the result + * gets better with the number of turns, as long as the robot stays in front of the wall. + */ +class GyroscopeCalibration { +public: + /** + * @brief Configuration struct for the calibration. + * + * @note The sensors are the two that look forward, to the left and to the right of the center + * line. The robot stands still for the settle time before and after turning, and the wall is + * measured over the second half of it. + */ + struct Config { + RobotModel model; + uint8_t left_sensor; + uint8_t right_sensor; + float turns; + float settle_time; + }; + + /** + * @brief Construct a new Gyroscope Calibration object. + * + * @param config The configuration for the calibration. + */ + explicit GyroscopeCalibration(const Config& config); + + /** + * @brief Start the procedure. + * + * @param pose The pose of the robot, which is held while it turns. + * @param limits The limits of the rotation in place. + */ + void start(const Pose& pose, const MotionLimits& limits); + + /** + * @brief Advance the procedure by one iteration. + * + * @param measurements The current measurements. + * @param bias The bias of the gyroscope, in rad/s after the scale correction. + * @param elapsed_time Time since the last iteration, in seconds. + * @return What the robot should be doing at this instant. + */ + Reference update(const Measurements& measurements, float bias, float elapsed_time); + + /** + * @brief Check if the procedure has ended. + * + * @return True once the wall was measured for the second time. + */ + bool is_finished() const; + + /** + * @brief Check if the wall was seen by both sensors during both measurements. + * + * @return True if the result can be trusted. + */ + bool is_valid() const; + + /** + * @brief Get the scale factor that was measured. + * + * @return The value for the gyroscope scale of the robot model. + */ + float get_scale() const; + +private: + /** + * @brief Steps of the procedure. + */ + enum class Phase : uint8_t { + BEFORE = 0, + TURNING = 1, + AFTER = 2, + FINISHED = 3, + }; + + /** + * @brief Get the orientation of the robot relative to the wall ahead of it. + * + * @param measurements The current measurements. + * @return The angle between the heading of the robot and the perpendicular of the wall. + */ + float get_wall_angle(const Measurements& measurements) const; + + /** + * @brief Parameters of the procedure, with the physical description of the robot. + */ + Config config; + + /** + * @brief Current step of the procedure. + */ + Phase phase{Phase::FINISHED}; + + /** + * @brief Time since the current step started. + */ + float phase_time{}; + + /** + * @brief Pose held by the robot, whose orientation is the one before turning. + */ + Pose pose{}; + + /** + * @brief Rotation in place. + */ + SpeedProfile spin; + + /** + * @brief Sum and number of the measurements of the wall being averaged. + */ + ///@{ + float angle_sum{}; + uint32_t angle_count{}; + ///@} + + /** + * @brief Orientation relative to the wall before turning. + */ + float angle_before{}; + + /** + * @brief Rotation reported by the raw gyroscope while turning, and the time it took. + */ + ///@{ + float raw_rotation{}; + float turning_time{}; + ///@} + + /** + * @brief Bias of the gyroscope when the robot started to turn. + */ + float bias{}; + + /** + * @brief Scale factor that was measured. + */ + float scale{1.0F}; + + /** + * @brief Whether both measurements of the wall were good. + */ + bool valid{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_GYROSCOPE_CALIBRATION_HPP diff --git a/micras_nav/include/micras/nav/measurements.hpp b/micras_nav/include/micras/nav/measurements.hpp new file mode 100644 index 0000000..cbf8ae1 --- /dev/null +++ b/micras_nav/include/micras/nav/measurements.hpp @@ -0,0 +1,53 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MEASUREMENTS_HPP +#define MICRAS_NAV_MEASUREMENTS_HPP + +#include + +#include "micras/core/vector.hpp" +#include "micras/nav/robot_model.hpp" + +namespace micras::nav { +/** + * @brief Reading of one wall sensor. + * + * @details The distance is measured along the optical axis, in meters, and comes in two versions: + * a fast one for everything that is a position, and a slow one for deciding whether there is + * something there at all. It is valid when the receiver sees anything above its noise, so a reading + * that is not valid means that nothing is within range. A receiver that saturates reports the + * shortest distance it can measure. + * + * @note The new flag is set for a single iteration after the sensor produces a value. An estimator + * that is fed the same sample twice counts it as two independent observations and becomes + * overconfident, so that is who needs it. + */ +struct WallReading { + float distance; + float slow_distance; + bool valid; + bool is_new; +}; + +/** + * @brief Everything the navigation needs from the hardware, sampled once per iteration. + * + * @details The navigation takes plain data instead of holding devices, so it depends on no proxy and + * runs anywhere this struct can be filled: on the robot, in a simulator or in a test. The wheel + * angles are cumulative, in radians, positive when the robot moves forward. The angular rate is the + * raw one around the vertical axis, in rad/s, positive counterclockwise, with the bias still in it. + * The acceleration is in the body frame, x forward and y to the left, in m/s^2. + */ +struct Measurements { + float left_wheel_angle; + float right_wheel_angle; + float angular_rate; + core::Vector acceleration; + bool imu_is_new; + std::array walls; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_MEASUREMENTS_HPP diff --git a/micras_nav/src/controller.cpp b/micras_nav/src/controller.cpp new file mode 100644 index 0000000..bfc0599 --- /dev/null +++ b/micras_nav/src/controller.cpp @@ -0,0 +1,95 @@ +/** + * @file + */ + +#include +#include + +#include "micras/nav/controller.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +Controller::Controller(const Config& config) : + config{config}, + linear_gains{compute_gains(config.linear, config.model.speed_constant(), config.model.acceleration_constant())}, + angular_gains{compute_gains( + config.angular, config.model.angular_speed_constant(), config.model.angular_acceleration_constant() + )} { } + +Controller::Command Controller::update(const Reference& reference, const State& estimate) { + const RobotModel& model = this->config.model; + + const float half_track = model.chassis.track_width / 2.0F; + const float left_speed = reference.twist.linear - reference.twist.angular * half_track; + const float right_speed = reference.twist.linear + reference.twist.angular * half_track; + + const float left_friction = std::clamp(left_speed / this->config.friction_speed, -1.0F, 1.0F); + const float right_friction = std::clamp(right_speed / this->config.friction_speed, -1.0F, 1.0F); + + const float forward_feed_forward = model.speed_constant() * reference.twist.linear + + model.acceleration_constant() * reference.acceleration.linear + + model.drive.static_friction_voltage * (right_friction + left_friction) / 2.0F; + + const float rotation_feed_forward = model.angular_speed_constant() * reference.twist.angular + + model.angular_acceleration_constant() * reference.acceleration.angular + + model.drive.static_friction_voltage * (right_friction - left_friction) / 2.0F; + + const float available = model.drive.supply_voltage * (1.0F - this->config.voltage_reserve); + const float demand = std::abs(forward_feed_forward) + std::abs(rotation_feed_forward); + + this->time_scale = demand > available ? available / demand : 1.0F; + + const Pose seen = reference.pose.relative(estimate.pose); + + const float along_error = + std::clamp(-seen.position.x, -this->config.linear.max_error, this->config.linear.max_error); + const float blend = std::clamp(reference.twist.linear / this->config.steering_blend_speed, -1.0F, 1.0F); + const float steering = + blend * std::clamp( + -this->config.steering_gain * seen.position.y, -this->config.max_steering, this->config.max_steering + ); + const float orientation_error = + std::clamp(-seen.orientation + steering, -this->config.angular.max_error, this->config.angular.max_error); + + const float forward_feedback = this->linear_gains.proportional * along_error + + this->linear_gains.derivative * (reference.twist.linear - estimate.velocity.linear); + + const float rotation_feedback = + this->angular_gains.proportional * orientation_error + + this->angular_gains.derivative * (reference.twist.angular - estimate.velocity.angular); + + this->status = { + .along_error = -seen.position.x, + .across_error = -seen.position.y, + .orientation_error = -seen.orientation, + .forward_feed_forward = forward_feed_forward, + .rotation_feed_forward = rotation_feed_forward, + .forward_feedback = forward_feedback, + .rotation_feedback = rotation_feedback, + }; + + const float to_percent = 100.0F / model.drive.supply_voltage; + + return { + .forward = to_percent * (forward_feed_forward + forward_feedback), + .rotation = to_percent * (rotation_feed_forward + rotation_feedback), + }; +} + +float Controller::get_time_scale() const { + return this->time_scale; +} + +const Controller::Status& Controller::get_status() const { + return this->status; +} + +Controller::Gains Controller::compute_gains(const Axis& axis, float speed_constant, float acceleration_constant) { + return { + .proportional = acceleration_constant * axis.natural_frequency * axis.natural_frequency, + .derivative = + std::max(2.0F * axis.damping * axis.natural_frequency * acceleration_constant - speed_constant, 0.0F), + }; +} +} // namespace micras::nav diff --git a/micras_nav/src/drive_identification.cpp b/micras_nav/src/drive_identification.cpp new file mode 100644 index 0000000..f23e070 --- /dev/null +++ b/micras_nav/src/drive_identification.cpp @@ -0,0 +1,207 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/nav/controller.hpp" +#include "micras/nav/drive_identification.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/robot_model.hpp" + +namespace micras::nav { +DriveIdentification::DriveIdentification(const Config& config) : config{config} { } + +void DriveIdentification::start(const Measurements& measurements) { + this->phase = Phase::RAMP; + this->phase_time = 0.0F; + this->rest_left = this->config.rest_time; + this->last_left_angle = measurements.left_wheel_angle; + this->last_right_angle = measurements.right_wheel_angle; + this->distance = 0.0F; + this->breakaway_voltage = 0.0F; + this->linear_fit = {}; + this->angular_fit = {}; + this->valid = false; +} + +Controller::Command DriveIdentification::update(const Measurements& measurements, float elapsed_time) { + const RobotModel& model = this->config.model; + + const float radius = model.chassis.wheel_radius; + const float left = radius * (measurements.left_wheel_angle - this->last_left_angle); + const float right = radius * (measurements.right_wheel_angle - this->last_right_angle); + + this->last_left_angle = measurements.left_wheel_angle; + this->last_right_angle = measurements.right_wheel_angle; + + const float linear_speed = (left + right) / (2.0F * elapsed_time); + const float angular_speed = model.gyroscope_scale * measurements.angular_rate; + + this->distance += (left + right) / 2.0F; + + if (this->phase == Phase::FINISHED) { + return {}; + } + + if (this->rest_left > 0.0F) { + this->rest_left -= elapsed_time; + this->start_speed = this->phase == Phase::LEFT or this->phase == Phase::RIGHT ? angular_speed : linear_speed; + return {}; + } + + this->phase_time += elapsed_time; + + const float to_volts = model.drive.supply_voltage / 100.0F; + + if (this->phase == Phase::RAMP) { + const float command = this->config.ramp_rate * this->phase_time; + + if (std::abs(linear_speed) > this->config.breakaway_speed or command >= this->config.linear_command) { + this->breakaway_voltage = command * to_volts; + this->advance(measurements); + return {}; + } + + return {.forward = command, .rotation = 0.0F}; + } + + const bool rotating = this->phase == Phase::LEFT or this->phase == Phase::RIGHT; + const bool positive = this->phase == Phase::FORWARD or this->phase == Phase::LEFT; + const float level = this->phase_time < this->config.step_time ? 1.0F : second_level; + const float magnitude = level * (rotating ? this->config.angular_command : this->config.linear_command); + const float command = positive ? magnitude : -magnitude; + const float speed = rotating ? angular_speed : linear_speed; + + this->voltage_integral += command * to_volts * elapsed_time; + this->sign_integral += (speed != 0.0F ? std::copysign(1.0F, speed) : 0.0F) * elapsed_time; + this->position += speed * elapsed_time; + + (rotating ? this->angular_fit : this->linear_fit) + .add({this->sign_integral, this->position, speed - this->start_speed}, this->voltage_integral); + + const bool too_far = + not rotating and std::abs(this->distance) > this->config.max_distance and (this->distance > 0.0F) == positive; + + if (this->phase_time >= 2.0F * this->config.step_time or too_far) { + this->advance(measurements); + return {}; + } + + return rotating ? Controller::Command{.forward = 0.0F, .rotation = command} : + Controller::Command{.forward = command, .rotation = 0.0F}; +} + +bool DriveIdentification::is_finished() const { + return this->phase == Phase::FINISHED; +} + +bool DriveIdentification::is_valid() const { + return this->valid; +} + +float DriveIdentification::get_breakaway_voltage() const { + return this->breakaway_voltage; +} + +const DriveIdentification::Axis& DriveIdentification::get_linear() const { + return this->linear; +} + +const DriveIdentification::Axis& DriveIdentification::get_angular() const { + return this->angular; +} + +RobotModel DriveIdentification::get_model() const { + RobotModel model = this->config.model; + + const RobotModel::Chassis& chassis = model.chassis; + + model.drive.static_friction_voltage = this->linear.static_friction; + model.drive.torque_constant = this->linear.speed_constant * chassis.wheel_radius / model.drive.gear_ratio; + model.drive.resistance = this->linear.acceleration_constant * 2.0F * model.drive.gear_ratio * + model.drive.torque_constant / (chassis.wheel_radius * chassis.mass); + model.chassis.yaw_inertia = this->angular.acceleration_constant * model.drive.gear_ratio * + model.drive.torque_constant * chassis.track_width / + (chassis.wheel_radius * model.drive.resistance); + + return model; +} + +void DriveIdentification::Fit::add(const std::array& regressors, float response) { + for (uint8_t i = 0; i < 3; i++) { + for (uint8_t j = 0; j < 3; j++) { + this->normal.at(i).at(j) += static_cast(regressors.at(i)) * static_cast(regressors.at(j)); + } + + this->moment.at(i) += static_cast(regressors.at(i)) * static_cast(response); + } +} + +bool DriveIdentification::Fit::solve(Axis& axis) const { + std::array, 3> system{}; + + for (uint8_t i = 0; i < 3; i++) { + for (uint8_t j = 0; j < 3; j++) { + system.at(i).at(j) = this->normal.at(i).at(j); + } + + system.at(i).at(3) = this->moment.at(i); + } + + for (uint8_t column = 0; column < 3; column++) { + uint8_t pivot = column; + + for (uint8_t row = column + 1; row < 3; row++) { + if (std::abs(system.at(row).at(column)) > std::abs(system.at(pivot).at(column))) { + pivot = row; + } + } + + if (std::abs(system.at(pivot).at(column)) < 1.0e-12) { + return false; + } + + std::swap(system.at(column), system.at(pivot)); + + for (uint8_t row = 0; row < 3; row++) { + if (row == column) { + continue; + } + + const double factor = system.at(row).at(column) / system.at(column).at(column); + + for (uint8_t k = column; k < 4; k++) { + system.at(row).at(k) -= factor * system.at(column).at(k); + } + } + } + + axis = { + .static_friction = static_cast(system.at(0).at(3) / system.at(0).at(0)), + .speed_constant = static_cast(system.at(1).at(3) / system.at(1).at(1)), + .acceleration_constant = static_cast(system.at(2).at(3) / system.at(2).at(2)), + }; + + return axis.speed_constant > 0.0F and axis.acceleration_constant > 0.0F; +} + +void DriveIdentification::advance(const Measurements& /*measurements*/) { + this->phase = static_cast(static_cast(this->phase) + 1); + this->phase_time = 0.0F; + this->rest_left = this->config.rest_time; + this->voltage_integral = 0.0F; + this->sign_integral = 0.0F; + this->position = 0.0F; + + if (this->phase == Phase::FINISHED) { + const bool linear_found = this->linear_fit.solve(this->linear); + const bool angular_found = this->angular_fit.solve(this->angular); + + this->valid = linear_found and angular_found; + } +} +} // namespace micras::nav diff --git a/micras_nav/src/gyroscope_calibration.cpp b/micras_nav/src/gyroscope_calibration.cpp new file mode 100644 index 0000000..44318d9 --- /dev/null +++ b/micras_nav/src/gyroscope_calibration.cpp @@ -0,0 +1,129 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/core/vector.hpp" +#include "micras/nav/gyroscope_calibration.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +GyroscopeCalibration::GyroscopeCalibration(const Config& config) : config{config} { } + +void GyroscopeCalibration::start(const Pose& pose, const MotionLimits& limits) { + this->phase = Phase::BEFORE; + this->phase_time = 0.0F; + this->pose = pose; + this->spin = SpeedProfile{2.0F * std::numbers::pi_v * this->config.turns, 0.0F, 0.0F, limits}; + this->angle_sum = 0.0F; + this->angle_count = 0; + this->raw_rotation = 0.0F; + this->turning_time = 0.0F; + this->valid = true; +} + +Reference GyroscopeCalibration::update(const Measurements& measurements, float bias, float elapsed_time) { + Reference reference{.pose = this->pose, .twist = {}, .acceleration = {}, .distance = 0.0F}; + + if (this->phase == Phase::FINISHED) { + return reference; + } + + this->phase_time += elapsed_time; + + if (this->phase == Phase::TURNING) { + const SpeedProfile::Sample sample = this->spin.sample(this->phase_time); + + this->raw_rotation += measurements.angular_rate * elapsed_time; + this->turning_time += elapsed_time; + + reference.pose.orientation += sample.distance; + reference.twist.angular = sample.speed; + reference.acceleration.angular = sample.acceleration; + reference.distance = sample.distance; + + if (this->phase_time >= this->spin.duration()) { + this->phase = Phase::AFTER; + this->phase_time = 0.0F; + } + + return reference; + } + + if (this->phase_time >= this->config.settle_time / 2.0F) { + const WallReading& left = measurements.walls.at(this->config.left_sensor); + const WallReading& right = measurements.walls.at(this->config.right_sensor); + + if (left.valid and right.valid) { + this->angle_sum += this->get_wall_angle(measurements); + this->angle_count++; + } + } + + if (this->phase_time < this->config.settle_time) { + return reference; + } + + const float angle = this->angle_count > 0 ? this->angle_sum / static_cast(this->angle_count) : 0.0F; + + this->valid = this->valid and this->angle_count > 0; + this->angle_sum = 0.0F; + this->angle_count = 0; + this->phase_time = 0.0F; + + if (this->phase == Phase::BEFORE) { + this->angle_before = angle; + this->bias = bias; + this->phase = Phase::TURNING; + return reference; + } + + const float rotation = this->spin.distance() + angle - this->angle_before; + + if (std::abs(this->raw_rotation) > 0.0F) { + this->scale = (rotation + this->bias * this->turning_time) / this->raw_rotation; + } else { + this->valid = false; + } + + this->phase = Phase::FINISHED; + + return reference; +} + +bool GyroscopeCalibration::is_finished() const { + return this->phase == Phase::FINISHED; +} + +bool GyroscopeCalibration::is_valid() const { + return this->valid; +} + +float GyroscopeCalibration::get_scale() const { + return this->scale; +} + +float GyroscopeCalibration::get_wall_angle(const Measurements& measurements) const { + const auto get_point = [this, &measurements](uint8_t sensor) { + const RobotModel::WallSensor& mounting = this->config.model.wall_sensors.at(sensor); + const float range = measurements.walls.at(sensor).distance; + + return core::Vector{ + .x = mounting.position.x + range * std::cos(mounting.angle), + .y = mounting.position.y + range * std::sin(mounting.angle), + }; + }; + + const core::Vector along = get_point(this->config.left_sensor) - get_point(this->config.right_sensor); + + return std::atan2(along.x, along.y); +} +} // namespace micras::nav From 73fc028d9bae552b37f4427daba22d4d808ae0bd Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 03:24:29 -0300 Subject: [PATCH 491/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Rebuild=20the=20na?= =?UTF-8?q?vigation=20around=20one=20pose=20estimate,=20a=20wall=20map=20a?= =?UTF-8?q?nd=20a=20time=20optimal=20planner?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit micras_nav now links micras_core alone. It takes plain measurements and gives a reference back, so it cannot reach the hardware and builds on any computer, and the application is the only place where proxies and navigation meet. The application names micras::hal nowhere but in main.cpp, which the simulators replace anyway. Pose. The Localizer is the one owner of the pose: an extended Kalman filter over position, orientation and gyroscope bias, in factored form so that it stays symmetric and positive in single precision. It is advanced with the encoders and the gyroscope in every state of the robot, and corrected with every range of a wall sensor that can be trusted, as a scalar measurement against the wall map: side walls, the wall ahead, and the ends of the side walls, which are the only reference along a corridor. A correction has to pass three guards: the spot the emitter lights up, enlarged by the uncertainty of the pose, has to lie on the face of one known wall, the innovation has to be plausible, and the correction is capped. Standing still measures the bias, against the encoders, so that a robot that is slowly turning cannot have its rotation taken for bias. Odometry, FollowWall, RelativePose and the grid pose kept by the application are gone. Walls. The WallObserver decides a wall by counting the readings taken while the whole spot would lie on that wall, and decides it once. A wall that never gets enough votes stays unknown instead of being guessed. The maze was being mapped from a single sample of a single sensor, since the configuration never set the sensor indices and they all defaulted to zero. The Maze is reduced to the walls, three explicit predicates for what is blocked, possibly open and known to be open, one flood fill redone in full from a preallocated queue, and a serialization of the walls with a version. The goal walls follow from the goal cells instead of being written for one maze size, and the dead end filler and its virtual walls are gone. Route. The Planner is a Dijkstra over the midpoints of the walls crossed with eight headings, where an edge is a run followed by a turn that the robot really executes and costs what the speed profile says it takes. Diagonals, the turns of 135 degrees and two sizes of the turn of 90 degrees are part of the search, and the clock stops at the line where the goal is entered. It replaces a recursive enumeration that did not finish on an open maze, ran inside the control loop under the watchdog, ranked routes with a sign error in the time of a straight, and aborted the firmware on any route with one cell between two turns. It runs in the new PLAN state with the motors stopped, where the few best candidates are timed exactly by the velocity planner, and in slices while the robot moves, where the Explorer asks it for the fastest route that could still exist and sends the robot to the unknown walls on it, until the map proves the fastest route known. Motion. The Mission decides one cell at a time, as late as it can. It only turns through a wall seen absent, watches the wall ahead while crossing a cell whose front was unknown and stops at the center if it shows up, squares up against a wall at every stop in front of one, parks where a run starts from, and ends every objective explicitly instead of through what was left in a queue. ActionQueuer, the Action classes and SpeedController are gone. Proxies. The wall sensors report distances in meters through two filters, fire in two groups so that no sensor is lit by its neighbor, read whole pairs of scans, flag new values and average their calibration, reporting its spread. The IMU delivers the raw rate, read together with the status in one transfer, and flags new samples. The locomotion gives the rotation priority when the motors saturate and returns what it applied. Application. The loop is paced by the tick and counts what it misses. The states are held by value and borrowed by the state machine, so nothing is allocated for them, and Micras moves to static storage, where a variable monitor can see it and where the arrays of the planner fit. PLAN and SAVE isolate the long jobs, the switches select the profile of a fast run, the crash detection is debounced, every proxy is checked at startup, and the identification of the drive train and the calibration of the gyroscope scale are reached with an extra long press. The constants are validated when the firmware is compiled. --- micras_core/include/micras/core/fsm.hpp | 10 +- micras_core/include/micras/core/impl/fsm.tpp | 6 +- micras_core/include/micras/core/types.hpp | 9 - micras_nav/CMakeLists.txt | 4 +- .../include/micras/nav/action_queuer.hpp | 226 ------- .../include/micras/nav/actions/base.hpp | 103 --- .../include/micras/nav/actions/move.hpp | 173 ----- .../include/micras/nav/actions/turn.hpp | 216 ------ micras_nav/include/micras/nav/costmap.hpp | 135 ---- micras_nav/include/micras/nav/explorer.hpp | 170 +++++ micras_nav/include/micras/nav/follow_wall.hpp | 173 ----- .../include/micras/nav/impl/explorer.tpp | 135 ++++ .../include/micras/nav/impl/localizer.tpp | 134 ++++ micras_nav/include/micras/nav/impl/maze.tpp | 265 ++++++++ .../include/micras/nav/impl/mission.tpp | 536 +++++++++++++++ .../include/micras/nav/impl/planner.tpp | 613 ++++++++++++++++++ .../include/micras/nav/impl/wall_model.tpp | 140 ++++ .../include/micras/nav/impl/wall_observer.tpp | 102 +++ micras_nav/include/micras/nav/localizer.hpp | 396 +++++++++++ micras_nav/include/micras/nav/maze.hpp | 234 ++++--- micras_nav/include/micras/nav/mission.hpp | 437 +++++++++++++ micras_nav/include/micras/nav/odometry.hpp | 124 ---- micras_nav/include/micras/nav/planner.hpp | 498 ++++++++++++++ .../include/micras/nav/route_compiler.hpp | 53 ++ .../include/micras/nav/speed_controller.hpp | 111 ---- micras_nav/include/micras/nav/state.hpp | 47 +- micras_nav/include/micras/nav/wall_model.hpp | 208 ++++++ .../include/micras/nav/wall_observer.hpp | 108 +++ micras_nav/src/action_queuer.cpp | 413 ------------ micras_nav/src/costmap.cpp | 152 ----- micras_nav/src/follow_wall.cpp | 152 ----- micras_nav/src/localizer.cpp | 345 ++++++++++ micras_nav/src/maze.cpp | 365 ----------- micras_nav/src/odometry.cpp | 70 -- micras_nav/src/route_compiler.cpp | 83 +++ micras_nav/src/speed_controller.cpp | 54 -- micras_nav/src/state.cpp | 42 -- micras_nav/src/wall_model.cpp | 86 +++ .../micras/proxy/impl/wall_sensors.tpp | 108 ++- micras_proxy/include/micras/proxy/imu.hpp | 21 +- .../include/micras/proxy/locomotion.hpp | 32 +- .../include/micras/proxy/wall_sensors.hpp | 176 ++++- micras_proxy/src/imu.cpp | 13 +- micras_proxy/src/locomotion.cpp | 24 +- 44 files changed, 4736 insertions(+), 2766 deletions(-) delete mode 100644 micras_nav/include/micras/nav/action_queuer.hpp delete mode 100644 micras_nav/include/micras/nav/actions/base.hpp delete mode 100644 micras_nav/include/micras/nav/actions/move.hpp delete mode 100644 micras_nav/include/micras/nav/actions/turn.hpp delete mode 100644 micras_nav/include/micras/nav/costmap.hpp create mode 100644 micras_nav/include/micras/nav/explorer.hpp delete mode 100644 micras_nav/include/micras/nav/follow_wall.hpp create mode 100644 micras_nav/include/micras/nav/impl/explorer.tpp create mode 100644 micras_nav/include/micras/nav/impl/localizer.tpp create mode 100644 micras_nav/include/micras/nav/impl/maze.tpp create mode 100644 micras_nav/include/micras/nav/impl/mission.tpp create mode 100644 micras_nav/include/micras/nav/impl/planner.tpp create mode 100644 micras_nav/include/micras/nav/impl/wall_model.tpp create mode 100644 micras_nav/include/micras/nav/impl/wall_observer.tpp create mode 100644 micras_nav/include/micras/nav/localizer.hpp create mode 100644 micras_nav/include/micras/nav/mission.hpp delete mode 100644 micras_nav/include/micras/nav/odometry.hpp create mode 100644 micras_nav/include/micras/nav/planner.hpp create mode 100644 micras_nav/include/micras/nav/route_compiler.hpp delete mode 100644 micras_nav/include/micras/nav/speed_controller.hpp create mode 100644 micras_nav/include/micras/nav/wall_model.hpp create mode 100644 micras_nav/include/micras/nav/wall_observer.hpp delete mode 100644 micras_nav/src/action_queuer.cpp delete mode 100644 micras_nav/src/costmap.cpp delete mode 100644 micras_nav/src/follow_wall.cpp create mode 100644 micras_nav/src/localizer.cpp delete mode 100644 micras_nav/src/maze.cpp delete mode 100644 micras_nav/src/odometry.cpp create mode 100644 micras_nav/src/route_compiler.cpp delete mode 100644 micras_nav/src/speed_controller.cpp create mode 100644 micras_nav/src/wall_model.cpp diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 239e7ea..70267a7 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -7,7 +7,6 @@ #include #include -#include namespace micras::core { /** @@ -84,11 +83,14 @@ class TFsm { explicit TFsm(uint8_t initial_state_id); /** - * @brief Add a state to the FSM, taking ownership of it. + * @brief Add a state to the FSM. + * + * @note The state is borrowed, not owned: whoever owns the machine holds its states by value, + * next to it, so that nothing is allocated and they live exactly as long as the machine does. * * @param state The state to be added. */ - void add_state(std::unique_ptr state); + void add_state(FsmState& state); /** * @brief Run the FSM current state to compute the next state. @@ -110,7 +112,7 @@ class TFsm { /** * @brief States of the machine, indexed by their id. */ - std::array, num_of_states> states{}; + std::array states{}; /** * @brief Id of the state currently running. diff --git a/micras_core/include/micras/core/impl/fsm.tpp b/micras_core/include/micras/core/impl/fsm.tpp index 323c267..b42b615 100644 --- a/micras_core/include/micras/core/impl/fsm.tpp +++ b/micras_core/include/micras/core/impl/fsm.tpp @@ -7,16 +7,14 @@ #include #include -#include -#include namespace micras::core { template TFsm::TFsm(uint8_t initial_state_id) : current_state_id{initial_state_id} { } template -void TFsm::add_state(std::unique_ptr state) { - this->states.at(state->get_id()) = std::move(state); +void TFsm::add_state(FsmState& state) { + this->states.at(state.get_id()) = &state; } template diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index 981df50..081c05c 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -8,15 +8,6 @@ #include namespace micras::core { -/** - * @brief Possible values for the observation from the sensors. - */ -struct Observation { - bool left{}; - bool front{}; - bool right{}; -}; - /** * @brief Possible objectives of the robot. */ diff --git a/micras_nav/CMakeLists.txt b/micras_nav/CMakeLists.txt index d981cc5..ff12eb6 100644 --- a/micras_nav/CMakeLists.txt +++ b/micras_nav/CMakeLists.txt @@ -14,6 +14,8 @@ target_include_directories(${PROJECT_NAME} PUBLIC include ) +# The navigation takes plain data and gives plain data back, so it links nothing that can reach the +# hardware and builds anywhere micras::core does target_link_libraries(${PROJECT_NAME} PUBLIC - micras::proxy + micras::core ) diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp deleted file mode 100644 index b63ef82..0000000 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ /dev/null @@ -1,226 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_ACTION_QUEUER_HPP -#define MICRAS_NAV_ACTION_QUEUER_HPP - -#include -#include -#include -#include - -#include "micras/nav/actions/move.hpp" -#include "micras/nav/actions/turn.hpp" - -namespace micras::nav { -/** - * @brief Class to queue actions for the robot. - */ -class ActionQueuer { -public: - /** - * @brief Enum for the exploration action types. - */ - enum class ActionType : uint8_t { - STOP = 0, - START = 1, - MOVE_FORWARD = 2, - TURN = 3, - SPIN = 4, - DIAGONAL = 5, - }; - - /** - * @brief Configuration struct for the ActionQueuer class. - */ - struct Config { - struct Dynamic { - float max_linear_speed; - float max_linear_acceleration; - float max_linear_deceleration; - float max_centrifugal_acceleration; - float max_angular_acceleration; - }; - - float cell_size; - float start_offset; - float curve_safety_margin; - Dynamic exploring; - Dynamic solving; - }; - - /** - * @brief Construct a new ActionQueuer object. - * - * @param config Configuration for the ActionQueuer. - */ - explicit ActionQueuer(Config config); - - /** - * @brief Push an exploring action to the queue. - * - * @param origin_pose Origin pose where the action will be performed. - * @param target_position Target position to move to. - */ - void push_exploring(const GridPose& origin_pose, const GridPoint& target_position); - - /** - * @brief Pop an action from the queue. - * - * @return Shared pointer to the action. - */ - std::shared_ptr pop(); - - /** - * @brief Check if the action queue is empty. - * - * @return True if the action queue is empty, false otherwise. - */ - bool empty() const; - - /** - * @brief Fill the action queue with a sequence of actions to the end. - */ - void recompute(const std::list& best_route, bool add_start = true); - - /** - * @brief Get the total time to complete the actions in the queue. - * - * @return Total time to complete the actions in the queue. - */ - float get_total_time() const; - - /** - * @brief Get the actions to perform from the origin pose to the target pose. - * - * @param origin_pose Origin pose of the robot. - * @param target_pose Target pose to move to. - * @param cell_size Size of the cells in the grid. - * @return List of actions to perform. - */ - static Action::Id get_action(const GridPose& origin_pose, const GridPoint& target_point, float cell_size); - -private: - /** - * @brief Struct to hold the parameters for a curve. - */ - struct CurveParameters { - float curve_radius; - float max_angular_speed; - float linear_speed; - float side_displacement; - float forward_displacement; - }; - - /** - * @brief Enum for the types of curves. - */ - enum class CurveType : uint8_t { - REGULAR_45 = 0, - REGULAR_90 = 1, - DIAGONAL_90 = 2, - REGULAR_135 = 3, - REGULAR_180 = 4, - NUMBER_OF_CURVES = 5, - }; - - /** - * @brief Add diagonals to the list of actions. - * - * @param actions List of actions to add diagonals to. - */ - void add_diagonals(std::list& actions) const; - - /** - * @brief Join consecutive actions of the same type. - * - * @param actions List of actions to join. - */ - void join_actions(std::list& actions); - - /** - * @brief Get the trim distances for a turn action. - * - * @param action_before Id of the action before the turn action. - * @param turn_action Turn action to get the trim distances for. - * @return Pair of trim distances before and after the turn. - */ - std::pair get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action); - - /** - * @brief Get the curve parameters for a given angle and diagonal status. - * - * @param angle Angle of the curve. - * @param is_diagonal True if the curve is diagonal, false otherwise. - * @return Reference to the CurveParameters struct for the given angle and diagonal status. - */ - CurveParameters& get_curve_parameters(float angle, bool is_diagonal); - - /** - * @brief Compute the curve parameters for a given angle and diagonal status. - * - * @param angle Angle of the curve. - * @param is_diagonal True if the curve is diagonal, false otherwise. - */ - void compute_curve_parameters(float angle, bool is_diagonal); - - /** - * @brief Calculate the radius of the curve based on the angle, cell size, and safety margin. - * - * @param angle Angle of the curve in radians. - * @param cell_size Size of the maze cells. - * @param safety_margin Minimum distance between the robot and the wall during the curve. - * @param is_diagonal True if the curve is diagonal, false otherwise. - * @return Calculated radius of the curve. - */ - static constexpr float calculate_curve_radius(float angle, float cell_size, float safety_margin, bool is_diagonal); - - /** - * @brief Size of the cells in the grid. - */ - float cell_size; - - /** - * @brief Distance of the robot to the on on the start cell. - */ - float start_offset; - - /** - * @brief Minimum distance between the robot and the wall during the curve. - */ - float curve_safety_margin; - - /** - * @brief Dynamic solving parameters. - */ - Config::Dynamic solving_params; - - /** - * @brief Pre-computed curve parameters for different angles. - */ - std::array curves_parameters{}; - - /** - * @brief Pre-built actions to use in the exploration. - */ - ///@{ - std::shared_ptr stop; - std::shared_ptr start; - std::shared_ptr move_forward; - std::shared_ptr move_half; - std::shared_ptr move_to_turn; - std::shared_ptr move_from_turn; - std::shared_ptr turn_left; - std::shared_ptr turn_right; - std::shared_ptr turn_back; - ///@} - - /** - * @brief Queue of actions to be performed. - */ - std::deque> action_queue; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_ACTION_QUEUER_HPP diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp deleted file mode 100644 index d14334f..0000000 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ /dev/null @@ -1,103 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_BASE_ACTION_HPP -#define MICRAS_NAV_BASE_ACTION_HPP - -#include "micras/nav/state.hpp" - -namespace micras::nav { -/** - * @brief Base class for actions. - */ -class Action { -public: - struct Id { - uint8_t type; - float value; - }; - - /** - * @brief Constructor for the Action class. - * - * @param action_id The ID of the action. - * @param follow_wall Whether the robot can follow walls while executing this action. - * @param total_time The total time it takes to perform the action. - */ - Action(const Id& action_id, bool follow_wall, float total_time = 0.0F) : - id{action_id}, follow_wall{follow_wall}, total_time{total_time} { } - - /** - * @brief Virtual destructor for the Action class. - */ - virtual ~Action() = default; - - /** - * @brief Get the desired speeds for the robot. - * - * @param current_pose The current pose of the robot. - * @param time_step The time step for the action in seconds. - * @return The desired speeds for the robot. - */ - virtual Twist get_speeds(const Pose& current_pose, float time_step) = 0; - - /** - * @brief Check if the action is finished. - * - * @param current_pose The current pose of the robot. - * @return True if the action is finished, false otherwise. - */ - virtual bool finished(const Pose& current_pose) = 0; - - /** - * @brief Get the ID of the action. - * - * @return The ID of the action. - */ - const Id& get_id() const { return id; } - - /** - * @brief Check if the action allow the robot to follow walls. - * - * @return True if the action allows the robot to follow walls, false otherwise. - */ - bool allow_follow_wall() const { return follow_wall; } - - /** - * @brief Get the total time it takes to perform the action. - * - * @return The total time of the action in seconds. - */ - float get_total_time() const { return this->total_time; }; - -protected: - /** - * @brief Special member functions declared as default. - */ - ///@{ - Action(const Action&) = default; - Action(Action&&) = default; - Action& operator=(const Action&) = default; - Action& operator=(Action&&) = default; - ///@} - -private: - /** - * @brief The ID of the action. - */ - Id id; - - /** - * @brief Whether the robot can follow walls while executing this action. - */ - bool follow_wall; - - /** - * @brief The total time it takes to perform the action. - */ - float total_time; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_BASE_ACTION_HPP diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp deleted file mode 100644 index 477caca..0000000 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ /dev/null @@ -1,173 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_MOVE_ACTION_HPP -#define MICRAS_NAV_MOVE_ACTION_HPP - -#include -#include - -#include "micras/nav/actions/base.hpp" - -namespace micras::nav { -/** - * @brief Action to move the robot a certain distance forward. - */ -class MoveAction : public Action { -public: - /** - * @brief Configuration parameters for the MoveAction. - */ - struct Config { - float start_speed; - float end_speed; - float max_speed; - float max_acceleration; - float max_deceleration; - }; - - /** - * @brief Construct a new Move Action object. - * - * @param action_type The type of the action to be performed. - * @param distance Distance to move in meters. - * @param config Dynamic parameters for the action. - * @param follow_wall Whether the robot can follow wall while executing this action. - */ - MoveAction(uint8_t action_type, float distance, const Config& config, bool follow_wall = true) : - Action{ - {.type = action_type, .value = distance}, - follow_wall, - calculate_total_time( - distance, config.start_speed, config.end_speed, config.max_speed, config.max_acceleration, - config.max_deceleration - ) - }, - distance(distance), - start_speed_2(config.start_speed * config.start_speed), - end_speed_2(config.end_speed * config.end_speed), - max_speed{config.max_speed}, - max_acceleration_doubled{2.0F * config.max_acceleration}, - max_deceleration_doubled{2.0F * config.max_deceleration}, - decelerate_distance{ - (end_speed_2 - start_speed_2 + max_deceleration_doubled * distance) / - (max_acceleration_doubled + max_deceleration_doubled) - } { } - - /** - * @brief Get the desired speeds for the robot to complete the action. - * - * @param current_pose The current pose of the robot. - * @param time_step The time step for the action in seconds. - * @return The desired speeds for the robot to complete the action. - * - * @details The desired velocity is calculated from the linear displacement based on the Torricelli equation. - */ - Twist get_speeds(const Pose& current_pose, float time_step) override { - const float current_distance = current_pose.position.magnitude(); - Twist twist{}; - - if (current_distance < this->decelerate_distance) { - twist = { - .linear = std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance) + - time_step * this->max_acceleration_doubled / 2.0F, - .angular = 0.0F, - }; - } else { - twist = { - .linear = - std::sqrt(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), - .angular = 0.0F, - }; - } - - twist.linear = std::clamp(twist.linear, 0.0F, this->max_speed); - - return twist; - } - - /** - * @brief Check if the action is finished. - * - * @param pose The current pose of the robot. - * @return True if the action is finished, false otherwise. - */ - bool finished(const Pose& pose) override { return pose.position.magnitude() >= this->distance; } - - /** - * @brief Calculate the total time to complete the action. - * - * @param distance Distance to move in meters. - * @param start_speed Initial speed in m/s. - * @param end_speed Final speed in m/s. - * @param max_speed Maximum speed in m/s. - * @param max_acceleration Maximum acceleration in m/s^2. - * @param max_deceleration Maximum deceleration in m/s^2. - * @return Total time to complete the action in seconds. - */ - static constexpr float calculate_total_time( - float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, - float max_deceleration - ) { - const float peak_velocity = std::sqrt( - (2.0F * distance * max_acceleration * max_deceleration + start_speed * start_speed * max_deceleration + - end_speed * end_speed * max_acceleration) / - (max_acceleration + max_deceleration) - ); - - if (peak_velocity < max_speed) { - return (peak_velocity - start_speed) / max_acceleration + (peak_velocity - end_speed) / max_deceleration; - } - - const float acceleration_time = (max_speed - start_speed) / max_acceleration; - const float deceleration_time = (max_speed - end_speed) / max_deceleration; - - const float acceleration_distance = - (start_speed * start_speed + max_speed * max_speed) / (2.0F * max_acceleration); - const float deceleration_distance = (max_speed * max_speed + end_speed * end_speed) / (2.0F * max_deceleration); - const float constant_speed_distance = distance - acceleration_distance - deceleration_distance; - const float constant_speed_time = constant_speed_distance / max_speed; - - return acceleration_time + deceleration_time + constant_speed_time; - } - -private: - /** - * @brief Distance to move in meters. - */ - float distance; - - /** - * @brief Initial linear speed squared. - */ - float start_speed_2; - - /** - * @brief Final linear speed squared. - */ - float end_speed_2; - - /** - * @brief Maximum linear speed in m/s. - */ - float max_speed; - - /** - * @brief Maximum linear acceleration multiplied by 2. - */ - float max_acceleration_doubled; - - /** - * @brief Maximum linear deceleration multiplied by 2. - */ - float max_deceleration_doubled; - - /** - * @brief Distance from the start where the robot should start to decelerate in meters. - */ - float decelerate_distance; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_MOVE_ACTION_HPP diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp deleted file mode 100644 index 7a37707..0000000 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ /dev/null @@ -1,216 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_TURN_ACTION_HPP -#define MICRAS_NAV_TURN_ACTION_HPP - -#include -#include -#include - -#include "micras/nav/actions/base.hpp" - -namespace micras::nav { -/** - * @brief Action to turn the robot following a curve radius. - */ -class TurnAction : public Action { -public: - /** - * @brief Configuration parameters for the TurnAction. - */ - struct Config { - float max_angular_speed; - float linear_speed; - float max_angular_acceleration; - }; - - /** - * @brief Construct a new Turn Action object. - * - * @param action_type The type of the action to be performed. - * @param angle Angle to turn in radians. - * @param config Dynamic parameters for the action. - */ - TurnAction(uint8_t action_type, float angle, const Config& config) : - Action{ - {.type = action_type, .value = angle}, - false, - calculate_total_time(angle, config.max_angular_speed, config.max_angular_acceleration) - }, - angle{angle}, - max_angular_speed{config.max_angular_speed}, - linear_speed{config.linear_speed}, - acceleration{config.max_angular_acceleration} { } - - /** - * @brief Get the desired speeds for the robot to complete the action. - * - * @param current_pose The current pose of the robot. - * @param time_step The time step for the action in seconds. - * @return The desired speeds for the robot to complete the action. - */ - Twist get_speeds(const Pose& /*current_pose*/, float time_step) override { - this->elapsed_time += time_step; - Twist twist{}; - - if (this->elapsed_time < this->get_total_time() / 2.0F) { - twist = { - .linear = linear_speed, - .angular = this->acceleration * this->elapsed_time, - }; - } else { - twist = { - .linear = linear_speed, - .angular = this->acceleration * (this->get_total_time() - this->elapsed_time), - }; - } - - twist.angular = std::copysign(std::clamp(twist.angular, 0.0F, this->max_angular_speed), this->angle); - - return twist; - } - - /** - * @brief Check if the action is finished. - * - * @param current_pose The current pose of the robot. - * @return True if the action is finished, false otherwise. - */ - bool finished(const Pose& /*current_pose*/) override { - if (this->elapsed_time >= this->get_total_time()) { - this->elapsed_time = 0.0F; - return true; - } - - return false; - } - - /** - * @brief Calculate the maximum angular speed for a given curve radius and linear speed. - * - * @param angle Angle to turn in radians. - * @param curve_radius Radius of the curve in meters. - * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. - * @param max_centripetal_acceleration Maximum centripetal acceleration in m/s^2. - * @return Maximum angular speed in rad/s. - * - * @details Maximum angular velocity is computed to generate a curve equivalent in displacement to one that - * maintains the desired radius of curvature throughout the entire turn. - * If linear speed is zero, a minimal angular speed is assigned. - */ - static constexpr float calculate_max_angular_speed( - float angle, float curve_radius, float max_angular_acceleration, float max_centripetal_acceleration - ) { - if (curve_radius == 0.0F) { - return max_angular_acceleration * 0.01F; - } - - const float correction_factor = - 14.35F - 13.57F * std::cos(std::abs(angle) - 2) - 10.0F / max_angular_acceleration; - - const float transformed_acceleration = - std::pow(1.0F - std::cos(angle), 2.0F) * correction_factor * max_angular_acceleration; - - const float max_angular_speed = std::sqrt( - (max_centripetal_acceleration * transformed_acceleration) / - (curve_radius * transformed_acceleration - max_centripetal_acceleration) - ); - - if (std::isnan(max_angular_speed)) { - std::terminate(); - } - - return max_angular_speed; - } - - /** - * @brief Calculate the total side displacement for a curve. - * - * @param angle Angle to turn in radians. - * @param max_angular_speed Maximum angular speed in rad/s. - * @param linear_speed Linear speed in m/s. - * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. - * @return Side displacement in meters. - */ - static constexpr float calculate_side_displacement( - float angle, float max_angular_speed, float linear_speed, float max_angular_acceleration - ) { - const float transformed_speed = max_angular_speed / (1.0F - std::cos(angle)); - const float correction_factor = - 14.35F - 13.57F * std::cos(std::abs(angle) - 2) - 10.0F / max_angular_acceleration; - - return linear_speed * - (1.0F / transformed_speed + transformed_speed / (correction_factor * max_angular_acceleration)); - } - - /** - * @brief Calculate the total forward displacement for a curve. - * - * @param angle Angle to turn in radians. - * @param max_angular_speed Maximum angular speed in rad/s. - * @param linear_speed Linear speed in m/s. - * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. - * @return Forward displacement in meters. - */ - static constexpr float calculate_forward_displacement( - float angle, float max_angular_speed, float linear_speed, float max_angular_acceleration - ) { - const float transformed_speed = max_angular_speed / std::sin(std::abs(angle)); - const float correction_factor = - 14.12F - 13.3F * std::cos(std::abs(angle) - 1.146F) - 10.0F / max_angular_acceleration; - - return linear_speed * - (1.0F / transformed_speed + transformed_speed / (correction_factor * max_angular_acceleration)); - } - - /** - * @brief Calculate the total time to complete the action. - * - * @param angle Angle to turn in radians. - * @param max_angular_speed Maximum angular speed in rad/s. - * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. - * @return Total time to complete the action in seconds. - */ - static constexpr float calculate_total_time(float angle, float max_angular_speed, float max_angular_acceleration) { - const float peak_velocity = std::sqrt(std::abs(angle) * max_angular_acceleration); - - if (peak_velocity < max_angular_speed) { - return 2.0F * peak_velocity / max_angular_acceleration; - } - - const float acceleration_time = max_angular_speed / max_angular_acceleration; - - return std::abs(angle) / max_angular_speed + acceleration_time; - } - -private: - /** - * @brief Angle to turn in radians. - */ - float angle; - - /** - * @brief Maximum angular speed in rad/s. - */ - float max_angular_speed; - - /** - * @brief Linear speed in m/s while turning. - */ - float linear_speed; - - /** - * @brief Maximum angular acceleration in rad/s^2. - */ - float acceleration; - - /** - * @brief Elapsed time in seconds since the action started. - */ - float elapsed_time{}; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_TURN_ACTION_HPP diff --git a/micras_nav/include/micras/nav/costmap.hpp b/micras_nav/include/micras/nav/costmap.hpp deleted file mode 100644 index 2506db4..0000000 --- a/micras_nav/include/micras/nav/costmap.hpp +++ /dev/null @@ -1,135 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_COSTMAP_HPP -#define MICRAS_NAV_COSTMAP_HPP - -#include -#include - -#include "micras/nav/grid_pose.hpp" - -namespace micras::nav { -constexpr int16_t max_cost{0x1FFF}; - -/** - * @brief Class for managing a layered costmap. - * - * @tparam width The width of the maze. - * @tparam height The height of the maze. - * @tparam layers The number of layers in the costmap. - */ -template -class Costmap { -public: - /** - * @brief Type to store the state of a wall. - */ - enum class WallState : uint8_t { - UNKNOWN = 0, - NO_WALL = 1, - WALL = 2, - VIRTUAL = 3, - }; - - /** - * @brief Type to store the information of a cell in the maze. - */ - struct Cell { - Cell() { costs.fill(max_cost); } - - std::array walls{WallState::UNKNOWN}; - std::array costs; - }; - - /** - * @brief Construct a new Costmap object. - */ - Costmap(); - - /** - * @brief Compute the costmap of a given layer from a reference point using the flood fill algorithm. - * - * @param reference The reference point to start the flood fill algorithm. - * @param layer The layer to compute the costmap for. - */ - void compute(const GridPoint& reference, uint8_t layer); - - /** - * @brief Reset the costs and recompute the new values locally for a given layer. - * - * @param reference The reference point to start resetting the stored cost. - * @param layer The layer to recompute. - */ - void recompute(const GridPoint& reference, uint8_t layer); - - /** - * @brief Return the cell at the given position. - * - * @param position The position of the cell. - * @return The cell at the given position. - */ - const Cell& get_cell(const GridPoint& position) const; - - /** - * @brief Get the cost of a cell at a given position and layer. - * - * @param position The position of the cell. - * @param layer The layer to get the cost for. - * @return The cost of the cell at the given position and layer. - */ - int16_t get_cost(const GridPoint& position, uint8_t layer) const; - - /** - * @brief Update the cost of a cell at a given position and layer. - * - * @param position The position of the cell. - * @param layer The layer to update the cost for. - * @param cost The new cost to set. - */ - void update_cost(const GridPoint& position, uint8_t layer, int16_t cost); - - /** - * @brief Check whether there is a wall at the front of a given pose. - * - * @param pose The pose to check.\ - * @param consider_virtual Whether to consider virtual walls. - * @return True if there is a wall, false otherwise. - */ - bool has_wall(const GridPose& pose, bool consider_virtual = false) const; - - /** - * @brief Update the existence of a wall in the maze. - * - * @param pose The pose of the robot. - * @param wall Whether there is a wall at the front of a given pose. - */ - bool update_wall(const GridPose& pose, bool wall); - - /** - * @brief Add a virtual wall to the costmap at a given position and side. - * - * @param pose The pose to add the virtual wall at. - */ - void add_virtual_wall(const GridPose& pose); - -private: - /** - * @brief Get the cell at the given position. - * - * @param position The position of the cell. - * @return The cell at the given position. - */ - Cell& cell_on_position(const GridPoint& position); - - /** - * @brief Cells matrix representing the maze. - */ - std::array, height> cells{}; -}; -} // namespace micras::nav - -#include "../src/costmap.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) - -#endif // MICRAS_NAV_COSTMAP_HPP diff --git a/micras_nav/include/micras/nav/explorer.hpp b/micras_nav/include/micras/nav/explorer.hpp new file mode 100644 index 0000000..72b4b6a --- /dev/null +++ b/micras_nav/include/micras/nav/explorer.hpp @@ -0,0 +1,170 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_EXPLORER_HPP +#define MICRAS_NAV_EXPLORER_HPP + +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" + +namespace micras::nav { +/** + * @brief Choice of what is still worth visiting, and of when the search is over. + * + * @details The fastest route that could exist is the one the planner finds when every wall that was + * never seen is taken as absent. If that route crosses no unknown wall, the map already proves that + * nothing faster exists and the search is over. Otherwise the cells next to the unknown walls it + * crosses are what is worth visiting, and the question is asked again once the map has changed. The + * test uses the very cost the fast run is chosen by, so a faster route cannot stay hidden, and only + * cells on some best conceivable route are ever targeted. + * + * The fastest route depends on the profile of the run, so the test is repeated for every profile the + * map has to be good for and the targets are put together. + * + * @note The planner is advanced a bounded number of nodes per call, which lets all of this happen + * while the robot is moving. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TExplorer { +public: + /** + * @brief How much slower than the best a candidate route can be and still be looked at. + * + * @note The search prices a route to within a few hundredths of a second of what the velocity + * planner will make of it, so a candidate this close to the best may turn out to be the best. + */ + static constexpr float candidate_window{0.05F}; + + /** + * @brief Largest number of cells that can be targeted at once. + */ + static constexpr uint8_t max_targets{64}; + + /** + * @brief Construct a new TExplorer object. + * + * @param planner The planner, borrowed for the lifetime of the explorer and shared with whoever + * plans the fast run, which never happens at the same time. + * @param profiles The profiles the map has to be good for, borrowed for the same time. + */ + TExplorer(TPlanner& planner, std::span profiles); + + /** + * @brief Forget the targets and start asking again. + */ + void reset(); + + /** + * @brief Advance the planning. + * + * @param maze The map of the walls. + * @param max_nodes The largest number of nodes the planner may expand in this call. + * @return True if the targets changed. + */ + bool update(const TMaze& maze, uint32_t max_nodes); + + /** + * @brief Check if an answer for the current map exists yet. + * + * @return True once every profile was planned at least once since the last reset. + */ + bool has_targets() const; + + /** + * @brief Check if the map proves that the fastest route of every profile is known. + * + * @return True if nothing is left that is worth visiting. + */ + bool is_complete() const; + + /** + * @brief Get the cells worth visiting. + * + * @return The cells next to the unknown walls of the fastest conceivable routes. + */ + std::span get_targets() const; + +private: + /** + * @brief Add the cells on both sides of a wall to the targets being collected. + * + * @param wall The cell and the side of it where the wall is. + */ + void add_target(const GridPose& wall); + + /** + * @brief Planner used to find the fastest conceivable routes. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + TPlanner& planner; + + /** + * @brief Profiles the map has to be good for. + */ + std::span profiles; + + /** + * @brief Route being inspected, kept to reuse its storage. + */ + Route route; + + /** + * @brief Cells worth visiting, as of the last complete round. + */ + std::array targets{}; + + /** + * @brief Number of cells worth visiting. + */ + uint8_t number_of_targets{}; + + /** + * @brief Cells collected by the round in progress. + */ + std::array collected{}; + + /** + * @brief Number of cells collected by the round in progress. + */ + uint8_t number_collected{}; + + /** + * @brief Index of the profile being planned. + */ + uint8_t profile_index{}; + + /** + * @brief Whether the planner is in the middle of a search of this explorer. + */ + bool planning{}; + + /** + * @brief Whether a round was completed since the last reset. + */ + bool answered{}; + + /** + * @brief Revision of the map the round in progress started from. + */ + uint32_t round_revision{}; + + /** + * @brief Revision of the map the current targets were computed for. + */ + uint32_t answered_revision{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/explorer.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_EXPLORER_HPP diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp deleted file mode 100644 index 8aa6519..0000000 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ /dev/null @@ -1,173 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_FOLLOW_WALL -#define MICRAS_NAV_FOLLOW_WALL - -#include "micras/core/pid_controller.hpp" -#include "micras/core/types.hpp" -#include "micras/nav/state.hpp" -#include "micras/proxy/wall_sensors.hpp" - -namespace micras::nav { -/** - * @brief Class to follow the side walls using a PID controller. - */ -class FollowWall { -public: - /** - * @brief Configuration struct for the FollowWall class. - */ - struct Config { - core::PidController::Config pid; - core::WallSensorsIndex wall_sensor_index{}; - float max_angular_acceleration{}; - float cell_size{}; - float post_threshold{}; - float post_reference{}; - float post_clearance{}; - }; - - /** - * @brief Construct a new FollowWall object. - * - * @param wall_sensors The wall sensors of the robot. - * @param config The configuration for the FollowWall class. - */ - FollowWall(const proxy::TWallSensors<4>& wall_sensors, const Config& config); - - /** - * @brief Calculate the desired angular speed to follow wall. - * - * @param elapsed_time The time elapsed since the last update. - * @param state The current state of the robot. - * @return The desired angular speed to follow wall. - */ - float compute_angular_correction(float elapsed_time, State& state); - - /** - * @brief Get the observation of the walls around the robot. - * - * @return Observations from all sensors. - */ - core::Observation get_observation() const; - - /** - * @brief Check if the robot is currently following the left wall. - * - * @return True if following the left wall, false otherwise. - */ - bool get_is_following_left() const; - - /** - * @brief Check if the robot is currently following the right wall. - * - * @return True if following the right wall, false otherwise. - */ - bool get_is_following_right() const; - -private: - /** - * @brief Check if the robot saw a post. - * - * @param pose The current pose of the robot. - * @return True if the robot saw a post, false otherwise. - * - * @details This function uses the derivative of the distance sensors readings with respect - * to the robot's traveled distance, comparing it to a threshold to detect posts. - */ - bool check_posts(const Pose& pose); - - /** - * @brief Reset the PID controller and the relative pose. - */ - void reset(); - - /** - * @brief Clear the positional error of the robot. - * - * @param state The state of the robot. - * @param error The error to be cleared. - */ - static void clear_position_error(State& state, float error); - - /** - * @brief Wall sensors of the robot. - */ - // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot - const proxy::TWallSensors<4>& wall_sensors; - - /** - * @brief PID controller for the wall following. - */ - core::PidController pid; - - /** - * @brief Index of each of the wall sensors used for wall following. - */ - core::WallSensorsIndex sensor_index; - - /** - * @brief Maximum angular acceleration of the robot. - */ - float max_angular_acceleration; - - /** - * @brief Size of the cells in the map. - */ - float cell_size; - - /** - * @brief Derivative threshold for detecting posts. - */ - float post_threshold; - - /** - * @brief Cell advance of the robot when it starts to see a post. - */ - float post_reference{}; - - /** - * @brief Distance the robot should travel after seeing a post to start making observations. - */ - float post_clearance{}; - - /** - * @brief Last grid pose of the robot. - */ - GridPose last_grid_pose{}; - - /** - * @brief Last pose of the robot. - */ - Pose last_pose{}; - - /** - * @brief Flag to indicate if the robot is currently following the left wall. - */ - bool following_left{true}; - - /** - * @brief Flag to indicate if the robot is currently following the right wall. - */ - bool following_right{true}; - - /** - * @brief Last reading measured by the left wall sensor, used to compute the derivative of the distance sensors. - */ - float last_left_reading{}; - - /** - * @brief Last reading measured by the right wall sensor, used to compute the derivative of the distance sensors. - */ - float last_right_reading{}; - - /** - * @brief Last response returned by the Follow Wall. - */ - float last_response{}; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_FOLLOW_WALL diff --git a/micras_nav/include/micras/nav/impl/explorer.tpp b/micras_nav/include/micras/nav/impl/explorer.tpp new file mode 100644 index 0000000..175b845 --- /dev/null +++ b/micras_nav/include/micras/nav/impl/explorer.tpp @@ -0,0 +1,135 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_EXPLORER_TPP +#define MICRAS_NAV_EXPLORER_TPP + +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/planner.hpp" + +namespace micras::nav { +template +TExplorer::TExplorer(TPlanner& planner, std::span profiles) : + planner{planner}, profiles{profiles} { + this->route.steps.reserve(static_cast(width) * height); +} + +template +void TExplorer::reset() { + this->number_of_targets = 0; + this->number_collected = 0; + this->profile_index = 0; + this->planning = false; + this->answered = false; +} + +template +bool TExplorer::update(const TMaze& maze, uint32_t max_nodes) { + if (this->profiles.empty()) { + this->answered = true; + return false; + } + + if (this->planning and maze.get_revision() != this->round_revision) { + this->planning = false; + this->profile_index = 0; + } + + if (not this->planning) { + if (this->answered and maze.get_revision() == this->answered_revision) { + return false; + } + + if (this->profile_index == 0) { + this->number_collected = 0; + this->round_revision = maze.get_revision(); + } + + this->planner.begin(maze, WallAssumption::OPTIMISTIC, this->profiles[this->profile_index]); + this->planning = true; + } + + if (not this->planner.step(max_nodes)) { + return false; + } + + this->planning = false; + + float best_time = 0.0F; + + for (uint8_t i = 0; i < this->planner.get_number_of_routes(); i++) { + this->planner.get_route(i, this->route); + + if (i == 0) { + best_time = this->route.time; + } else if (this->route.time > best_time + candidate_window) { + break; + } + + TPlanner::for_each_wall(this->route, [this, &maze](const GridPose& wall) { + if (maze.get_wall(wall) == WallState::UNKNOWN) { + this->add_target(wall); + } + }); + } + + this->profile_index++; + + if (this->profile_index < this->profiles.size()) { + return false; + } + + this->profile_index = 0; + + const bool changed = + not this->answered or this->number_collected != this->number_of_targets or + not std::equal( + this->collected.begin(), this->collected.begin() + this->number_collected, this->targets.begin() + ); + + this->targets = this->collected; + this->number_of_targets = this->number_collected; + this->answered = true; + this->answered_revision = this->round_revision; + + return changed; +} + +template +bool TExplorer::has_targets() const { + return this->answered; +} + +template +bool TExplorer::is_complete() const { + return this->answered and this->number_of_targets == 0; +} + +template +std::span TExplorer::get_targets() const { + return std::span{this->targets}.first(this->number_of_targets); +} + +template +void TExplorer::add_target(const GridPose& wall) { + for (const GridPoint& cell : {wall.position, wall.front().position}) { + const auto end = this->collected.begin() + this->number_collected; + + if (not TMaze::contains(cell) or this->number_collected == max_targets or + std::find(this->collected.begin(), end, cell) != end) { + continue; + } + + this->collected.at(this->number_collected) = cell; + this->number_collected++; + } +} +} // namespace micras::nav + +#endif // MICRAS_NAV_EXPLORER_TPP diff --git a/micras_nav/include/micras/nav/impl/localizer.tpp b/micras_nav/include/micras/nav/impl/localizer.tpp new file mode 100644 index 0000000..32fcf3a --- /dev/null +++ b/micras_nav/include/micras/nav/impl/localizer.tpp @@ -0,0 +1,134 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_LOCALIZER_TPP +#define MICRAS_NAV_LOCALIZER_TPP + +#include +#include + +#include "micras/nav/maze.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/wall_model.hpp" + +namespace micras::nav { +template +void Localizer::correct( + const Measurements& measurements, const WallModel& wall_model, const TMaze& maze +) { + if (std::abs(this->state.velocity.angular) > this->config.max_angular_speed) { + for (EdgeTracker& tracker : this->edge_trackers) { + tracker.tracking = false; + } + + return; + } + + const float gate = this->is_stationary() ? this->config.stationary_gate : this->config.gate; + + for (uint8_t sensor = 0; sensor < number_of_wall_sensors; sensor++) { + const WallReading& reading = measurements.walls.at(sensor); + + if (not reading.is_new) { + continue; + } + + const Pose sampled = this->state.pose.compose({ + .position = {.x = -this->state.velocity.linear * this->config.range_delay, .y = 0.0F}, + .orientation = -this->state.velocity.angular * this->config.range_delay, + }); + + const RayHit hit = wall_model.cast(sampled, sensor, maze); + + if (this->config.use_edges and wall_model.is_side_looking(sensor)) { + this->track_edge(sensor, reading, sampled, hit, wall_model, maze); + } + + if (not reading.valid or not hit.valid or hit.state != WallState::WALL or + not wall_model.is_footprint_clear( + hit, sensor, this->get_position_deviation(), this->get_orientation_deviation() + )) { + continue; + } + + const float deviation = wall_model.get_range_deviation(hit.range); + + this->update( + reading.distance - hit.range, {hit.jacobian.at(0), hit.jacobian.at(1), hit.jacobian.at(2), 0.0F}, + deviation * deviation * this->config.range_correlation, gate, this->config.max_position_correction + ); + } +} + +template +void Localizer::track_edge( + uint8_t sensor, const WallReading& reading, const Pose& sampled, const RayHit& hit, const WallModel& wall_model, + const TMaze& maze +) { + EdgeTracker& tracker = this->edge_trackers.at(sensor); + + const auto is_on = [this, &reading](float range) { + return reading.valid and std::abs(reading.distance - range) < + this->config.range_tolerance + this->config.relative_range_tolerance * range; + }; + + const bool on_wall = + hit.valid and hit.state == WallState::WALL and hit.range < this->config.edge_range and is_on(hit.range); + const bool was_tracking = tracker.tracking; + const bool was_on_wall = tracker.on_wall; + + const RayHit previous = tracker.hit; + + tracker.tracking = this->state.velocity.linear > this->config.edge_speed; + tracker.on_wall = on_wall; + + if (on_wall) { + tracker.hit = hit; + } + + if (not was_tracking or not tracker.tracking or was_on_wall == on_wall) { + return; + } + + const RayHit& wall = on_wall ? hit : previous; + + const PlaneCrossing crossing = wall_model.cross(sampled, sensor, wall); + + if (not on_wall and (is_on(crossing.range) or (reading.valid and reading.distance < crossing.range))) { + return; + } + + const float heading = this->state.pose.orientation; + const float travel = this->state.velocity.linear * (wall.vertical ? std::sin(heading) : std::cos(heading)); + const bool leaving_upwards = (travel > 0.0F) == (not on_wall); + + const Side towards = + wall.vertical ? (leaving_upwards ? Side::UP : Side::DOWN) : (leaving_upwards ? Side::RIGHT : Side::LEFT); + const GridPose beyond{.position = wall.wall.position + towards, .orientation = wall.wall.orientation}; + + if (maze.get_wall(beyond) == WallState::WALL) { + return; + } + + const float half_wall = this->config.model.maze.wall_thickness / 2.0F; + const float edge = leaving_upwards ? this->config.model.maze.cell_size + half_wall : -half_wall; + const float innovation = edge - crossing.offset; + + if (std::abs(innovation) > this->config.edge_window) { + return; + } + + if (this->update( + innovation, {crossing.jacobian.at(0), crossing.jacobian.at(1), crossing.jacobian.at(2), 0.0F}, + this->config.edge_deviation * this->config.edge_deviation, this->config.gate, + this->config.max_edge_correction + )) { + this->status.edges++; + } +} +} // namespace micras::nav + +#endif // MICRAS_NAV_LOCALIZER_TPP diff --git a/micras_nav/include/micras/nav/impl/maze.tpp b/micras_nav/include/micras/nav/impl/maze.tpp new file mode 100644 index 0000000..bb38e6d --- /dev/null +++ b/micras_nav/include/micras/nav/impl/maze.tpp @@ -0,0 +1,265 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MAZE_TPP +#define MICRAS_NAV_MAZE_TPP + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" + +namespace micras::nav { +template +TMaze::TMaze(const Config& config) : start{config.start}, goal{config.goal} { + this->reset(); +} + +template +void TMaze::reset() { + for (auto& row : this->walls) { + for (auto& cell : row) { + cell.fill(WallState::UNKNOWN); + } + } + + for (uint8_t row = 0; row < height; row++) { + this->write_wall({.position = {.x = 0, .y = row}, .orientation = Side::LEFT}, WallState::WALL); + this->write_wall( + {.position = {.x = static_cast(width - 1), .y = row}, .orientation = Side::RIGHT}, WallState::WALL + ); + } + + for (uint8_t column = 0; column < width; column++) { + this->write_wall({.position = {.x = column, .y = 0}, .orientation = Side::DOWN}, WallState::WALL); + this->write_wall( + {.position = {.x = column, .y = static_cast(height - 1)}, .orientation = Side::UP}, WallState::WALL + ); + } + + for (const Side side : all_sides) { + const GridPose wall{.position = this->start.position, .orientation = side}; + this->write_wall(wall, side == this->start.orientation ? WallState::NO_WALL : WallState::WALL); + } + + for (const GridPoint& cell : this->goal) { + for (const Side side : all_sides) { + if (this->is_goal(cell + side)) { + this->write_wall({.position = cell, .orientation = side}, WallState::NO_WALL); + } + } + } + + this->revision = 0; + this->flood(this->goal); +} + +template +WallState TMaze::get_wall(const GridPose& pose) const { + if (not contains(pose.position)) { + return WallState::WALL; + } + + return this->walls.at(pose.position.y).at(pose.position.x).at(std::to_underlying(pose.orientation)); +} + +template +bool TMaze::set_wall(const GridPose& pose, bool present) { + if (this->get_wall(pose) != WallState::UNKNOWN) { + return false; + } + + this->write_wall(pose, present ? WallState::WALL : WallState::NO_WALL); + this->revision++; + + return true; +} + +template +bool TMaze::is_blocked(const GridPose& pose) const { + return this->get_wall(pose) == WallState::WALL; +} + +template +bool TMaze::is_possibly_open(const GridPose& pose) const { + return this->get_wall(pose) != WallState::WALL; +} + +template +bool TMaze::is_known_open(const GridPose& pose) const { + return this->get_wall(pose) == WallState::NO_WALL; +} + +template +bool TMaze::is_goal(const GridPoint& position) const { + for (const GridPoint& cell : this->goal) { + if (cell == position) { + return true; + } + } + + return false; +} + +template +const GridPose& TMaze::get_start() const { + return this->start; +} + +template +std::span TMaze::get_goal() const { + return this->goal; +} + +template +uint32_t TMaze::get_revision() const { + return this->revision; +} + +template +void TMaze::flood(std::span targets) { + for (auto& row : this->costs) { + row.fill(unreachable); + } + + std::size_t head = 0; + std::size_t tail = 0; + + for (const GridPoint& target : targets) { + if (contains(target) and this->costs.at(target.y).at(target.x) != 0) { + this->costs.at(target.y).at(target.x) = 0; + this->queue.at(tail++) = target; + } + } + + while (head < tail) { + const GridPoint cell = this->queue.at(head++); + const uint16_t next_cost = this->costs.at(cell.y).at(cell.x) + 1; + + for (const Side side : all_sides) { + if (this->is_blocked({.position = cell, .orientation = side})) { + continue; + } + + const GridPoint neighbor = cell + side; + + if (this->costs.at(neighbor.y).at(neighbor.x) == unreachable) { + this->costs.at(neighbor.y).at(neighbor.x) = next_cost; + this->queue.at(tail++) = neighbor; + } + } + } +} + +template +uint16_t TMaze::get_cost(const GridPoint& position) const { + return contains(position) ? this->costs.at(position.y).at(position.x) : unreachable; +} + +template +std::optional TMaze::get_next(const GridPose& pose) const { + const std::array preference{ + pose.orientation, + pose.turned_left().orientation, + pose.turned_right().orientation, + pose.turned_back().orientation, + }; + + std::optional best{}; + uint16_t best_cost = unreachable; + + for (const Side side : preference) { + if (this->is_blocked({.position = pose.position, .orientation = side})) { + continue; + } + + const GridPoint neighbor = pose.position + side; + const uint16_t cost = this->get_cost(neighbor); + + if (cost < best_cost) { + best_cost = cost; + best = GridPose{.position = neighbor, .orientation = side}; + } + } + + return best; +} + +template +std::vector TMaze::serialize() const { + std::vector buffer(header_size + walls_size, 0); + + buffer.at(0) = format_version; + buffer.at(1) = width; + buffer.at(2) = height; + + for (uint8_t row = 0; row < height; row++) { + for (uint8_t column = 0; column < width; column++) { + const auto& cell = this->walls.at(row).at(column); + const uint16_t index = row * width + column; + + const auto packed = static_cast( + std::to_underlying(cell.at(std::to_underlying(Side::RIGHT))) | + (std::to_underlying(cell.at(std::to_underlying(Side::UP))) << 2U) + ); + + buffer.at(header_size + index / 2U) |= static_cast(packed << (4U * (index % 2U))); + } + } + + return buffer; +} + +template +void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { + const std::span data{buffer, size}; + + if (size != header_size + walls_size or data[0] != format_version or data[1] != width or data[2] != height) { + return; + } + + this->reset(); + + for (uint8_t row = 0; row < height; row++) { + for (uint8_t column = 0; column < width; column++) { + const uint16_t index = row * width + column; + const uint8_t packed = (data[header_size + index / 2U] >> (4U * (index % 2U))) & 0x0FU; + + const std::array, 2> sides{ + {{Side::RIGHT, packed & 0x03U}, {Side::UP, packed >> 2U}} + }; + + for (const auto& [side, state] : sides) { + if (state == std::to_underlying(WallState::WALL) or state == std::to_underlying(WallState::NO_WALL)) { + this->set_wall( + {.position = {.x = column, .y = row}, .orientation = side}, + state == std::to_underlying(WallState::WALL) + ); + } + } + } + } + + this->flood(this->goal); +} + +template +void TMaze::write_wall(const GridPose& pose, WallState state) { + this->walls.at(pose.position.y).at(pose.position.x).at(std::to_underlying(pose.orientation)) = state; + + const GridPose opposite = pose.front().turned_back(); + + if (contains(opposite.position)) { + this->walls.at(opposite.position.y).at(opposite.position.x).at(std::to_underlying(opposite.orientation)) = + state; + } +} +} // namespace micras::nav + +#endif // MICRAS_NAV_MAZE_TPP diff --git a/micras_nav/include/micras/nav/impl/mission.tpp b/micras_nav/include/micras/nav/impl/mission.tpp new file mode 100644 index 0000000..adcdac8 --- /dev/null +++ b/micras_nav/include/micras/nav/impl/mission.tpp @@ -0,0 +1,536 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MISSION_TPP +#define MICRAS_NAV_MISSION_TPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/core/types.hpp" +#include "micras/nav/executor.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/localizer.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/route_compiler.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/velocity_planner.hpp" + +namespace micras::nav { +template +TMission::TMission(const Dynamics& dynamics, const WallModel& wall_model, const Config& config) : + dynamics{dynamics}, + wall_model{wall_model}, + config{config}, + maze{config.maze}, + observer{config.observer}, + planner{dynamics, config.planner}, + explorer{planner, config.map_profiles}, + executor{dynamics, config.executor}, + search_speed{std::min( + dynamics.get_linear_limits(config.search_profile).max_speed, + dynamics.get_turn_speed(config.search_profile, TurnId::SS90S) + )} { + this->solve_segments.reserve(config.executor.capacity); + this->candidate_segments.reserve(config.executor.capacity); + this->candidate_route.steps.reserve(config.executor.capacity); + this->observer.reset(); +} + +template +TMaze& TMission::get_maze() { + return this->maze; +} + +template +const TMaze& TMission::get_maze() const { + return this->maze; +} + +template +Pose TMission::get_start_pose() const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + + return this->get_center_pose(this->maze.get_start()) + .compose({.position = {.x = this->config.start_offset - cell_size / 2.0F, .y = 0.0F}, .orientation = 0.0F}); +} + +template +void TMission::begin_plan(const RunProfile& profile) { + this->solve_profile = profile; + this->solve_segments.clear(); + this->route_time = std::numeric_limits::infinity(); + this->planner.begin(this->maze, WallAssumption::PESSIMISTIC, profile); + this->planning = true; +} + +template +bool TMission::update_plan(uint32_t max_nodes) { + if (not this->planning) { + return true; + } + + if (not this->planner.step(max_nodes)) { + return false; + } + + this->planning = false; + + const MotionLimits limits = this->dynamics.get_linear_limits(this->solve_profile); + + for (uint8_t i = 0; i < this->planner.get_number_of_routes(); i++) { + this->planner.get_route(i, this->candidate_route); + + RouteCompiler::compile( + this->candidate_route, this->dynamics, this->solve_profile, this->config.planner.start_distance, + this->candidate_segments + ); + + const float total_time = + VelocityPlanner::plan(this->candidate_segments, this->dynamics, this->solve_profile, 0.0F, 0.0F); + + const Segment& last = this->candidate_segments.back(); + const SpeedProfile braking{last.length, last.start_speed, 0.0F, limits}; + + float after_line = braking.duration(); + + if (this->candidate_route.finish_distance <= last.length) { + after_line -= braking.time_at(last.length - this->candidate_route.finish_distance); + } else { + after_line += (this->candidate_route.finish_distance - last.length) / last.start_speed; + } + + if (total_time - after_line < this->route_time) { + this->route_time = total_time - after_line; + std::swap(this->solve_segments, this->candidate_segments); + } + } + + return true; +} + +template +bool TMission::has_route() const { + return not this->planning and not this->solve_segments.empty(); +} + +template +float TMission::get_route_time() const { + return this->route_time; +} + +template +void TMission::start(core::Objective objective) { + const float cell_size = this->dynamics.get_model().maze.cell_size; + + this->objective = objective; + this->status = Status::RUNNING; + this->finishing = false; + this->watching_front = false; + this->looks = 0; + + switch (objective) { + case core::Objective::EXPLORE: { + this->executor.reset(this->get_start_pose(), this->config.search_profile); + this->cell = this->maze.get_start().front(); + this->at_center = false; + this->flood(); + + Move move{}; + move.add( + make_segment(SegmentKind::STRAIGHT, cell_size - this->config.start_offset, this->get_start_pose()) + ); + this->execute(move, 0.0F, this->search_speed); + break; + } + + case core::Objective::RETURN: + this->executor.reset(this->get_center_pose(this->cell), this->config.search_profile); + this->at_center = true; + this->explorer.reset(); + this->flood(); + break; + + case core::Objective::SOLVE: + this->executor.reset(this->get_start_pose(), this->solve_profile); + this->executor.push(this->solve_segments); + this->finishing = true; + break; + } + + this->reference = this->executor.get_reference(); +} + +template +TMission::Status TMission::update( + const Measurements& measurements, Localizer& localizer, float elapsed_time, float time_scale +) { + if (this->status != Status::RUNNING) { + return this->status; + } + + if (this->objective != core::Objective::SOLVE) { + bool changed = this->observer.update(measurements, localizer, this->wall_model, this->maze); + + if (this->objective == core::Objective::RETURN) { + changed = this->explorer.update(this->maze, this->config.nodes_per_iteration) or changed; + } + + if (changed) { + this->flood(); + } + } + + const Segment* watched = this->executor.get_current(); + + if (this->watching_front and watched != nullptr and watched->kind == SegmentKind::STRAIGHT and + watched->start.position.distance(this->get_entry_pose(this->watched_cell).position) < watch_tolerance) { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const float braking = this->search_speed * this->search_speed / + (2.0F * this->dynamics.get_linear_limits(this->config.search_profile).deceleration); + + const WallState wall = this->maze.get_wall(this->watched_cell); + + if (wall == WallState::NO_WALL) { + this->watching_front = false; + } else if ( + wall == WallState::WALL or + this->executor.get_reference().distance >= cell_size / 2.0F - braking - this->config.commit_margin + ) { + this->divert_to_center(); + } + } + + if (this->executor.is_ending(elapsed_time)) { + if (this->finishing) { + if (this->executor.is_finished()) { + this->status = Status::FINISHED; + } + } else if (this->at_center) { + this->decide_at_center(); + } else { + this->decide_at_entry(); + } + } + + this->reference = this->executor.update(elapsed_time, time_scale, localizer.get_state()); + + const Segment* current = this->executor.get_current(); + + if (current == nullptr or current->kind == SegmentKind::STOP or current->kind == SegmentKind::ATTACH) { + localizer.correct_at_rest(measurements, elapsed_time); + + if (this->objective != core::Objective::SOLVE and this->at_center and current != nullptr and + localizer.get_cell().position != this->cell.position) { + this->status = Status::FAILED; + } + } + + return this->status; +} + +template +const Reference& TMission::get_reference() const { + return this->reference; +} + +template +const Executor& TMission::get_executor() const { + return this->executor; +} + +template +const GridPose& TMission::get_cell() const { + return this->cell; +} + +template +Pose TMission::get_entry_pose(const GridPose& cell) const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + + return this->get_center_pose(cell).compose({.position = {.x = -cell_size / 2.0F, .y = 0.0F}, .orientation = 0.0F}); +} + +template +Pose TMission::get_center_pose(const GridPose& cell) const { + return { + .position = cell.position.to_vector(this->dynamics.get_model().maze.cell_size), + .orientation = static_cast(std::to_underlying(cell.orientation)) * std::numbers::pi_v / 2.0F, + }; +} + +template +void TMission::flood() { + if (this->objective != core::Objective::RETURN) { + this->maze.flood(this->maze.get_goal()); + return; + } + + std::array::max_targets> targets{}; + std::size_t number_of_targets = 0; + + if (this->explorer.has_targets()) { + for (const GridPoint& target : this->explorer.get_targets()) { + const bool worth_visiting = std::ranges::any_of(all_sides, [this, &target](Side side) { + return this->maze.get_wall({.position = target, .orientation = side}) == WallState::UNKNOWN; + }); + + if (worth_visiting) { + targets.at(number_of_targets++) = target; + } + } + } + + if (number_of_targets == 0) { + targets.at(number_of_targets++) = this->maze.get_start().position; + } + + this->maze.flood(std::span{targets}.first(number_of_targets)); +} + +template +void TMission::execute(Move& move, float start_speed, float end_speed) { + const std::span segments = std::span{move.segments}.first(move.size); + + VelocityPlanner::plan(segments, this->dynamics, this->config.search_profile, start_speed, end_speed); + this->executor.push(segments); +} + +template +void TMission::decide_at_entry() { + const float cell_size = this->dynamics.get_model().maze.cell_size; + + this->watching_front = false; + this->looks = 0; + + if (this->finish_at_entry()) { + return; + } + + const std::optional next = this->maze.get_next(this->cell); + + if (not next.has_value()) { + this->status = Status::FAILED; + return; + } + + const Pose entry = this->get_entry_pose(this->cell); + Move move{}; + + if (next->orientation == this->cell.orientation) { + move.add(make_segment(SegmentKind::STRAIGHT, cell_size, entry)); + this->execute(move, this->search_speed, this->search_speed); + + this->watching_front = this->maze.get_wall(this->cell) == WallState::UNKNOWN; + this->watched_cell = this->cell; + this->cell = *next; + return; + } + + if (next->orientation == this->cell.turned_back().orientation) { + const GridPose reversed = this->cell.turned_back(); + + this->add_stop_at_center(move); + move.add(make_segment(SegmentKind::SPIN, std::numbers::pi_v, this->get_center_pose(this->cell))); + move.add(make_segment(SegmentKind::STRAIGHT, cell_size / 2.0F, this->get_center_pose(reversed))); + this->execute(move, this->search_speed, this->search_speed); + + this->cell = *next; + return; + } + + if (this->maze.get_wall({.position = this->cell.position, .orientation = next->orientation}) != + WallState::NO_WALL) { + this->add_stop_at_center(move); + this->execute(move, this->search_speed, 0.0F); + this->at_center = true; + return; + } + + const TurnShape& shape = this->dynamics.get_turn(this->config.search_profile, TurnId::SS90S); + const bool to_left = next->orientation == this->cell.turned_left().orientation; + + Segment turn = make_segment( + SegmentKind::TURN, to_left ? shape.angle : -shape.angle, + entry.compose({.position = {.x = shape.pre, .y = 0.0F}, .orientation = 0.0F}) + ); + turn.turn = TurnId::SS90S; + + if (shape.pre > min_straight) { + move.add(make_segment(SegmentKind::STRAIGHT, shape.pre, entry)); + } + + move.add(turn); + + if (shape.post > min_straight) { + move.add(make_segment( + SegmentKind::STRAIGHT, shape.post, + this->get_entry_pose(*next).compose({.position = {.x = -shape.post, .y = 0.0F}, .orientation = 0.0F}) + )); + } + + this->execute(move, this->search_speed, this->search_speed); + this->cell = *next; +} + +template +void TMission::decide_at_center() { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const Pose center = this->get_center_pose(this->cell); + + Move move{}; + + if (this->objective == core::Objective::RETURN and not this->explorer.has_targets()) { + move.add(make_segment(SegmentKind::STOP, this->config.look_time, center)); + this->execute(move, 0.0F, 0.0F); + return; + } + + const std::optional next = this->maze.get_next(this->cell); + + if (not next.has_value()) { + this->status = Status::FAILED; + return; + } + + if (next->orientation != this->cell.orientation) { + float angle = std::numbers::pi_v; + + if (next->orientation == this->cell.turned_left().orientation) { + angle = std::numbers::pi_v / 2.0F; + } else if (next->orientation == this->cell.turned_right().orientation) { + angle = -std::numbers::pi_v / 2.0F; + } + + move.add(make_segment(SegmentKind::SPIN, angle, center)); + this->cell.orientation = next->orientation; + move.add(make_segment(SegmentKind::STOP, this->config.stop_time, this->get_center_pose(this->cell))); + this->execute(move, 0.0F, 0.0F); + return; + } + + if (this->maze.get_wall(this->cell) == WallState::NO_WALL) { + move.add(make_segment(SegmentKind::STRAIGHT, cell_size / 2.0F, center)); + this->execute(move, 0.0F, this->search_speed); + this->at_center = false; + this->looks = 0; + this->cell = *next; + return; + } + + this->looks++; + + if (this->looks > this->config.max_looks) { + this->status = Status::FAILED; + return; + } + + move.add(make_segment(SegmentKind::STOP, this->config.look_time, center)); + this->execute(move, 0.0F, 0.0F); +} + +template +void TMission::add_stop_at_center(Move& move) const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const Pose center = this->get_center_pose(this->cell); + + move.add(make_segment(SegmentKind::STRAIGHT, cell_size / 2.0F, this->get_entry_pose(this->cell))); + + if (this->maze.get_wall(this->cell) == WallState::WALL) { + move.add(make_segment(SegmentKind::ATTACH, this->config.attach_time, center)); + } else { + move.add(make_segment(SegmentKind::STOP, this->config.stop_time, center)); + } +} + +template +bool TMission::finish_at_entry() { + const float cell_size = this->dynamics.get_model().maze.cell_size; + + Move move{}; + + if (this->objective == core::Objective::EXPLORE and this->maze.is_goal(this->cell.position)) { + this->add_stop_at_center(move); + } else if ( + this->objective == core::Objective::RETURN and this->explorer.is_complete() and + this->cell.position == this->maze.get_start().position + ) { + const GridPose parked{.position = this->cell.position, .orientation = this->maze.get_start().orientation}; + + this->add_stop_at_center(move); + move.add(make_segment(SegmentKind::SPIN, std::numbers::pi_v, this->get_center_pose(this->cell))); + move.add(make_segment(SegmentKind::STOP, this->config.stop_time, this->get_center_pose(parked))); + move.add(make_segment( + SegmentKind::STRAIGHT, this->config.start_offset - cell_size / 2.0F, this->get_center_pose(parked) + )); + } else { + return false; + } + + this->execute(move, this->search_speed, 0.0F); + this->at_center = true; + this->finishing = true; + + return true; +} + +template +void TMission::divert_to_center() { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const Reference& current = this->executor.get_reference(); + const float remaining = cell_size / 2.0F - current.distance; + + this->watching_front = false; + + if (remaining <= 0.0F) { + return; + } + + const Pose center = this->get_center_pose(this->watched_cell); + + Move move{}; + move.add(make_segment( + SegmentKind::STRAIGHT, remaining, + this->get_entry_pose(this->watched_cell) + .compose({.position = {.x = current.distance, .y = 0.0F}, .orientation = 0.0F}) + )); + + if (this->maze.get_wall(this->watched_cell) == WallState::WALL) { + move.add(make_segment(SegmentKind::ATTACH, this->config.attach_time, center)); + } else { + move.add(make_segment(SegmentKind::STOP, this->config.stop_time, center)); + } + + const std::span segments = std::span{move.segments}.first(move.size); + + VelocityPlanner::plan(segments, this->dynamics, this->config.search_profile, current.twist.linear, 0.0F); + this->executor.divert(segments); + + this->cell = this->watched_cell; + this->at_center = true; +} + +template +Segment TMission::make_segment(SegmentKind kind, float length, const Pose& start) { + return { + .kind = kind, + .turn = TurnId::SS90S, + .length = length, + .start_speed = 0.0F, + .end_speed = 0.0F, + .max_speed = 0.0F, + .start = start, + }; +} +} // namespace micras::nav + +#endif // MICRAS_NAV_MISSION_TPP diff --git a/micras_nav/include/micras/nav/impl/planner.tpp b/micras_nav/include/micras/nav/impl/planner.tpp new file mode 100644 index 0000000..049dd1d --- /dev/null +++ b/micras_nav/include/micras/nav/impl/planner.tpp @@ -0,0 +1,613 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_PLANNER_TPP +#define MICRAS_NAV_PLANNER_TPP + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/speed_profile.hpp" + +namespace micras::nav { +template +TPlanner::TPlanner(const Dynamics& dynamics, const Config& config) : + dynamics{dynamics}, start_distance{config.start_distance} { } + +template +void TPlanner::begin( + const TMaze& maze, WallAssumption assumption, const RunProfile& profile +) { + this->maze = &maze; + this->assumption = assumption; + this->run_profile = profile; + this->limits = this->dynamics.get_linear_limits(profile); + + for (uint8_t i = 0; i < number_of_turns; i++) { + this->turn_speeds.at(i) = this->dynamics.get_turn_speed(profile, static_cast(i)); + } + + for (auto& runs : this->edges) { + for (auto& turns : runs) { + turns.fill({.cost = -1.0F, .speed_ratio = 0}); + } + } + + this->cleared = 0; + this->queue_size = 0; + this->number_of_terminals = 0; + + const GridPose& start_pose = maze.get_start(); + const auto heading = static_cast(2 * std::to_underlying(start_pose.orientation)); + const LatticePose center{ + .point = + {.x = static_cast(2 * start_pose.position.x + 1), + .y = static_cast(2 * start_pose.position.y + 1)}, + .heading = heading, + }; + const LatticePoint step = center.step(); + + this->start = { + .point = + {.x = static_cast(center.point.x + step.x / 2), + .y = static_cast(center.point.y + step.y / 2)}, + .heading = heading, + }; +} + +template +bool TPlanner::step(uint32_t max_nodes) { + while (this->cleared < number_of_states and max_nodes > 0) { + const uint16_t end = std::min(this->cleared + states_per_node_budget, number_of_states); + + std::fill( + this->costs.begin() + this->cleared, this->costs.begin() + end, std::numeric_limits::infinity() + ); + std::fill(this->positions.begin() + this->cleared, this->positions.begin() + end, not_queued); + + this->cleared = end; + max_nodes--; + + if (this->cleared == number_of_states and is_inside(this->start) and this->is_traversable(this->start)) { + this->relax(encode({.node = this->start, .arrival = Arrival::REST}), 0.0F, 0, full_speed); + } + } + + for (uint32_t i = 0; i < max_nodes and this->queue_size > 0; i++) { + const uint16_t index = this->pop(); + + if (this->number_of_terminals == number_of_candidates and + this->costs.at(index) >= this->terminals.at(number_of_candidates - 1).cost) { + this->queue_size = 0; + break; + } + + this->expand(index); + } + + return this->is_finished(); +} + +template +bool TPlanner::is_finished() const { + return this->cleared == number_of_states and this->queue_size == 0; +} + +template +uint8_t TPlanner::get_number_of_routes() const { + return this->number_of_terminals; +} + +template +void TPlanner::get_route(uint8_t index, Route& route) const { + const Terminal& terminal = this->terminals.at(index); + + route.start = this->start; + route.steps.clear(); + route.stop_distance = terminal.stop_distance; + route.finish_distance = terminal.finish_distance; + route.time = terminal.cost; + + route.steps.push_back( + {.run = terminal.run, .has_turn = terminal.has_turn, .turn = terminal.turn, .side = terminal.side} + ); + + uint16_t index_of_state = terminal.state; + + while (true) { + const State state = decode(index_of_state); + const LatticePose& node = state.node; + const Arrival arrival = state.arrival; + + if (arrival == Arrival::REST) { + break; + } + + const uint16_t link = this->links.at(index_of_state); + const auto run = static_cast(link & 0x3FU); + const auto previous = static_cast((link >> 6U) & 0x0FU); + const TurnSide side = (link >> 10U) != 0 ? TurnSide::RIGHT : TurnSide::LEFT; + const TurnId turn = to_turn(arrival); + + route.steps.push_back({.run = run, .has_turn = true, .turn = turn, .side = side}); + + const TurnPrimitive& primitive = get_primitive(turn); + const uint8_t rotation = + side == TurnSide::LEFT ? primitive.rotation : LatticePose::number_of_headings - primitive.rotation; + const auto entry_heading = static_cast( + (node.heading + LatticePose::number_of_headings - rotation) % LatticePose::number_of_headings + ); + const LatticePoint offset = from_canonical(primitive.exit, primitive.diagonal_entry, entry_heading, side); + const LatticePoint step = LatticePose{.point = {}, .heading = entry_heading}.step(); + + const LatticePose origin{ + .point = + {.x = static_cast(node.point.x - offset.x - run * step.x), + .y = static_cast(node.point.y - offset.y - run * step.y)}, + .heading = entry_heading, + }; + + index_of_state = encode({.node = origin, .arrival = previous}); + } + + std::ranges::reverse(route.steps); +} + +template +template +void TPlanner::for_each_wall(const Route& route, F&& function) { + LatticePose node = route.start; + function(node.wall()); + + for (const RouteStep& step : route.steps) { + for (uint8_t i = 0; i < step.run; i++) { + node = node.advanced(); + function(node.wall()); + } + + if (not step.has_turn) { + continue; + } + + for (uint8_t i = 0; i < get_primitive(step.turn).number_of_gates; i++) { + function(LatticePose{.point = get_turn_gate(node, step.turn, step.side, i), .heading = 0}.wall()); + } + + node = get_turn_exit(node, step.turn, step.side); + function(node.wall()); + } +} + +template +constexpr TPlanner::Arrival TPlanner::to_arrival(TurnId turn) { + constexpr std::array arrivals{ + Arrival::SS90S, Arrival::SS90L, Arrival::SS180, Arrival::SD45, + Arrival::SD135, Arrival::DS45, Arrival::DS135, Arrival::DD90, + }; + + return arrivals.at(std::to_underlying(turn)); +} + +template +constexpr TurnId TPlanner::to_turn(Arrival arrival) { + constexpr std::array turns{ + TurnId::SS90S, TurnId::SS90S, TurnId::SS90L, TurnId::SS180, TurnId::DS45, + TurnId::DS135, TurnId::SD45, TurnId::SD135, TurnId::DD90, + }; + + return turns.at(std::to_underlying(arrival)); +} + +template +constexpr bool TPlanner::is_inside(const LatticePose& node) { + return node.point.x >= 0 and node.point.x <= 2 * width and node.point.y >= 0 and node.point.y <= 2 * height and + (node.point.on_vertical_wall() or node.point.on_horizontal_wall()); +} + +template +constexpr uint16_t TPlanner::encode(const State& state) { + const LatticePoint& point = state.node.point; + const uint8_t heading = state.node.heading; + + const int32_t wall = point.on_vertical_wall() ? + (point.x / 2) * height + (point.y - 1) / 2 : + (width + 1) * height + ((point.x - 1) / 2) * (height + 1) + point.y / 2; + + const int32_t sub = heading % 2 == 0 ? (heading / 4) * 6 + std::to_underlying(state.arrival) : + 12 + (heading / 2) * 3 + (std::to_underlying(state.arrival) - 6); + + return static_cast(wall * states_per_wall + sub); +} + +template +constexpr TPlanner::State TPlanner::decode(uint16_t index) { + const uint16_t wall = index / states_per_wall; + const uint16_t sub = index % states_per_wall; + + State state{}; + + if (wall < (width + 1) * height) { + state.node.point = { + .x = static_cast(2 * (wall / height)), .y = static_cast(2 * (wall % height) + 1) + }; + } else { + const uint16_t horizontal = wall - (width + 1) * height; + + state.node.point = { + .x = static_cast(2 * (horizontal / (height + 1)) + 1), + .y = static_cast(2 * (horizontal % (height + 1))), + }; + } + + if (sub < 12) { + state.node.heading = static_cast((state.node.point.on_vertical_wall() ? 0 : 2) + 4 * (sub / 6)); + state.arrival = static_cast(sub % 6); + } else { + state.node.heading = static_cast(2 * ((sub - 12) / 3) + 1); + state.arrival = static_cast(6 + (sub - 12) % 3); + } + + return state; +} + +template +bool TPlanner::is_traversable(const LatticePose& node) const { + const GridPose wall = node.wall(); + + return this->assumption == WallAssumption::OPTIMISTIC ? this->maze->is_possibly_open(wall) : + this->maze->is_known_open(wall); +} + +template +bool TPlanner::is_goal(const LatticePoint& cell) const { + return cell.x >= 0 and cell.y >= 0 and + this->maze->is_goal({.x = static_cast(cell.x), .y = static_cast(cell.y)}); +} + +template +float TPlanner::get_stop_distance(const LatticePose& node) const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const LatticePoint step = node.step(); + + LatticePoint cell = node.cell_ahead(); + LatticePose wall = node; + uint8_t cells = 0; + + while (this->is_goal(cell)) { + cells++; + wall = wall.advanced(); + + if (not is_inside(wall) or not this->maze->is_known_open(wall.wall())) { + break; + } + + cell = {.x = static_cast(cell.x + step.x / 2), .y = static_cast(cell.y + step.y / 2)}; + } + + return (static_cast(cells) - 0.5F) * cell_size; +} + +template +float TPlanner::get_speed(uint16_t index, Arrival arrival) const { + if (arrival == Arrival::REST) { + return 0.0F; + } + + return this->turn_speeds.at(std::to_underlying(to_turn(arrival))) * + static_cast(this->speed_ratios.at(index)) / static_cast(full_speed); +} + +template +float TPlanner::get_offset(Arrival arrival) const { + return arrival == Arrival::REST ? this->start_distance : + this->dynamics.get_turn(this->run_profile, to_turn(arrival)).post; +} + +template +TPlanner::Edge + TPlanner::get_edge(uint16_t index, Arrival arrival, uint8_t run, TurnId turn) { + const bool at_full_speed = arrival == Arrival::REST or this->speed_ratios.at(index) == full_speed; + Edge& cached = this->edges.at(std::to_underlying(arrival)).at(run).at(std::to_underlying(turn)); + + if (at_full_speed and cached.cost >= 0.0F) { + return cached; + } + + const float cell_size = this->dynamics.get_model().maze.cell_size; + const bool diagonal = std::to_underlying(arrival) >= std::to_underlying(Arrival::SD45); + const float step = diagonal ? cell_size / std::numbers::sqrt2_v : cell_size; + + const TurnShape& shape = this->dynamics.get_turn(this->run_profile, turn); + const float distance = this->get_offset(arrival) + static_cast(run) * step + shape.pre; + const float nominal_speed = this->turn_speeds.at(std::to_underlying(turn)); + + float start_speed = this->get_speed(index, arrival); + float penalty = 0.0F; + + const float brakeable = SpeedProfile::get_brakeable_speed(distance, nominal_speed, this->limits); + + if (start_speed > brakeable) { + const float previous_length = this->dynamics.get_turn(this->run_profile, to_turn(arrival)).length(); + penalty = previous_length * (1.0F / brakeable - 1.0F / start_speed); + start_speed = brakeable; + } + + const float end_speed = + std::min(nominal_speed, SpeedProfile::get_reachable_speed(distance, start_speed, this->limits)); + + const Edge edge{ + .cost = SpeedProfile{distance, start_speed, end_speed, this->limits}.duration() + shape.length() / end_speed + + penalty, + .speed_ratio = static_cast(std::lround(static_cast(full_speed) * end_speed / nominal_speed)), + }; + + if (at_full_speed) { + cached = edge; + } + + return edge; +} + +template +void TPlanner::expand(uint16_t index) { + const State state = decode(index); + + const bool diagonal = state.node.is_diagonal(); + const float cell_size = this->dynamics.get_model().maze.cell_size; + + LatticePose entry = state.node; + + for (uint8_t run = 0; run <= max_run; run++) { + if (run > 0) { + entry = entry.advanced(); + + if (not is_inside(entry) or not this->is_traversable(entry)) { + break; + } + } + + if (this->is_goal(entry.cell_ahead())) { + if (not diagonal) { + const float line = this->get_offset(state.arrival) + static_cast(run) * cell_size; + const float stop_distance = this->get_stop_distance(entry); + + const SpeedProfile approach{ + line + stop_distance, this->get_speed(index, state.arrival), 0.0F, this->limits + }; + + this->add_terminal({ + .cost = this->costs.at(index) + approach.time_at(line), + .state = index, + .run = run, + .has_turn = false, + .turn = TurnId::SS90S, + .side = TurnSide::LEFT, + .stop_distance = stop_distance, + .finish_distance = stop_distance, + }); + } + + break; + } + + if (diagonal) { + this->relax_turn(index, state, entry, run, TurnId::DS45); + this->relax_turn(index, state, entry, run, TurnId::DS135); + this->relax_turn(index, state, entry, run, TurnId::DD90); + } else { + this->relax_turn(index, state, entry, run, TurnId::SS90S); + this->relax_turn(index, state, entry, run, TurnId::SS90L); + this->relax_turn(index, state, entry, run, TurnId::SS180); + + if (this->run_profile.diagonal) { + this->relax_turn(index, state, entry, run, TurnId::SD45); + this->relax_turn(index, state, entry, run, TurnId::SD135); + } + } + } +} + +template +void TPlanner::relax_turn( + uint16_t index, const State& state, const LatticePose& entry, uint8_t run, TurnId turn +) { + const TurnPrimitive& primitive = get_primitive(turn); + + if (not this->dynamics.get_turn(this->run_profile, turn).valid) { + return; + } + + for (const TurnSide side : {TurnSide::LEFT, TurnSide::RIGHT}) { + if (primitive.diagonal_entry and side != entry.diagonal_turn_side()) { + continue; + } + + const LatticePose exit = get_turn_exit(entry, turn, side); + + if (not is_inside(exit) or not exit.is_valid() or not this->is_traversable(exit)) { + continue; + } + + LatticePoint cell = entry.cell_ahead(); + int8_t finish_gate = -1; + bool feasible = true; + + for (uint8_t i = 0; i < primitive.number_of_gates and feasible; i++) { + const LatticePose gate{.point = get_turn_gate(entry, turn, side, i), .heading = 0}; + + if (not is_inside(gate) or not this->is_traversable(gate)) { + feasible = false; + break; + } + + const bool vertical = gate.point.on_vertical_wall(); + const LatticePoint low{ + .x = static_cast(vertical ? gate.point.x / 2 - 1 : (gate.point.x - 1) / 2), + .y = static_cast(vertical ? (gate.point.y - 1) / 2 : gate.point.y / 2 - 1), + }; + const LatticePoint high{ + .x = static_cast(vertical ? low.x + 1 : low.x), + .y = static_cast(vertical ? low.y : low.y + 1) + }; + + cell = cell == low ? high : low; + + if (this->is_goal(cell)) { + finish_gate = finish_gate < 0 ? static_cast(i) : finish_gate; + } else if (finish_gate >= 0) { + feasible = false; + } + } + + if (not feasible) { + continue; + } + + const Edge edge = this->get_edge(index, state.arrival, run, turn); + const float cost = this->costs.at(index) + edge.cost; + + if (finish_gate >= 0) { + if (exit.is_diagonal() or not this->is_goal(exit.cell_ahead())) { + continue; + } + + const TurnShape& shape = this->dynamics.get_turn(this->run_profile, turn); + const float remaining = + shape.pre + shape.length() - std::max(shape.gates.at(static_cast(finish_gate)), shape.pre); + const float speed = this->turn_speeds.at(std::to_underlying(turn)) * static_cast(edge.speed_ratio) / + static_cast(full_speed); + + const float stop_distance = this->get_stop_distance(exit); + + this->add_terminal({ + .cost = cost - remaining / speed, + .state = index, + .run = run, + .has_turn = true, + .turn = turn, + .side = side, + .stop_distance = stop_distance, + .finish_distance = remaining + shape.post + stop_distance, + }); + + continue; + } + + const uint16_t target = encode({.node = exit, .arrival = to_arrival(turn)}); + + if (cost < this->costs.at(target)) { + const auto link = static_cast( + static_cast(run) | (static_cast(std::to_underlying(state.arrival)) << 6U) | + ((side == TurnSide::RIGHT ? 1U : 0U) << 10U) + ); + + this->relax(target, cost, link, edge.speed_ratio); + } + } +} + +template +void TPlanner::add_terminal(const Terminal& terminal) { + uint8_t position = this->number_of_terminals; + + while (position > 0 and this->terminals.at(position - 1).cost > terminal.cost) { + if (position < number_of_candidates) { + this->terminals.at(position) = this->terminals.at(position - 1); + } + + position--; + } + + if (position < number_of_candidates) { + this->terminals.at(position) = terminal; + this->number_of_terminals = std::min(this->number_of_terminals + 1, number_of_candidates); + } +} + +template +void TPlanner::relax(uint16_t index, float cost, uint16_t link, uint8_t speed_ratio) { + this->costs.at(index) = cost; + this->links.at(index) = link; + this->speed_ratios.at(index) = speed_ratio; + + if (this->positions.at(index) == not_queued) { + this->positions.at(index) = this->queue_size; + this->queue.at(this->queue_size) = index; + this->queue_size++; + } + + this->sift_up(this->positions.at(index)); +} + +template +uint16_t TPlanner::pop() { + const uint16_t top = this->queue.at(0); + + this->queue_size--; + this->positions.at(top) = not_queued; + + if (this->queue_size > 0) { + this->queue.at(0) = this->queue.at(this->queue_size); + this->positions.at(this->queue.at(0)) = 0; + this->sift_down(0); + } + + return top; +} + +template +void TPlanner::sift_up(uint16_t position) { + const uint16_t index = this->queue.at(position); + + while (position > 0) { + const uint16_t parent = (position - 1) / 2; + + if (this->costs.at(this->queue.at(parent)) <= this->costs.at(index)) { + break; + } + + this->queue.at(position) = this->queue.at(parent); + this->positions.at(this->queue.at(position)) = position; + position = parent; + } + + this->queue.at(position) = index; + this->positions.at(index) = position; +} + +template +void TPlanner::sift_down(uint16_t position) { + const uint16_t index = this->queue.at(position); + + while (2 * position + 1 < this->queue_size) { + uint16_t child = 2 * position + 1; + + if (child + 1 < this->queue_size and + this->costs.at(this->queue.at(child + 1)) < this->costs.at(this->queue.at(child))) { + child++; + } + + if (this->costs.at(index) <= this->costs.at(this->queue.at(child))) { + break; + } + + this->queue.at(position) = this->queue.at(child); + this->positions.at(this->queue.at(position)) = position; + position = child; + } + + this->queue.at(position) = index; + this->positions.at(index) = position; +} +} // namespace micras::nav + +#endif // MICRAS_NAV_PLANNER_TPP diff --git a/micras_nav/include/micras/nav/impl/wall_model.tpp b/micras_nav/include/micras/nav/impl/wall_model.tpp new file mode 100644 index 0000000..160666b --- /dev/null +++ b/micras_nav/include/micras/nav/impl/wall_model.tpp @@ -0,0 +1,140 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_WALL_MODEL_TPP +#define MICRAS_NAV_WALL_MODEL_TPP + +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +template +RayHit WallModel::cast(const Pose& pose, uint8_t sensor, const TMaze& maze) const { + const RobotModel::WallSensor& mounting = this->sensors.at(sensor); + + const Pose origin = pose.compose({.position = mounting.position, .orientation = mounting.angle}); + const float direction_x = std::cos(origin.orientation); + const float direction_y = std::sin(origin.orientation); + + const float cell_size = this->maze_geometry.cell_size; + const float half_wall = this->maze_geometry.wall_thickness / 2.0F; + + auto column = static_cast(std::floor(origin.position.x / cell_size)); + auto row = static_cast(std::floor(origin.position.y / cell_size)); + + bool grazed_post = false; + + for (uint8_t crossing = 0; crossing < 4; crossing++) { + if (column < 0 or row < 0 or column >= width or row >= height) { + break; + } + + const float line_x = static_cast(direction_x > 0.0F ? column + 1 : column) * cell_size; + const float line_y = static_cast(direction_y > 0.0F ? row + 1 : row) * cell_size; + + const float distance_x = + direction_x != 0.0F ? (line_x - origin.position.x) / direction_x : std::numeric_limits::infinity(); + const float distance_y = + direction_y != 0.0F ? (line_y - origin.position.y) / direction_y : std::numeric_limits::infinity(); + + const bool vertical = distance_x < distance_y; + const float distance = vertical ? distance_x : distance_y; + const float across = vertical ? direction_x : direction_y; + const float along = vertical ? direction_y : direction_x; + const float range = distance - half_wall / std::abs(across); + + if (range > this->max_range) { + break; + } + + const GridPose wall{ + .position = {.x = static_cast(column), .y = static_cast(row)}, + .orientation = vertical ? (direction_x > 0.0F ? Side::RIGHT : Side::LEFT) : + (direction_y > 0.0F ? Side::UP : Side::DOWN), + }; + + const float base = static_cast(vertical ? row : column) * cell_size; + const float offset = + (vertical ? origin.position.y + distance * direction_y : origin.position.x + distance * direction_x) - base; + + const WallState state = maze.get_wall(wall); + const bool at_post = offset < half_wall or offset > cell_size - half_wall; + + if (state != WallState::WALL and (at_post or grazed_post)) { + if (state == WallState::UNKNOWN or grazed_post) { + break; + } + + grazed_post = true; + } + + if (state == WallState::NO_WALL) { + column = static_cast(column + (vertical ? (direction_x > 0.0F ? 1 : -1) : 0)); + row = static_cast(row + (vertical ? 0 : (direction_y > 0.0F ? 1 : -1))); + continue; + } + + if (range < this->min_range) { + break; + } + + const float lever = vertical ? origin.position.y - pose.position.y : -(origin.position.x - pose.position.x); + const float gap = distance * across; + const float swing = + (vertical ? 1.0F : -1.0F) * along * (gap - std::copysign(half_wall, across)) / (across * across); + + RayHit hit{ + .valid = true, + .wall = wall, + .state = state, + .range = range, + .offset = offset, + .cosine = std::abs(across), + .jacobian = {}, + .vertical = vertical, + .face = (vertical ? line_x : line_y) - std::copysign(half_wall, across), + .base = base, + .low_end = half_wall, + .high_end = cell_size - half_wall, + }; + + hit.jacobian.at(vertical ? 0 : 1) = -1.0F / across; + hit.jacobian.at(2) = lever / across + swing; + + if (state == WallState::WALL) { + hit.low_end = -half_wall; + hit.high_end = cell_size + half_wall; + + GridPose lower = wall; + GridPose higher = wall; + + for (uint8_t i = 0; i < 2; i++) { + lower.position = lower.position + (vertical ? Side::DOWN : Side::LEFT); + higher.position = higher.position + (vertical ? Side::UP : Side::RIGHT); + + if (hit.low_end == -half_wall - static_cast(i) * cell_size and + TMaze::contains(lower.position) and maze.get_wall(lower) == WallState::WALL) { + hit.low_end -= cell_size; + } + + if (hit.high_end == cell_size + half_wall + static_cast(i) * cell_size and + TMaze::contains(higher.position) and maze.get_wall(higher) == WallState::WALL) { + hit.high_end += cell_size; + } + } + } + + return hit; + } + + return {}; +} +} // namespace micras::nav + +#endif // MICRAS_NAV_WALL_MODEL_TPP diff --git a/micras_nav/include/micras/nav/impl/wall_observer.tpp b/micras_nav/include/micras/nav/impl/wall_observer.tpp new file mode 100644 index 0000000..3abfdf7 --- /dev/null +++ b/micras_nav/include/micras/nav/impl/wall_observer.tpp @@ -0,0 +1,102 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_WALL_OBSERVER_TPP +#define MICRAS_NAV_WALL_OBSERVER_TPP + +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/localizer.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/wall_model.hpp" + +namespace micras::nav { +template +TWallObserver::TWallObserver(const Config& config) : config{config} { } + +template +void TWallObserver::reset() { + this->votes.fill(0); +} + +template +bool TWallObserver::update( + const Measurements& measurements, const Localizer& localizer, const WallModel& wall_model, + TMaze& maze +) { + const State& state = localizer.get_state(); + + if (std::abs(state.velocity.angular) > this->config.max_angular_speed) { + return false; + } + + const Pose sampled = state.pose.compose({ + .position = {.x = -state.velocity.linear * this->config.range_delay, .y = 0.0F}, + .orientation = -state.velocity.angular * this->config.range_delay, + }); + + bool changed = false; + + for (uint8_t sensor = 0; sensor < number_of_wall_sensors; sensor++) { + const WallReading& reading = measurements.walls.at(sensor); + + if (not reading.is_new) { + continue; + } + + const RayHit hit = wall_model.cast(sampled, sensor, maze); + + if (not hit.valid or hit.state != WallState::UNKNOWN or + not wall_model.is_footprint_clear( + hit, sensor, localizer.get_position_deviation(), localizer.get_orientation_deviation() + )) { + continue; + } + + const float tolerance = this->config.tolerance + this->config.relative_tolerance * hit.range; + int8_t& count = this->votes.at(get_index(hit.wall)); + + if (reading.valid and std::abs(reading.distance - hit.range) <= tolerance) { + count = static_cast(std::min(count + 1, this->config.votes_to_decide)); + } else if ( + (not reading.valid or reading.distance > hit.range + tolerance) and + hit.range <= this->config.detection_range + ) { + count = static_cast(std::max(count - 1, -this->config.votes_to_decide)); + } else { + continue; + } + + if (std::abs(count) >= this->config.votes_to_decide) { + changed = maze.set_wall(hit.wall, count > 0) or changed; + } + } + + return changed; +} + +template +constexpr uint16_t TWallObserver::get_index(const GridPose& wall) { + const uint8_t x = wall.position.x; + const uint8_t y = wall.position.y; + + switch (wall.orientation) { + case Side::LEFT: + return static_cast(x * height + y); + case Side::RIGHT: + return static_cast((x + 1) * height + y); + case Side::DOWN: + return static_cast((width + 1) * height + x * (height + 1) + y); + case Side::UP: + default: + return static_cast((width + 1) * height + x * (height + 1) + y + 1); + } +} +} // namespace micras::nav + +#endif // MICRAS_NAV_WALL_OBSERVER_TPP diff --git a/micras_nav/include/micras/nav/localizer.hpp b/micras_nav/include/micras/nav/localizer.hpp new file mode 100644 index 0000000..05cacef --- /dev/null +++ b/micras_nav/include/micras/nav/localizer.hpp @@ -0,0 +1,396 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_LOCALIZER_HPP +#define MICRAS_NAV_LOCALIZER_HPP + +#include +#include + +#include "micras/nav/maze.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/wall_model.hpp" + +namespace micras::nav { +/** + * @brief The one owner of the pose of the robot. + * + * @details An extended Kalman filter over the position, the orientation and the bias of the + * gyroscope, in the maze frame. It is advanced every iteration with the encoders and the gyroscope, + * and corrected with every range of a wall sensor that can be trusted, each as one scalar + * measurement against the map of the walls. Because the filter knows that a lateral drift and an + * orientation error go together, a wall seen for a moment identifies the bias of the gyroscope, and + * the estimate stays good through the gaps, turns and diagonals that follow. Standing still is one + * more measurement, of the bias alone. + * + * Every correction goes through three guards: the illuminated spot has to lie on the face of a + * single known wall, the innovation has to be plausible for what the filter believes, and the + * correction is capped, so that a wrong reading is rejected and a wrong wall in the map can only + * drag the pose slowly. + * + * @note The covariance is kept as the factors of `P = U * D * U^T`, with U unit upper triangular and + * D diagonal, and updated with the algorithms of Thornton and Bierman. In single precision this + * stays symmetric and positive by construction, which the conventional form does not. + */ +class Localizer { +public: + /** + * @brief Number of states of the filter: x, y, orientation and gyroscope bias. + */ + static constexpr uint8_t number_of_states{4}; + + /** + * @brief Largest number of iterations the speed can be measured over. + */ + static constexpr uint8_t max_speed_window{32}; + + /** + * @brief Configuration struct for the localizer. + * + * @note The gate is the largest squared innovation accepted, in variances, and the one used + * while standing still is wider since a reading taken at rest has no timing error. The caps + * bound what one correction may move the pose by. The range delay is the delay of the filter + * the fast ranges went through. That filter is slower than the ranges are sampled, so the noise + * of consecutive ranges is mostly the same noise, and the range correlation is how many of them + * share it: the variance of each is multiplied by it, so that what a second of ranges is worth + * does not depend on how many there are in it. The speed window is the number of iterations the linear + * speed is measured over, which trades noise for delay. The rest window is the time over which + * the gyroscope is compared with the encoders to measure its bias. + * + * The ends of the side walls are used as references along the path when use_edges is set. An + * end is accepted while the robot moves forward faster than the edge speed, since the only time + * it reverses is to park, from a pose it has just measured. The wall has to be closer than the + * edge range, so that nothing can be hiding its end, and the end has to be found within the edge + * window of where it was expected. An end is a single event rather than a stream of readings, so it has + * a cap of its own. The range tolerance, a constant part plus a part proportional to the range, + * tells a reading that is on a wall from one that is past its end. + */ + struct Config { + RobotModel model; + float initial_position_deviation; + float initial_orientation_deviation; + float initial_bias_deviation; + float gate; + float stationary_gate; + float max_position_correction; + float max_orientation_correction; + float max_angular_speed; + float stationary_linear_speed; + float stationary_angular_speed; + float range_delay; + float range_correlation; + float rest_window; + bool use_edges; + float edge_deviation; + float edge_window; + float edge_speed; + float edge_range; + float max_edge_correction; + float range_tolerance; + float relative_range_tolerance; + uint8_t speed_window; + }; + + /** + * @brief Diagnostics of the filter, for a monitor. + * + * @note The innovation level is the running mean of the squared innovations in variances, which + * stays near one while the filter is consistent. + */ + struct Status { + float innovation_level; + uint32_t accepted; + uint32_t rejected; + uint32_t edges; + }; + + /** + * @brief Construct a new Localizer object. + * + * @param config The configuration for the localizer. + */ + explicit Localizer(const Config& config); + + /** + * @brief Place the robot at a known pose, forgetting the previous estimate but not the bias. + * + * @param pose The pose of the robot in the maze frame. + * @param measurements The current measurements, which give the reference for the encoders. + */ + void reset(const Pose& pose, const Measurements& measurements); + + /** + * @brief Advance the estimate by one iteration. + * + * @param measurements The current measurements. + * @param elapsed_time Time since the last iteration, in seconds. + */ + void predict(const Measurements& measurements, float elapsed_time); + + /** + * @brief Correct the estimate with the ranges of the wall sensors that can be trusted. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + * @param measurements The current measurements. + * @param wall_model The geometry of the wall sensors. + * @param maze The map of the walls, of which only the known walls are used. + */ + template + void correct(const Measurements& measurements, const WallModel& wall_model, const TMaze& maze); + + /** + * @brief Correct the bias of the gyroscope while the robot is commanded to stand still. + * + * @details The rotation the gyroscope reports over a window of time is compared with the one + * the encoders report, and the difference is a measurement of the bias. The encoders are what + * makes this safe: a robot that is slowly turning while it believes it is at rest would + * otherwise have its rotation taken for bias, which is the very error that makes it turn. + * Nothing is measured while the wheels are moving forward, since wheels that slip would then + * report a rotation that did not happen. + * + * @param measurements The current measurements. + * @param elapsed_time Time since the last iteration, in seconds. + */ + void correct_at_rest(const Measurements& measurements, float elapsed_time); + + /** + * @brief Get the estimated state of the robot. + * + * @return The pose in the maze frame and the velocity in the body frame. + */ + const State& get_state() const; + + /** + * @brief Get the estimated pose of the robot. + * + * @return The pose in the maze frame. + */ + const Pose& get_pose() const; + + /** + * @brief Get the cell the robot is in and the direction it faces. + * + * @return The grid pose derived from the estimate. + */ + GridPose get_cell() const; + + /** + * @brief Get the estimated bias of the gyroscope. + * + * @return The bias in rad/s. + */ + float get_gyroscope_bias() const; + + /** + * @brief Get the standard deviation of the estimated position. + * + * @return The largest of the deviations along x and y, in meters. + */ + float get_position_deviation() const; + + /** + * @brief Get the standard deviation of the estimated orientation. + * + * @return The deviation in radians. + */ + float get_orientation_deviation() const; + + /** + * @brief Get the diagnostics of the filter. + * + * @return The status. + */ + const Status& get_status() const; + +private: + /** + * @brief Vector with one element per state. + */ + using Vector = std::array; + + /** + * @brief Matrix with one row and one column per state. + */ + using Matrix = std::array; + + /** + * @brief Indexes of the states. + */ + ///@{ + static constexpr uint8_t x_index{0}; + static constexpr uint8_t y_index{1}; + static constexpr uint8_t orientation_index{2}; + static constexpr uint8_t bias_index{3}; + + ///@} + + /** + * @brief What a side looking sensor was seeing at its previous sample. + * + * @note The end of a wall shows up as a change between two consecutive samples, from a reading + * on the wall to a reading past it or the other way around, so both are remembered. + */ + struct EdgeTracker { + bool tracking; + bool on_wall; + RayHit hit; + }; + + /** + * @brief Look for the end of a side wall in the reading of a sensor, and use it as a reference. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + * @param sensor The index of the sensor. + * @param reading The reading of the sensor. + * @param sampled The pose of the robot when the reading was taken. + * @param hit What the axis of the sensor meets from that pose. + * @param wall_model The geometry of the wall sensors. + * @param maze The map of the walls. + */ + template + void track_edge( + uint8_t sensor, const WallReading& reading, const Pose& sampled, const RayHit& hit, const WallModel& wall_model, + const TMaze& maze + ); + + /** + * @brief Set the covariance to a diagonal one. + * + * @param variances The variance of each state. + */ + void set_covariance(const Vector& variances); + + /** + * @brief Propagate the covariance through one iteration, with the algorithm of Thornton. + * + * @param transition The derivative of the new state with respect to the old one. + * @param noise_input How each source of process noise enters the state. + * @param noise_variances The variance of each source of process noise. + */ + void propagate(const Matrix& transition, const Matrix& noise_input, const Vector& noise_variances); + + /** + * @brief Apply one scalar measurement, with the algorithm of Bierman. + * + * @param innovation The measurement minus its prediction. + * @param jacobian The derivative of the measurement with respect to the state. + * @param variance The variance of the measurement noise. + * @param gate The largest squared innovation accepted, in variances. + * @param max_position_correction The most the measurement may move the position by. + * @return True if the measurement was accepted. + */ + bool update(float innovation, const Vector& jacobian, float variance, float gate, float max_position_correction); + + /** + * @brief Get the variance of one state. + * + * @param index The index of the state. + * @return The element of the diagonal of the covariance. + */ + float get_variance(uint8_t index) const; + + /** + * @brief Check if the robot is standing still, as far as its sensors can tell. + * + * @return True if both speeds are below the stationary thresholds. + */ + bool is_stationary() const; + + /** + * @brief Parameters of the filter, with the physical description of the robot. + */ + Config config; + + /** + * @brief Estimated state of the robot. + */ + State state{}; + + /** + * @brief Estimated bias of the gyroscope. + */ + float bias{}; + + /** + * @brief Unit upper triangular factor of the covariance. + */ + Matrix upper{}; + + /** + * @brief Diagonal factor of the covariance. + */ + Vector diagonal{}; + + /** + * @brief Wheel angles at the last iteration. + */ + ///@{ + float last_left_angle{}; + float last_right_angle{}; + ///@} + + /** + * @brief Time since the gyroscope last produced a sample. + */ + float time_since_imu{}; + + /** + * @brief Duration of the window over which the bias is being measured. + */ + float rest_time{}; + + /** + * @brief Rotation reported by the gyroscope over the window. + */ + float rest_rotation{}; + + /** + * @brief Difference between the wheel angles at the start of the window. + */ + float rest_wheel_difference{}; + + /** + * @brief Distance traveled at each of the last iterations, for the speed estimate. + */ + std::array distances{}; + + /** + * @brief Duration of each of the last iterations, for the speed estimate. + */ + std::array durations{}; + + /** + * @brief Index of the oldest entry of the speed window. + */ + uint8_t window_index{}; + + /** + * @brief Distance traveled over the speed window. + */ + float window_distance{}; + + /** + * @brief Duration of the speed window. + */ + float window_duration{}; + + /** + * @brief What each sensor was seeing at its previous sample. + */ + std::array edge_trackers{}; + + /** + * @brief Diagnostics of the filter. + */ + Status status{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/localizer.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_LOCALIZER_HPP diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index 435c8c0..d4fef81 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -5,35 +5,49 @@ #ifndef MICRAS_NAV_MAZE_HPP #define MICRAS_NAV_MAZE_HPP +#include #include -#include -#include -#include +#include +#include +#include #include "micras/core/serializable.hpp" -#include "micras/core/types.hpp" -#include "micras/nav/action_queuer.hpp" -#include "micras/nav/costmap.hpp" #include "micras/nav/grid_pose.hpp" namespace micras::nav { /** - * @brief Class for storing the robot information about the maze. + * @brief What is known about a wall. + */ +enum class WallState : uint8_t { + UNKNOWN = 0, + NO_WALL = 1, + WALL = 2, +}; + +/** + * @brief Map of the walls of the maze, with the flood fill that guides the search. * - * @tparam width The width of the maze. - * @tparam height The height of the maze. + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. */ template class TMaze : public core::ISerializable { public: /** - * @brief Configuration structure for the maze. + * @brief Cost of a cell that cannot reach any target. + */ + static constexpr uint16_t unreachable{0xFFFF}; + + /** + * @brief Configuration struct for the maze. + * + * @note The goal is borrowed, so it has to outlive the maze, which a constant of the + * configuration does. */ + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) no defaults, so that a missing field is a warning struct Config { - GridPose start{}; - std::unordered_set goal; - float cost_margin{}; - ActionQueuer::Config action_queuer_config{}; + GridPose start; + std::span goal; }; /** @@ -44,165 +58,195 @@ class TMaze : public core::ISerializable { explicit TMaze(const Config& config); /** - * @brief Update the maze walls with the current pose and new information. + * @brief Forget every wall that was observed, keeping what the rules guarantee. * - * @param pose The pose of the robot. - * @param observation The observation from the wall sensors. + * @note The rules give the border, the walls around the start cell and the absence of walls + * between the cells of the goal. */ - void update_walls(const GridPose& pose, const core::Observation& observation); + void reset(); /** - * @brief Return the next point the robot should go based on the costmap. + * @brief Check if a cell is inside the maze. * - * @param position The current position of the robot. - * @param returning Whether the robot is returning to the start position. - * @return The next point the robot should go. + * @param position The position of the cell. + * @return True if the cell exists. */ - GridPose get_next_goal(const GridPose& pose, bool returning); + static constexpr bool contains(const GridPoint& position) { return position.x < width and position.y < height; } /** - * @brief Check whether the robot has finished the maze. + * @brief Get what is known about the wall ahead of a pose. * - * @param position The current position of the robot. - * @param returning Whether the robot is returning to the start position. - * @return True if the robot has finished the maze, false otherwise. + * @param pose The cell and the side of it. + * @return The state of the wall, which is a wall for anything outside of the maze. */ - bool finished(const GridPoint& position, bool returning) const; + WallState get_wall(const GridPose& pose) const; /** - * @brief Calculate the best route to the goal using the current costmap. + * @brief Record the observation of a wall that was unknown. + * + * @note A wall is decided once: an observation of a wall that is already known changes nothing. + * + * @param pose The cell and the side of it. + * @param present Whether there is a wall. + * @return True if the map changed. */ - void compute_best_route(); + bool set_wall(const GridPose& pose, bool present); /** - * @brief Return the best route to the goal. + * @brief Check if the robot cannot go through, as far as it is known. * - * @return The best route to the goal. + * @param pose The cell and the side of it. + * @return True if there is a wall. */ - const std::list& get_best_route() const; + bool is_blocked(const GridPose& pose) const; /** - * @brief Serialize the best route to the goal. + * @brief Check if the robot might go through, which is what the search assumes. * - * @return The serialized data. + * @param pose The cell and the side of it. + * @return True unless a wall was seen. */ - std::vector serialize() const override; + bool is_possibly_open(const GridPose& pose) const; /** - * @brief Deserialize the best route to the goal. + * @brief Check if the robot can go through for certain, which is what a fast run requires. * - * @param buffer The serialized data. - * @param size The size of the serialized data. + * @param pose The cell and the side of it. + * @return True if the absence of a wall was seen. */ - void deserialize(const uint8_t* buffer, uint16_t size) override; + bool is_known_open(const GridPose& pose) const; -private: /** - * @brief The layers of the costmap. + * @brief Check if a cell belongs to the goal. + * + * @param position The position of the cell. + * @return True if the cell is part of the goal. */ - enum class Layer : uint8_t { - EXPLORE = 0, - RETURN = 1, - NUM_OF_LAYERS = 2, - }; + bool is_goal(const GridPoint& position) const; /** - * @brief Compute the minumum cost from the start to the end considering only discoverd cells. + * @brief Get the pose the robot starts from. + * + * @return The start cell and the direction the robot faces in it. */ - void compute_minimum_cost(); + const GridPose& get_start() const; /** - * @brief Update the cell costs at the given position. + * @brief Get the cells of the goal. * - * @param position The position of the cell. + * @return The cells of the goal. */ - void update_cell(const GridPoint& position); + std::span get_goal() const; /** - * @brief Get the next goal for the robot using a BFS algorithm. + * @brief Get a number that changes every time a wall is recorded. * - * @param pose The current pose of the robot. - * @param discover Whether the robot is discovering new cells. - * @return A pair containing the next discovery goal for the robot and the total distance. + * @return The number of changes since the map was last reset. */ - std::pair get_next_bfs_goal(const GridPose& pose, bool discover) const; + uint32_t get_revision() const; - void recursive_backtracking( - const GridPoint& position, std::list& route, std::unordered_set& visited - ); - - uint16_t heuristic(const GridPoint& position) const; + /** + * @brief Compute the number of cells from every cell to the nearest target. + * + * @note Walls that were not seen count as absent. The whole maze is flooded again on every + * call, from a queue with room for every cell, so the cost is bounded and nothing is allocated. + * + * @param targets The cells with a cost of zero. + */ + void flood(std::span targets); - float get_route_time(const std::list& route); + /** + * @brief Get the number of cells from a cell to the nearest target of the last flood. + * + * @param position The position of the cell. + * @return The cost of the cell, or unreachable. + */ + uint16_t get_cost(const GridPoint& position) const; /** - * @brief Check if the cell is a dead end. + * @brief Get the neighbor to move to in order to approach the targets of the last flood. * - * @param cell The cell to check. - * @return True if the cell is a dead end, false otherwise. + * @note Going straight is preferred to turning and turning to going back, among the neighbors + * that are equally close. + * + * @param pose The current cell and the direction the robot faces. + * @return The neighbor and the direction to reach it, or nothing if no target can be reached. */ - static bool is_dead_end(const Costmap::Cell& cell); + std::optional get_next(const GridPose& pose) const; /** - * @brief Check if the cell was visited. + * @brief Serialize the walls of the maze. * - * @param cell The cell to check. - * @return True if the cell was visited, false otherwise. + * @return The version, the size of the maze and the state of every wall. */ - static bool was_visited(const Costmap::Cell& cell); + std::vector serialize() const override; /** - * @brief Check if the cell must be visited. + * @brief Load the walls of the maze. + * + * @note A record of another version or size is ignored. * - * @param cell The cell to check. - * @param cost_threshold The cost threshold for the cell. - * @return True if the cell must be visited, false otherwise. + * @param buffer The serialized data. + * @param size The size of the serialized data. */ - static bool must_visit( - const Costmap::Cell& cell, int16_t cost_threshold - ); + void deserialize(const uint8_t* buffer, uint16_t size) override; +private: /** - * @brief Layered costmap for the maze. + * @brief Version of the serialized format. */ - Costmap costmap; + static constexpr uint8_t format_version{1}; - ActionQueuer action_queuer; + /** + * @brief Number of bytes of the serialized walls, at two bits for each of two sides of a cell. + */ + static constexpr uint16_t walls_size{(width * height + 1) / 2}; /** - * @brief Start pose of the robot in the maze. + * @brief Number of bytes of the header of the serialized format. */ - GridPose start; + static constexpr uint16_t header_size{3}; /** - * @brief Goal points in the maze. + * @brief Write the state of a wall on both of the cells it belongs to. + * + * @param pose The cell and the side of it. + * @param state The state of the wall. */ - std::unordered_set goal; + void write_wall(const GridPose& pose, WallState state); /** - * @brief Cost margin above the minimum cost that the robot should explore. + * @brief State of the walls of every cell, indexed by row, column and side. */ - float cost_margin; + std::array, width>, height> walls{}; /** - * @brief Minimum cost of path to the goal containing only visited cells. + * @brief Number of cells from every cell to the nearest target of the last flood. */ - int16_t minimum_cost{}; + std::array, height> costs{}; /** - * @brief Flag indicating whether the robot has finished discovering the cells of the maze. + * @brief Queue of the flood fill, with room for every cell. */ - bool finished_discovery{false}; + std::array(width) * height> queue{}; /** - * @brief Current best found route to the goal. + * @brief Pose the robot starts from. */ - std::list best_route; + GridPose start; - float best_route_time{std::numeric_limits::max()}; + /** + * @brief Cells of the goal. + */ + std::span goal; + + /** + * @brief Number of walls recorded since the last reset. + */ + uint32_t revision{}; }; } // namespace micras::nav -#include "../src/maze.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) +#include "micras/nav/impl/maze.tpp" // IWYU pragma: export #endif // MICRAS_NAV_MAZE_HPP diff --git a/micras_nav/include/micras/nav/mission.hpp b/micras_nav/include/micras/nav/mission.hpp new file mode 100644 index 0000000..a0ed953 --- /dev/null +++ b/micras_nav/include/micras/nav/mission.hpp @@ -0,0 +1,437 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_MISSION_HPP +#define MICRAS_NAV_MISSION_HPP + +#include +#include +#include +#include + +#include "micras/core/types.hpp" +#include "micras/nav/executor.hpp" +#include "micras/nav/explorer.hpp" +#include "micras/nav/localizer.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/wall_model.hpp" +#include "micras/nav/wall_observer.hpp" + +namespace micras::nav { +/** + * @brief Sequencing of a run: what to map, where to go next and which motion gets there. + * + * @details The mission owns the outer loop of the navigation: the wall observer fills the map from + * the readings, the map is flooded towards whatever is worth reaching, and one cell at a time the + * next move is handed to the executor, which turns it into the reference the controller follows. + * + * - Exploring, the robot descends the flood towards the goal and stops at it. + * - Returning, the explorer names the cells still worth visiting and the robot descends the flood + * towards them, until the map proves that the fastest route is known. Then it goes back to the + * start cell, squares up against its back wall and parks where a run starts from. + * - Solving, the route planned beforehand is executed as it is. + * + * The move through a cell is decided when the robot crosses into it, as late as it can be, from the + * walls that were decided by then. A turn is only made through a wall that was seen absent. A + * straight may start with the wall ahead still unknown, since that wall only comes into range + * inside the cell, but then it is watched: if it turns out to be there, or is still unknown at the + * last point from which the robot can stop at the center of the cell, the robot stops there and + * decides again. A wall seen late costs a stop instead of a crash. + * + * The mission keeps its own account of which cell the robot is in, from the moves it made. Whenever + * the robot stands still at the center of a cell, that account is checked against the pose estimate, + * and a run in which the two disagree fails instead of going on to write walls in the wrong cells. + * + * @note Every decision takes a bounded time, since it happens while the robot is moving. The search + * of the planner, which does not, is spread over the iterations. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TMission { +public: + /** + * @brief Progress of a run. + */ + enum class Status : uint8_t { + RUNNING = 0, + FINISHED = 1, + FAILED = 2, + }; + + /** + * @brief Configuration struct for the mission. + * + * @note The start offset is the distance from the back edge of the start cell to the axle of + * the robot when a run starts. The map profiles are the run profiles the map has to be complete + * for, borrowed for the lifetime of the mission. The times are how long to stand still before + * turning in place, at most while squaring up against a wall and between two looks at a wall + * that is still unknown, of which only so many are taken before giving up. The commit margin is + * kept between where the robot could still stop at the center of a cell and where it decides to. + * + * No field has a default value, here or in any other configuration of the navigation, so that + * leaving one out of a configuration is a compiler warning instead of a silent zero. + */ + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) see the note above + struct Config { + TMaze::Config maze; + TPlanner::Config planner; + TWallObserver::Config observer; + Executor::Config executor; + RunProfile search_profile; + std::span map_profiles; + float start_offset; + float stop_time; + float attach_time; + float look_time; + uint16_t max_looks; + float commit_margin; + uint32_t nodes_per_iteration; + }; + + /** + * @brief Construct a new TMission object. + * + * @param dynamics The physical limits of the robot, borrowed for the lifetime of the mission. + * @param wall_model The geometry of the wall sensors, borrowed for the same time. + * @param config The configuration for the mission. + */ + TMission(const Dynamics& dynamics, const WallModel& wall_model, const Config& config); + + /** + * @brief Get the map of the walls. + * + * @return The maze. + */ + TMaze& get_maze(); + + /** + * @brief Get the map of the walls. + * + * @return The maze. + */ + const TMaze& get_maze() const; + + /** + * @brief Get the pose a run starts from. + * + * @return The pose in the maze frame. + */ + Pose get_start_pose() const; + + /** + * @brief Start planning the route of a fast run. + * + * @param profile The profile of the run. + */ + void begin_plan(const RunProfile& profile); + + /** + * @brief Advance the planning of the route of a fast run. + * + * @note Only to be called with the robot stopped: when the search ends, the best candidates + * are compiled and timed, which is not bounded to fit in an iteration. + * + * @param max_nodes The largest number of nodes the planner may expand in this call. + * @return True if the planning has finished. + */ + bool update_plan(uint32_t max_nodes); + + /** + * @brief Check if a route for a fast run exists. + * + * @return True if the last planning found a route. + */ + bool has_route() const; + + /** + * @brief Get the time the planned route is expected to take. + * + * @return The time from the start until the robot comes to rest inside the goal, in seconds. + */ + float get_route_time() const; + + /** + * @brief Start a run. + * + * @note Exploring and solving start from the start pose. Returning starts from wherever the + * exploration ended, with the robot stopped at the center of a cell. + * + * @param objective What the run is for. + */ + void start(core::Objective objective); + + /** + * @brief Advance the run by one iteration. + * + * @param measurements The current measurements. + * @param localizer The pose estimate, which is corrected while the robot is commanded to rest. + * @param elapsed_time Time since the last iteration, in seconds. + * @param time_scale How much slower the reference has to be played, from the controller. + * @return The progress of the run. + */ + Status update(const Measurements& measurements, Localizer& localizer, float elapsed_time, float time_scale); + + /** + * @brief Get what the robot should be doing at this instant. + * + * @return The reference in the maze frame. + */ + const Reference& get_reference() const; + + /** + * @brief Get the playback of the moves, to see which segment is in progress. + * + * @return The executor. + */ + const Executor& get_executor() const; + + /** + * @brief Get the cell the mission believes the robot is in or entering. + * + * @note Comparing it with the cell of the pose estimate tells whether the two still agree. + * + * @return The cell and the direction of travel. + */ + const GridPose& get_cell() const; + +private: + /** + * @brief Largest number of segments a move through one cell takes. + */ + static constexpr uint8_t max_move_segments{6}; + + /** + * @brief Shortest straight worth a segment of its own, in meters. + */ + static constexpr float min_straight{1.0e-4F}; + + /** + * @brief Distance within which a segment is taken to start where the watched cell is entered. + */ + static constexpr float watch_tolerance{0.001F}; + + /** + * @brief Small list of segments, built on the stack for one move. + */ + struct Move { + /** + * @brief Append a segment. + * + * @param segment The segment. + */ + void add(const Segment& segment) { + if (this->size < max_move_segments) { + this->segments.at(this->size++) = segment; + } + } + + std::array segments{}; + uint8_t size{}; + }; + + /** + * @brief Get the nominal pose of the robot as it crosses into a cell. + * + * @param cell The cell entered and the direction of travel. + * @return The pose at the middle of the edge of the cell, in the maze frame. + */ + Pose get_entry_pose(const GridPose& cell) const; + + /** + * @brief Get the nominal pose of the robot at the center of a cell. + * + * @param cell The cell and the direction the robot faces. + * @return The pose in the maze frame. + */ + Pose get_center_pose(const GridPose& cell) const; + + /** + * @brief Flood the map towards whatever the current objective wants to reach. + */ + void flood(); + + /** + * @brief Plan the speeds of a move and hand it to the executor. + * + * @param move The segments of the move. + * @param start_speed The speed at the start of the move. + * @param end_speed The speed at the end of the move. + */ + void execute(Move& move, float start_speed, float end_speed); + + /** + * @brief Decide the next move, with the robot crossing into a cell at the search speed. + */ + void decide_at_entry(); + + /** + * @brief Decide the next move, with the robot stopped at the center of a cell. + */ + void decide_at_center(); + + /** + * @brief Add to a move the segments that stop the robot at the center of the cell it is entering. + * + * @note When the wall ahead is known to be there, the robot then squares up against it. + * Otherwise it just stands still for a moment. + * + * @param move The move. + */ + void add_stop_at_center(Move& move) const; + + /** + * @brief Check if the run is over now that the robot is entering a cell, and end it if so. + * + * @return True if the last move of the run was handed to the executor. + */ + bool finish_at_entry(); + + /** + * @brief Stop at the center of the cell being crossed instead of going through it. + */ + void divert_to_center(); + + /** + * @brief Make a segment. + * + * @param kind The kind of the segment. + * @param length The length of the segment, in the unit of its kind. + * @param start The pose the segment nominally starts from. + * @return The segment, with no speeds. + */ + static Segment make_segment(SegmentKind kind, float length, const Pose& start); + + /** + * @brief Physical limits of the robot. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const Dynamics& dynamics; + + /** + * @brief Geometry of the wall sensors. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const WallModel& wall_model; + + /** + * @brief Parameters of the mission. + */ + Config config; + + /** + * @brief Map of the walls. + */ + TMaze maze; + + /** + * @brief Decision of which walls exist. + */ + TWallObserver observer; + + /** + * @brief Search for the fastest route. + */ + TPlanner planner; + + /** + * @brief Choice of what is still worth visiting. + */ + TExplorer explorer; + + /** + * @brief Playback of the moves as a reference. + */ + Executor executor; + + /** + * @brief Speed the robot searches at, which every move through a cell starts and ends with. + */ + float search_speed; + + /** + * @brief What the run in progress is for. + */ + core::Objective objective{core::Objective::EXPLORE}; + + /** + * @brief Progress of the run in progress. + */ + Status status{Status::FINISHED}; + + /** + * @brief Cell the robot is in or entering, and the direction of travel. + */ + GridPose cell{}; + + /** + * @brief Whether the robot is stopped at the center of the cell, rather than crossing into it. + */ + bool at_center{}; + + /** + * @brief Whether the moves handed to the executor are the last ones of the run. + */ + bool finishing{}; + + /** + * @brief Whether the robot is crossing a cell whose wall ahead was unknown when it entered. + */ + bool watching_front{}; + + /** + * @brief Cell being crossed while the wall ahead of it is watched. + */ + GridPose watched_cell{}; + + /** + * @brief Number of times in a row the robot stood still waiting for a wall to be decided. + */ + uint16_t looks{}; + + /** + * @brief Profile of the fast run that was planned. + */ + RunProfile solve_profile{}; + + /** + * @brief Segments of the fast run that was planned. + */ + std::vector solve_segments; + + /** + * @brief Segments of a candidate route being timed. + */ + std::vector candidate_segments; + + /** + * @brief Candidate route being compiled, kept to reuse its storage. + */ + Route candidate_route; + + /** + * @brief Time the planned fast run is expected to take. + */ + float route_time{}; + + /** + * @brief Whether the route of a fast run is being planned. + */ + bool planning{}; + + /** + * @brief Last reference produced. + */ + Reference reference{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/mission.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_MISSION_HPP diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp deleted file mode 100644 index 3160545..0000000 --- a/micras_nav/include/micras/nav/odometry.hpp +++ /dev/null @@ -1,124 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_ODOMETRY_HPP -#define MICRAS_NAV_ODOMETRY_HPP - -#include "micras/core/butterworth_filter.hpp" -#include "micras/nav/state.hpp" -#include "micras/proxy/imu.hpp" -#include "micras/proxy/rotary_sensor.hpp" - -namespace micras::nav { -/** - * @brief Class for computing the robot odometry. - */ -class Odometry { -public: - /** - * @brief Configuration for the odometry. - */ - struct Config { - core::ButterworthFilter::Config linear_filter; - float wheel_radius; - Pose initial_pose; - }; - - /** - * @brief Construct a new Odometry object. - * - * @param left_rotary_sensor Left rotary sensor. - * @param right_rotary_sensor Right rotary sensor. - * @param imu IMU sensor. - * @param config Configuration for the odometry. - */ - Odometry( - const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - const proxy::Imu& imu, Config config - ); - - /** - * @brief Update the odometry. - * - * @param elapsed_time Time since the last update. - */ - void update(float elapsed_time); - - /** - * @brief Reset the odometry. - */ - void reset(); - - /** - * @brief Get the state of the robot. - * - * @return Current state of the robot in space. - */ - const State& get_state() const; - - /** - * @brief Get the state of the robot. - * - * @return Current state of the robot in space. - */ - State& get_state(); - - /** - * @brief Set the state of the robot. - * - * @param state New state of the robot. - */ - void set_state(const State& new_state); - -private: - // NOLINTBEGIN(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot - /** - * @brief Left rotary sensor. - */ - const proxy::RotarySensor& left_rotary_sensor; - - /** - * @brief Right rotary sensor. - */ - const proxy::RotarySensor& right_rotary_sensor; - - /** - * @brief IMU sensor. - */ - const proxy::Imu& imu; - // NOLINTEND(*-avoid-const-or-ref-data-members) - - /** - * @brief Wheel radius. - */ - float wheel_radius; - - /** - * @brief Initial pose of the robot. - */ - Pose initial_pose; - - /** - * @brief Last left rotary sensor position. - */ - float left_last_position{}; - - /** - * @brief Last right rotary sensor position. - */ - float right_last_position{}; - - /** - * @brief Linear velocity filter. - */ - core::ButterworthFilter linear_filter; - - /** - * @brief Current state of the robot in space. - */ - State state; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_ODOMETRY_HPP diff --git a/micras_nav/include/micras/nav/planner.hpp b/micras_nav/include/micras/nav/planner.hpp new file mode 100644 index 0000000..5fd4443 --- /dev/null +++ b/micras_nav/include/micras/nav/planner.hpp @@ -0,0 +1,498 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_PLANNER_HPP +#define MICRAS_NAV_PLANNER_HPP + +#include +#include +#include +#include +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" + +namespace micras::nav { +/** + * @brief What the planner assumes about the walls that were never observed. + */ +enum class WallAssumption : uint8_t { + OPTIMISTIC = 0, // An unknown wall is absent: the fastest route that could still exist. + PESSIMISTIC = 1, // An unknown wall is present: the fastest route that is known to exist. +}; + +/** + * @brief One move of a route: a number of steps straight ahead, followed by a turn. + * + * @note The last move of a route may have no turn, when the goal is entered on a straight. + */ +struct RouteStep { + uint8_t run; + bool has_turn; + TurnId turn; + TurnSide side; +}; + +/** + * @brief A route from the start to the goal, as the moves that drive it. + * + * @note The stop distance is measured from the node where the last move ends to the place inside the + * goal where the robot comes to rest, and the finish distance from the line where the robot enters + * the goal, which is where the clock of a run stops, to that same place. The time is the one the + * search found for reaching that line. + */ +struct Route { + LatticePose start{}; + std::vector steps; + float stop_distance{}; + float finish_distance{}; + float time{std::numeric_limits::infinity()}; +}; + +/** + * @brief Search for the fastest route from the start to the goal. + * + * @details The search is a Dijkstra over the lattice of lattice.hpp. A state is a node together with + * the turn that was just made to reach it, because that turn fixes the speed the next straight + * starts at and how much of it the turn already covered. An edge is a motion the robot actually + * executes, a run of steps ahead followed by a turn, and it exists only if every wall it crosses + * passes the wall assumption. Its cost is the time of the run, from the evaluator the executor plays + * back, plus the time of the turn at the speed the run profile gives it, so the fastest path of the + * graph is the fastest route the robot can drive. Diagonals, the turns of 135 degrees and the two + * sizes of the turn of 90 degrees are part of the search rather than a rewrite of its result. + * + * The clock of a run stops when the robot enters the goal, so a route is priced up to that line, + * while still being required to come to rest inside the goal. + * + * @note The arrays of the search are members, which makes an object of this class large: it belongs + * in static storage, not on the stack. The search can be advanced a bounded number of nodes at a + * time, which is how it runs while the robot is moving. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TPlanner { +public: + /** + * @brief Number of finished routes kept, for the caller to time exactly and pick from. + * + * @note The cost of an edge cannot see two turns so close together that one of them has to be + * driven slower, which the velocity planner can. Timing the few best candidates with it closes + * that gap. + */ + static constexpr uint8_t number_of_candidates{4}; + + /** + * @brief Configuration struct for the planner. + * + * @note The start distance is how far the robot is from the first wall it crosses when a run + * begins. + */ + struct Config { + float start_distance; + }; + + /** + * @brief Construct a new TPlanner object. + * + * @param dynamics The physical limits of the robot, borrowed for the lifetime of the planner. + * @param config The configuration for the planner. + */ + TPlanner(const Dynamics& dynamics, const Config& config); + + /** + * @brief Start a search. + * + * @param maze The map of the walls, borrowed until the search ends. + * @param assumption What to assume about the walls that were never observed. + * @param profile The profile of the run the route is for. + */ + void begin(const TMaze& maze, WallAssumption assumption, const RunProfile& profile); + + /** + * @brief Advance the search. + * + * @param max_nodes The largest number of nodes to expand in this call. + * @return True if the search has finished. + */ + bool step(uint32_t max_nodes); + + /** + * @brief Check if the search has finished. + * + * @return True if there is nothing left to expand. + */ + bool is_finished() const; + + /** + * @brief Get the number of routes found. + * + * @return The number of candidates, which is zero if the goal cannot be reached. + */ + uint8_t get_number_of_routes() const; + + /** + * @brief Get one of the routes found. + * + * @param index The index of the candidate, the first being the fastest for the search. + * @param route The route to fill in, whose storage is reused. + */ + void get_route(uint8_t index, Route& route) const; + + /** + * @brief Call a function for every wall a route crosses. + * + * @tparam F Type of the function, taking the cell and the side of the wall as a GridPose. + * @param route The route. + * @param function The function to call. + */ + template + static void for_each_wall(const Route& route, F&& function); + +private: + /** + * @brief How a state was reached, which fixes the speed and the offset of the next straight. + */ + enum class Arrival : uint8_t { + REST = 0, + SS90S = 1, + SS90L = 2, + SS180 = 3, + DS45 = 4, + DS135 = 5, + SD45 = 6, + SD135 = 7, + DD90 = 8, + NUMBER_OF_ARRIVALS = 9, + }; + + /** + * @brief Node of the search. + */ + struct State { + LatticePose node; + Arrival arrival; + }; + + /** + * @brief End of a route inside the goal. + */ + struct Terminal { + float cost; + uint16_t state; + uint8_t run; + bool has_turn; + TurnId turn; + TurnSide side; + float stop_distance; + float finish_distance; + }; + + /** + * @brief Cost of an edge, and how fast its turn is driven. + * + * @note The speed is a fraction of the speed the run profile gives the turn, in steps of 1/255, + * and is lower than that only when the run before the turn is too short to reach it. + */ + struct Edge { + float cost; + uint8_t speed_ratio; + }; + + /** + * @brief Speed ratio of a turn driven at the speed the run profile gives it. + */ + static constexpr uint8_t full_speed{255}; + + /** + * @brief Number of states of one wall: two headings along the grid reached in six ways, and + * four diagonal headings reached in three. + */ + static constexpr uint16_t states_per_wall{24}; + + /** + * @brief Number of walls of the maze, including the ones of the border. + */ + static constexpr uint16_t number_of_walls{(width + 1) * height + width * (height + 1)}; + + /** + * @brief Number of states of the search. + */ + static constexpr uint16_t number_of_states{number_of_walls * states_per_wall}; + + /** + * @brief Largest number of steps of a run. + */ + static constexpr uint8_t max_run{2 * (width > height ? width : height)}; + + /** + * @brief Number of states whose cost is reset for the price of expanding one node. + * + * @note Resetting every state takes longer than an iteration of the control loop may, so it is + * spread over the calls to step like the search itself. + */ + static constexpr uint16_t states_per_node_budget{512}; + + /** + * @brief Marker of a state that is not in the queue. + */ + static constexpr uint16_t not_queued{0xFFFF}; + + /** + * @brief Get the arrival that a turn produces. + * + * @param turn The turn. + * @return The arrival of the state at the exit of the turn. + */ + static constexpr Arrival to_arrival(TurnId turn); + + /** + * @brief Get the turn that produced an arrival. + * + * @param arrival The arrival, which must not be REST. + * @return The turn. + */ + static constexpr TurnId to_turn(Arrival arrival); + + /** + * @brief Check if a node is inside the maze. + * + * @param node The node. + * @return True if the node is the midpoint of a wall of the maze. + */ + static constexpr bool is_inside(const LatticePose& node); + + /** + * @brief Get the index of a state. + * + * @param state The state, whose node must be valid and inside the maze. + * @return The index of the state in the arrays of the search. + */ + static constexpr uint16_t encode(const State& state); + + /** + * @brief Get the state behind an index. + * + * @param index The index of the state in the arrays of the search. + * @return The state. + */ + static constexpr State decode(uint16_t index); + + /** + * @brief Check if the robot may cross the wall a node is on. + * + * @param node The node. + * @return True if the wall passes the wall assumption. + */ + bool is_traversable(const LatticePose& node) const; + + /** + * @brief Check if a cell of the lattice belongs to the goal. + * + * @param cell The coordinates of the cell, which may be outside of the maze. + * @return True if the cell is part of the goal. + */ + bool is_goal(const LatticePoint& cell) const; + + /** + * @brief Get how far past a node the robot can stop, when it enters the goal there. + * + * @param node The node where the goal is entered, heading along the grid. + * @return The distance from the node to the center of the last cell of the goal ahead. + */ + float get_stop_distance(const LatticePose& node) const; + + /** + * @brief Get the speed a state leaves its node with. + * + * @param index The index of the state. + * @param arrival The arrival of the state. + * @return The speed in m/s. + */ + float get_speed(uint16_t index, Arrival arrival) const; + + /** + * @brief Get the straight left between the curve that reached a state and its node. + * + * @param arrival The arrival of the state. + * @return The distance in meters. + */ + float get_offset(Arrival arrival) const; + + /** + * @brief Get the time of a run followed by a turn. + * + * @details When the run is too short to change from the speed of one turn to the speed of the + * other, one of the turns has to be driven slower, which is what the velocity planner will do + * to it. If it is the turn ahead, it is priced at the speed the run allows and the state it leads + * to remembers that speed. If it is the turn behind, which was already priced, the time it + * loses is added to this edge. + * + * @param index The index of the state the run starts from. + * @param arrival The arrival of that state. + * @param run The number of steps of the run. + * @param turn The turn after the run. + * @return The time from the end of the previous curve to the end of this one, and the speed of + * this one. + */ + Edge get_edge(uint16_t index, Arrival arrival, uint8_t run, TurnId turn); + + /** + * @brief Expand a state, relaxing every edge that leaves it. + * + * @param index The index of the state. + */ + void expand(uint16_t index); + + /** + * @brief Relax the edges of one turn, to both sides, from the end of a run. + * + * @param index The index of the state the run starts from. + * @param state The state the run starts from. + * @param entry The node where the run ends and the turn starts. + * @param run The number of steps of the run. + * @param turn The turn. + */ + void relax_turn(uint16_t index, const State& state, const LatticePose& entry, uint8_t run, TurnId turn); + + /** + * @brief Record a route that ends inside the goal, if it is among the best. + * + * @param terminal The end of the route. + */ + void add_terminal(const Terminal& terminal); + + /** + * @brief Lower the cost of a state, moving it up in the queue. + * + * @param index The index of the state. + * @param cost The new cost, lower than the current one. + * @param link How the state was reached, for rebuilding the route. + * @param speed_ratio How fast the turn that reached the state is driven. + */ + void relax(uint16_t index, float cost, uint16_t link, uint8_t speed_ratio); + + /** + * @brief Remove the state with the lowest cost from the queue. + * + * @return The index of the state. + */ + uint16_t pop(); + + /** + * @brief Move an entry of the queue up to its place. + * + * @param position The position of the entry in the queue. + */ + void sift_up(uint16_t position); + + /** + * @brief Move an entry of the queue down to its place. + * + * @param position The position of the entry in the queue. + */ + void sift_down(uint16_t position); + + /** + * @brief Physical limits of the robot. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const Dynamics& dynamics; + + /** + * @brief Distance from the start pose to the first node. + */ + float start_distance; + + /** + * @brief Map of the walls being searched. + */ + const TMaze* maze{nullptr}; + + /** + * @brief Assumption about the unknown walls. + */ + WallAssumption assumption{WallAssumption::PESSIMISTIC}; + + /** + * @brief Profile of the run being planned. + */ + RunProfile run_profile{}; + + /** + * @brief Limits of the motion along a straight, for the run being planned. + */ + MotionLimits limits{}; + + /** + * @brief Speed of each turn, for the run being planned. + */ + std::array turn_speeds{}; + + /** + * @brief Edge of every combination of arrival, run and turn, for an arrival at full speed, + * filled in when first needed. + */ + std::array< + std::array, max_run + 1>, std::to_underlying(Arrival::NUMBER_OF_ARRIVALS)> + edges{}; + + /** + * @brief Lowest cost found for each state. + */ + std::array costs{}; + + /** + * @brief How each state was reached: the run, the previous arrival and the side of the turn. + */ + std::array links{}; + + /** + * @brief How fast the turn that reached each state is driven, as a fraction of its speed. + */ + std::array speed_ratios{}; + + /** + * @brief Binary heap of the states waiting to be expanded, ordered by cost. + */ + std::array queue{}; + + /** + * @brief Position of each state in the queue. + */ + std::array positions{}; + + /** + * @brief Number of states in the queue. + */ + uint16_t queue_size{}; + + /** + * @brief Number of states whose cost was reset since the search began. + */ + uint16_t cleared{number_of_states}; + + /** + * @brief Best ends of route found, ordered by cost. + */ + std::array terminals{}; + + /** + * @brief Number of ends of route found. + */ + uint8_t number_of_terminals{}; + + /** + * @brief Start node of the search. + */ + LatticePose start{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/planner.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_PLANNER_HPP diff --git a/micras_nav/include/micras/nav/route_compiler.hpp b/micras_nav/include/micras/nav/route_compiler.hpp new file mode 100644 index 0000000..d6ee2a4 --- /dev/null +++ b/micras_nav/include/micras/nav/route_compiler.hpp @@ -0,0 +1,53 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_ROUTE_COMPILER_HPP +#define MICRAS_NAV_ROUTE_COMPILER_HPP + +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/segment.hpp" + +namespace micras::nav { +/** + * @brief Translation of a route on the lattice into the segments that drive it. + * + * @details The moves of a route already are the commands, so this is a direct mapping. The straights + * that a turn carries before and after its curve are merged with the runs around it, which leaves + * one straight between every two curves, and each segment gets the pose it nominally starts from, + * computed from the nodes of the lattice so that no error accumulates along the route. + */ +class RouteCompiler { +public: + /** + * @brief Shortest straight worth a segment of its own, in meters. + */ + static constexpr float min_straight{1.0e-4F}; + + /** + * @brief Deleted constructor for static class. + */ + RouteCompiler() = delete; + + /** + * @brief Compile a route. + * + * @note The speeds of the segments are left for the velocity planner to fill in. + * + * @param route The route. + * @param dynamics The physical limits of the robot, which hold the shape of the turns. + * @param profile The profile of the run. + * @param start_distance The distance from where the robot starts to the first node of the route. + * @param segments The segments of the route, whose storage is reused. + */ + static void compile( + const Route& route, const Dynamics& dynamics, const RunProfile& profile, float start_distance, + std::vector& segments + ); +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_ROUTE_COMPILER_HPP diff --git a/micras_nav/include/micras/nav/speed_controller.hpp b/micras_nav/include/micras/nav/speed_controller.hpp deleted file mode 100644 index 46f904a..0000000 --- a/micras_nav/include/micras/nav/speed_controller.hpp +++ /dev/null @@ -1,111 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_SPEED_CONTROLLER_HPP -#define MICRAS_NAV_SPEED_CONTROLLER_HPP - -#include - -#include "micras/core/pid_controller.hpp" -#include "micras/nav/state.hpp" - -namespace micras::nav { -/** - * @brief Class to compute a command to follow a pair of linear and angular speeds. - */ -class SpeedController { -public: - /** - * @brief Configuration struct for the SpeedController class. - */ - struct Config { - struct FeedForward { - float linear_speed; - float linear_acceleration; - float angular_speed; - float angular_acceleration; - }; - - core::PidController::Config linear_pid; - core::PidController::Config angular_pid; - FeedForward left_feed_forward{}; - FeedForward right_feed_forward{}; - }; - - /** - * @brief Construct a new SpeedController object. - * - * @param config The configuration for the SpeedController class. - */ - explicit SpeedController(const Config& config); - - /** - * @brief Calculate the command to achieve the desired speeds. - * - * @param current_twist The current speeds of the robot. - * @param desired_twist The desired speeds of the robot. - * @param elapsed_time The time since the last update. - * @return A pair of floats representing the left and right commands. - */ - std::pair - compute_control_commands(const Twist& current_twist, const Twist& desired_twist, float elapsed_time); - - /** - * @brief Calculate the feed-forward command to achieve the desired speeds and accelerations. - * - * @param desired_twist The desired speeds of the robot. - * @param elapsed_time The time since the last update. - * @return A pair of floats representing the left and right feed-forward commands. - */ - std::pair compute_feed_forward_commands(const Twist& desired_twist, float elapsed_time); - - /** - * @brief Reset the PID controllers. - */ - void reset(); - -private: - /** - * @brief Calculate the feed-forward term for a motor. - * - * @param speed The current speeds of the robot. - * @param acceleration The current accelerations of the robot. - * @param config The configuration for the feed-forward term. - * @return The feed-forward parameters for a motor. - */ - static float feed_forward(const Twist& speed, const Twist& acceleration, const Config::FeedForward& config); - - /** - * @brief The last linear speed of the robot. - */ - float last_linear_speed{}; - - /** - * @brief The last angular speed of the robot. - */ - float last_angular_speed{}; - - /** - * @brief PID controller for stopping at the goal. - */ - core::PidController linear_pid; - - /** - * @brief PID controller for the orientation. - */ - core::PidController angular_pid; - - /** - * @brief Feed-forward parameters for the left motor. - */ - Config::FeedForward left_feed_forward; - - /** - * @brief Feed-forward parameters for the right motor. - */ - Config::FeedForward right_feed_forward; -}; -} // namespace micras::nav - -#endif // MICRAS_NAV_SPEED_CONTROLLER_HPP diff --git a/micras_nav/include/micras/nav/state.hpp b/micras_nav/include/micras/nav/state.hpp index b62c896..685ef33 100644 --- a/micras_nav/include/micras/nav/state.hpp +++ b/micras_nav/include/micras/nav/state.hpp @@ -2,8 +2,8 @@ * @file */ -#ifndef MICRAS_NAV_POSE_HPP -#define MICRAS_NAV_POSE_HPP +#ifndef MICRAS_NAV_STATE_HPP +#define MICRAS_NAV_STATE_HPP #include "micras/core/vector.hpp" #include "micras/nav/grid_pose.hpp" @@ -21,14 +21,6 @@ struct Pose { */ GridPose to_grid(float cell_size) const; - /** - * @brief Convert the pose to a cell reference. - * - * @param cell_size The size of the grid cells. - * @return The point inside the cell reference. - */ - core::Vector to_cell(float cell_size) const; - /** * @brief Express in the frame of this pose a pose given relative to it. * @@ -85,39 +77,6 @@ struct State { */ Twist velocity; }; - -class RelativePose { -public: - /** - * @brief Construct a new Relative Pose object. - * - * @param absolute_pose The absolute pose to be used as a reference. - */ - explicit RelativePose(const Pose& absolute_pose); - - /** - * @brief Get the relative pose. - * - * @return The pose relative to the reference. - */ - Pose get() const; - - /** - * @brief Reset the reference to the current absolute pose. - */ - void reset_reference(); - -private: - /** - * @brief A reference to the absolute pose. - */ - const Pose* absolute_pose; - - /** - * @brief The reference pose to be used for calculations. - */ - Pose reference_pose{}; -}; } // namespace micras::nav -#endif // MICRAS_NAV_POSE_HPP +#endif // MICRAS_NAV_STATE_HPP diff --git a/micras_nav/include/micras/nav/wall_model.hpp b/micras_nav/include/micras/nav/wall_model.hpp new file mode 100644 index 0000000..f99572a --- /dev/null +++ b/micras_nav/include/micras/nav/wall_model.hpp @@ -0,0 +1,208 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_WALL_MODEL_HPP +#define MICRAS_NAV_WALL_MODEL_HPP + +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +/** + * @brief What the optical axis of a wall sensor meets. + * + * @details The wall is the first one along the axis that is not known to be absent, so its state is + * either a wall or unknown. The range is the distance from the sensor to the face of that wall, the + * offset is where along the wall the axis lands, measured from the end of the wall with the lowest + * coordinate, and the cosine is that of the angle between the axis and the perpendicular of the + * wall. The jacobian is the derivative of the range with respect to the x, y and orientation of + * the robot. The next three fields place the wall in the maze frame: whether it runs along y, the + * coordinate of its face across it, and the coordinate along it where the offset is measured from. + * The ends are the offsets between which the face is known to be one continuous surface: a known + * wall continues over its posts and over the known walls in line with it, while an unknown one is + * only considered between its posts. + * + * @note A hit is not valid when the axis meets nothing within range, or meets a post that has no + * known wall flush with it. + */ +struct RayHit { + bool valid; + GridPose wall; + WallState state; + float range; + float offset; + float cosine; + std::array jacobian; + bool vertical; + float face; + float base; + float low_end; + float high_end; +}; + +/** + * @brief Where the optical axis of a sensor crosses the plane of a wall, extended past its ends. + * + * @note The offset is measured along the wall like the one of a RayHit, the range is the distance + * from the sensor to the plane along the axis, and the jacobian is the derivative of the offset with + * respect to the x, y and orientation of the robot. + */ +struct PlaneCrossing { + float offset; + float range; + std::array jacobian; +}; + +/** + * @brief Geometry of the wall sensors against the map of the walls. + * + * @details It answers one question, what should a sensor see from a pose, for the two users that + * need it. The localizer compares the answer with the reading to correct the pose, and the wall + * observer compares it to decide whether a wall that is still unknown is there. Both only trust a + * reading when the whole spot the emitter illuminates, enlarged by how uncertain the pose is, lies + * on the face of a single wall: not on a post, not across the gap at the end of the wall. + */ +class WallModel { +public: + /** + * @brief Configuration struct for the wall model. + * + * @note A wall is only looked for between the two ranges, which are what the sensors measure + * well. The edge margin is kept between the illuminated spot and the ends of a wall, and the + * confidence is how many standard deviations of the pose the spot is enlarged by. + */ + struct Config { + RobotModel model; + float min_range; + float max_range; + float edge_margin; + float confidence; + }; + + /** + * @brief Construct a new Wall Model object. + * + * @param config The configuration for the wall model. + */ + explicit WallModel(const Config& config); + + /** + * @brief Find what the optical axis of a sensor meets. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + * @param pose The pose of the robot in the maze frame. + * @param sensor The index of the sensor. + * @param maze The map of the walls. + * @return The first wall along the axis that is not known to be absent. + */ + template + RayHit cast(const Pose& pose, uint8_t sensor, const TMaze& maze) const; + + /** + * @brief Find where the optical axis of a sensor crosses the plane of a wall it met before. + * + * @param pose The pose of the robot in the maze frame. + * @param sensor The index of the sensor. + * @param hit The hit that identified the wall. + * @return The crossing of the axis with the plane of the face of that wall. + */ + PlaneCrossing cross(const Pose& pose, uint8_t sensor, const RayHit& hit) const; + + /** + * @brief Check if a sensor looks far enough to the side to sweep along the walls beside the robot. + * + * @param sensor The index of the sensor. + * @return True if the sensor can see the ends of the side walls go by. + */ + bool is_side_looking(uint8_t sensor) const; + + /** + * @brief Check if the spot a sensor illuminates lies entirely on the face of the wall it meets. + * + * @param hit What the axis of the sensor meets. + * @param sensor The index of the sensor. + * @param position_deviation The standard deviation of the position of the robot, in meters. + * @param orientation_deviation The standard deviation of the orientation of the robot, in radians. + * @return True if a reading of the sensor can only come from that wall. + */ + bool is_footprint_clear( + const RayHit& hit, uint8_t sensor, float position_deviation, float orientation_deviation + ) const; + + /** + * @brief Get the range a sensor reads with the robot centered in a cell, facing along it. + * + * @note This is the pose the sensors are calibrated in: the side sensors against the walls of + * a corridor and the front ones against a wall ahead. + * + * @param sensor The index of the sensor. + * @return The range to the wall the sensor points at, or zero if it points at none. + */ + float get_centered_range(uint8_t sensor) const; + + /** + * @brief Get the standard deviation of a range reading. + * + * @param range The range in meters. + * @return The standard deviation in meters. + */ + float get_range_deviation(float range) const; + +private: + /** + * @brief Sine of the angle of the optical axis from which a sensor counts as looking sideways. + */ + static constexpr float side_looking_sine{0.3F}; + + /** + * @brief Dimensions of the maze. + */ + RobotModel::Maze maze_geometry; + + /** + * @brief Mounting of each sensor. + */ + std::array sensors; + + /** + * @brief Constant part of the range noise. + */ + float range_noise; + + /** + * @brief Part of the range noise proportional to the range. + */ + float range_noise_per_meter; + + /** + * @brief Shortest range a wall is looked for at. + */ + float min_range; + + /** + * @brief Largest range a wall is looked for at. + */ + float max_range; + + /** + * @brief Distance kept between the illuminated spot and the ends of a wall. + */ + float edge_margin; + + /** + * @brief Number of standard deviations of the pose the spot is enlarged by. + */ + float confidence; +}; +} // namespace micras::nav + +#include "micras/nav/impl/wall_model.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_WALL_MODEL_HPP diff --git a/micras_nav/include/micras/nav/wall_observer.hpp b/micras_nav/include/micras/nav/wall_observer.hpp new file mode 100644 index 0000000..e88be75 --- /dev/null +++ b/micras_nav/include/micras/nav/wall_observer.hpp @@ -0,0 +1,108 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_WALL_OBSERVER_HPP +#define MICRAS_NAV_WALL_OBSERVER_HPP + +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/localizer.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/wall_model.hpp" + +namespace micras::nav { +/** + * @brief Decision of which walls exist, from the readings of the wall sensors. + * + * @details A reading only says something about a wall while the whole spot the emitter illuminates + * would lie on the face of that wall, allowing for how uncertain the pose is, which the wall model + * answers from the geometry of the sensors. Every reading taken in those conditions is a vote: + * for the wall when something is seen at the range the wall would be at, against it when nothing is + * seen up to beyond that range. A wall is decided when enough votes agree, and it is decided once. + * A wall that never gets enough votes stays unknown rather than being guessed. + * + * @note Every sensor votes on whatever wall it happens to point at, so the wall ahead is decided by + * the sensors that look forward as soon as it comes within their range, with no special case. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TWallObserver { +public: + /** + * @brief Configuration struct for the wall observer. + * + * @note The tolerance on the range is a constant part plus a part proportional to the range. + * A wall can only be voted absent while it would be closer than the detection range, which has + * to be inside of what the sensors can measure. The range delay is the delay of the filter the + * fast ranges went through. + */ + struct Config { + float tolerance; + float relative_tolerance; + float detection_range; + float max_angular_speed; + float range_delay; + int8_t votes_to_decide; + }; + + /** + * @brief Construct a new TWallObserver object. + * + * @param config The configuration for the wall observer. + */ + explicit TWallObserver(const Config& config); + + /** + * @brief Forget every vote. + */ + void reset(); + + /** + * @brief Count the votes of the current readings and record the walls they decide. + * + * @param measurements The current measurements. + * @param localizer The pose estimate and its uncertainty. + * @param wall_model The geometry of the wall sensors. + * @param maze The map of the walls, which is updated. + * @return True if a wall was decided. + */ + bool update( + const Measurements& measurements, const Localizer& localizer, const WallModel& wall_model, + TMaze& maze + ); + +private: + /** + * @brief Number of walls of the maze, including the ones of the border. + */ + static constexpr uint16_t number_of_walls{(width + 1) * height + width * (height + 1)}; + + /** + * @brief Get the index of a wall. + * + * @param wall The cell and the side of it where the wall is. + * @return The index of the wall, which is the same from both of its sides. + */ + static constexpr uint16_t get_index(const GridPose& wall); + + /** + * @brief Parameters of the observer. + */ + Config config; + + /** + * @brief Votes for each wall minus votes against it. + */ + std::array votes{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/wall_observer.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_WALL_OBSERVER_HPP diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp deleted file mode 100644 index 1da9a77..0000000 --- a/micras_nav/src/action_queuer.cpp +++ /dev/null @@ -1,413 +0,0 @@ -/** - * @file - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "micras/nav/action_queuer.hpp" -#include "micras/nav/actions/base.hpp" -#include "micras/nav/actions/move.hpp" -#include "micras/nav/actions/turn.hpp" -#include "micras/nav/grid_pose.hpp" - -namespace micras::nav { -ActionQueuer::ActionQueuer(Config config) : - cell_size{config.cell_size}, - start_offset{config.start_offset}, - curve_safety_margin{config.curve_safety_margin}, - solving_params{config.solving} { - const float exploration_curve_radius = config.exploring.max_linear_speed * config.exploring.max_linear_speed / - config.exploring.max_centrifugal_acceleration; - - const float exploration_max_angular_speed = TurnAction::calculate_max_angular_speed( - std::numbers::pi_v / 2.0F, exploration_curve_radius, config.exploring.max_angular_acceleration, - config.exploring.max_centrifugal_acceleration - ); - - const float exploration_linear_speed = - config.exploring.max_centrifugal_acceleration / exploration_max_angular_speed; - - MoveAction::Config move_config = { - .start_speed = 0.0F, - .end_speed = exploration_linear_speed, - .max_speed = exploration_linear_speed, - .max_acceleration = config.exploring.max_linear_acceleration, - .max_deceleration = config.exploring.max_linear_deceleration - }; - - this->start = std::make_shared( - std::to_underlying(ActionType::START), this->cell_size - config.start_offset, move_config - ); - this->move_half = - std::make_shared(std::to_underlying(ActionType::MOVE_FORWARD), cell_size / 2.0F, move_config); - - move_config.start_speed = exploration_linear_speed; - - this->move_forward = - std::make_shared(std::to_underlying(ActionType::MOVE_FORWARD), cell_size, move_config); - this->move_to_turn = std::make_shared( - std::to_underlying(ActionType::MOVE_FORWARD), this->cell_size / 2.0F - exploration_curve_radius, move_config, - false - ); - this->move_from_turn = std::make_shared( - std::to_underlying(ActionType::MOVE_FORWARD), this->cell_size / 2.0F - exploration_curve_radius, move_config - ); - - move_config.end_speed = 0.0F; - - this->stop = - std::make_shared(std::to_underlying(ActionType::STOP), cell_size / 2.0F, move_config, false); - - const TurnAction::Config turn_config = { - .max_angular_speed = exploration_max_angular_speed, - .linear_speed = exploration_linear_speed, - .max_angular_acceleration = config.exploring.max_angular_acceleration - }; - - this->turn_left = std::make_shared( - std::to_underlying(ActionType::TURN), std::numbers::pi_v / 2.0F, turn_config - ); - this->turn_right = std::make_shared( - std::to_underlying(ActionType::TURN), -std::numbers::pi_v / 2.0F, turn_config - ); - - const TurnAction::Config turn_back_config = { - .max_angular_speed = config.exploring.max_angular_acceleration * 0.01F, - .linear_speed = 0.0F, - .max_angular_acceleration = config.exploring.max_angular_acceleration - }; - - this->turn_back = - std::make_shared(std::to_underlying(ActionType::SPIN), std::numbers::pi_v, turn_back_config); - - this->compute_curve_parameters(std::numbers::pi_v / 4.0F, false); - this->compute_curve_parameters(std::numbers::pi_v / 2.0F, false); - this->compute_curve_parameters(std::numbers::pi_v / 2.0F, true); - this->compute_curve_parameters(3.0F * std::numbers::pi_v / 4.0F, false); - this->compute_curve_parameters(std::numbers::pi_v, false); -} - -void ActionQueuer::push_exploring(const GridPose& origin_pose, const GridPoint& target_position) { - if (origin_pose.front().position == target_position) { - this->action_queue.emplace_back(this->move_forward); - return; - } - - if (origin_pose.turned_left().front().position == target_position) { - this->action_queue.emplace_back(this->move_to_turn); - this->action_queue.emplace_back(this->turn_left); - this->action_queue.emplace_back(this->move_to_turn); - return; - } - - if (origin_pose.turned_right().front().position == target_position) { - this->action_queue.emplace_back(this->move_to_turn); - this->action_queue.emplace_back(this->turn_right); - this->action_queue.emplace_back(this->move_from_turn); - return; - } - - if (origin_pose.turned_back().front().position == target_position) { - this->action_queue.emplace_back(this->stop); - this->action_queue.emplace_back(this->turn_back); - this->action_queue.emplace_back(this->move_half); - return; - } -} - -std::shared_ptr ActionQueuer::pop() { - auto action = this->action_queue.front(); - this->action_queue.pop_front(); - return action; -} - -bool ActionQueuer::empty() const { - return this->action_queue.empty(); -} - -void ActionQueuer::recompute(const std::list& best_route, bool add_start) { - this->action_queue = {}; - - if (add_start) { - this->action_queue.emplace_back(start); - } - - if (best_route.empty()) { - return; - } - - std::list actions; - - Side direction = best_route.front().direction(*std::next(best_route.begin())); - - for (auto route_it = best_route.begin(); std::next(route_it) != best_route.end(); route_it++) { - actions.push_back( - get_action({.position = *route_it, .orientation = direction}, *std::next(route_it), this->cell_size) - ); - direction = (*route_it).direction(*std::next(route_it)); - } - - this->add_diagonals(actions); - this->join_actions(actions); - - actions.front().value -= this->start_offset; - float start_speed = 0.0F; - - for (auto action_it = std::next(actions.begin()); action_it != actions.end(); action_it++) { - if (action_it->type != std::to_underlying(ActionType::TURN)) { - continue; - } - - if (std::prev(action_it)->value < 0.0F) { - std::terminate(); - } - - const CurveParameters& curve_parameters = this->get_curve_parameters( - action_it->value, std::prev(action_it)->type == std::to_underlying(ActionType::DIAGONAL) - ); - - const MoveAction::Config move_config = { - .start_speed = start_speed, - .end_speed = curve_parameters.linear_speed, - .max_speed = this->solving_params.max_linear_speed, - .max_acceleration = this->solving_params.max_linear_acceleration, - .max_deceleration = this->solving_params.max_linear_deceleration - }; - this->action_queue.emplace_back( - std::make_shared( - std::prev(action_it)->type, std::prev(action_it)->value, move_config, - std::prev(action_it)->type != std::to_underlying(ActionType::DIAGONAL) - ) - ); - - const TurnAction::Config turn_config = { - .max_angular_speed = curve_parameters.max_angular_speed, - .linear_speed = curve_parameters.linear_speed, - .max_angular_acceleration = this->solving_params.max_angular_acceleration - }; - this->action_queue.emplace_back(std::make_shared(action_it->type, action_it->value, turn_config)); - - start_speed = curve_parameters.linear_speed; - } - - const MoveAction::Config stop_config = { - .start_speed = start_speed, - .end_speed = 0.0F, - .max_speed = this->solving_params.max_linear_speed, - .max_acceleration = this->solving_params.max_linear_acceleration, - .max_deceleration = this->solving_params.max_linear_deceleration - }; - - if (actions.back().type == std::to_underlying(ActionType::MOVE_FORWARD)) { - this->action_queue.emplace_back( - std::make_shared( - std::to_underlying(ActionType::STOP), actions.back().value + this->cell_size / 2.0F, stop_config - ) - ); - } else { - this->action_queue.emplace_back( - std::make_shared(std::to_underlying(ActionType::STOP), this->cell_size / 2.0F, stop_config) - ); - } -} - -float ActionQueuer::get_total_time() const { - float total_time = 0.0F; - - for (const auto& action : this->action_queue) { - total_time += action->get_total_time(); - } - - return total_time; -} - -Action::Id ActionQueuer::get_action(const GridPose& origin_pose, const GridPoint& target_point, float cell_size) { - if (origin_pose.front().position == target_point) { - const float distance = origin_pose.position.to_vector(cell_size).distance(target_point.to_vector(cell_size)); - return {.type = std::to_underlying(ActionType::MOVE_FORWARD), .value = distance}; - } - - if (origin_pose.turned_left().front().position == target_point) { - return {.type = std::to_underlying(ActionType::TURN), .value = std::numbers::pi_v / 2.0F}; - } - - if (origin_pose.turned_right().front().position == target_point) { - return {.type = std::to_underlying(ActionType::TURN), .value = -std::numbers::pi_v / 2.0F}; - } - - return {.type = std::to_underlying(ActionType::STOP), .value = cell_size / 2.0F}; -} - -void ActionQueuer::add_diagonals(std::list& actions) const { - for (auto action_it = actions.begin(); std::next(action_it) != actions.end(); action_it++) { - uint8_t diagonal_count = 1; - - while (action_it->type == std::to_underlying(ActionType::TURN) and - std::next(action_it)->type == std::to_underlying(ActionType::TURN) and - std::abs(action_it->value) == std::numbers::pi_v / 2.0F and - action_it->value == -std::next(action_it)->value) { - diagonal_count++; - action_it = actions.erase(action_it); - } - - if (diagonal_count == 1) { - continue; - } - - action_it->value /= 2.0F; - const float first_angle = diagonal_count % 2 == 0 ? -action_it->value : action_it->value; - const float diagonal_distance = diagonal_count * this->cell_size * std::numbers::sqrt2_v / 2.0F; - actions.insert( - action_it, {{.type = std::to_underlying(ActionType::TURN), .value = first_angle}, - {.type = std::to_underlying(ActionType::DIAGONAL), .value = diagonal_distance}} - ); - } -} - -void ActionQueuer::join_actions(std::list& actions) { - for (auto action_it = actions.begin(); std::next(action_it) != actions.end();) { - if (action_it->type == std::next(action_it)->type) { - const float value = action_it->value; - action_it = actions.erase(action_it); - action_it->value += value; - continue; - } - - if (action_it->type != std::to_underlying(ActionType::TURN)) { - action_it++; - continue; - } - - auto [trim_before_distance, trim_after_distance] = this->get_trim_distances(*std::prev(action_it), *action_it); - - std::prev(action_it)->value -= trim_before_distance; - std::next(action_it)->value -= trim_after_distance; - action_it++; - } -} - -std::pair - ActionQueuer::get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) { - const float turn_angle = std::abs(turn_action.value); - const CurveParameters& curve_parameters = - this->get_curve_parameters(turn_angle, action_before.type == std::to_underlying(ActionType::DIAGONAL)); - - if (turn_angle == std::numbers::pi_v / 4.0F) { - const float trim_before_distance = curve_parameters.forward_displacement - curve_parameters.side_displacement; - const float trim_after_distance = curve_parameters.side_displacement * std::numbers::sqrt2_v; - - return {trim_before_distance, trim_after_distance}; - } - - if (turn_angle == std::numbers::pi_v / 2.0F) { - float trim_before_distance = curve_parameters.forward_displacement; - float trim_after_distance = curve_parameters.side_displacement; - - if (action_before.type != std::to_underlying(ActionType::DIAGONAL)) { - trim_before_distance -= this->cell_size / 2.0F; - trim_after_distance -= this->cell_size / 2.0F; - } - - return {trim_before_distance, trim_after_distance}; - } - - if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { - float trim_before_distance = curve_parameters.forward_displacement + curve_parameters.side_displacement; - float trim_after_distance = std::numbers::sqrt2_v * curve_parameters.side_displacement; - - if (action_before.type == std::to_underlying(ActionType::DIAGONAL)) { - trim_before_distance -= this->cell_size * std::numbers::sqrt2_v / 2.0F; - trim_after_distance -= this->cell_size; - } else { - trim_before_distance -= this->cell_size; - trim_after_distance -= this->cell_size * std::numbers::sqrt2_v / 2.0F; - } - - return {trim_before_distance, trim_after_distance}; - } - - if (turn_angle == std::numbers::pi_v) { - const float trim_distance = this->cell_size / 2.0F - this->curve_safety_margin; - - return {trim_distance, trim_distance}; - } - - return {0.0F, 0.0F}; -} - -ActionQueuer::CurveParameters& ActionQueuer::get_curve_parameters(float angle, bool is_diagonal) { - const float turn_angle = std::abs(angle); - - if (turn_angle == std::numbers::pi_v / 4.0F) { - return std::get(this->curves_parameters); - } - - if (turn_angle == std::numbers::pi_v / 2.0F) { - return is_diagonal ? std::get(this->curves_parameters) : - std::get(this->curves_parameters); - } - - if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { - return std::get(this->curves_parameters); - } - - return std::get(this->curves_parameters); -} - -void ActionQueuer::compute_curve_parameters(float angle, bool is_diagonal) { - const float curve_radius = calculate_curve_radius(angle, this->cell_size, this->curve_safety_margin, is_diagonal); - const float max_angular_speed = TurnAction::calculate_max_angular_speed( - angle, curve_radius, this->solving_params.max_angular_acceleration, - this->solving_params.max_centrifugal_acceleration - ); - const float curve_linear_speed = this->solving_params.max_centrifugal_acceleration / max_angular_speed; - - this->get_curve_parameters(angle, is_diagonal) = { - .curve_radius = curve_radius, - .max_angular_speed = max_angular_speed, - .linear_speed = curve_linear_speed, - .side_displacement = TurnAction::calculate_side_displacement( - angle, max_angular_speed, curve_linear_speed, this->solving_params.max_angular_acceleration - ), - .forward_displacement = TurnAction::calculate_forward_displacement( - angle, max_angular_speed, curve_linear_speed, this->solving_params.max_angular_acceleration - ), - }; -} - -constexpr float - ActionQueuer::calculate_curve_radius(float angle, float cell_size, float safety_margin, bool is_diagonal) { - const float turn_angle = std::abs(angle); - - if (turn_angle == std::numbers::pi_v / 4.0F) { - return cell_size * (1.0F + std::numbers::sqrt2_v) / 2.0F; - } - - if (turn_angle == std::numbers::pi_v / 2.0F) { - const float curve_radius = (cell_size - 2.0F * safety_margin) / (2.0F * std::numbers::sqrt2_v - 2.0F); - - if (is_diagonal) { - return curve_radius; - } - - return curve_radius + cell_size / 2.0F; - } - - if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { - const float constant = 10.0F * std::numbers::sqrt2_v - 14.0F; - const float root_term = - (std::sqrt(constant * (cell_size * cell_size - 2.0F * cell_size * safety_margin)) + cell_size) / 2.0F; - - return root_term - safety_margin * (std::numbers::sqrt2_v - 1.0F); - } - - return cell_size / 2.0F; -} -} // namespace micras::nav diff --git a/micras_nav/src/costmap.cpp b/micras_nav/src/costmap.cpp deleted file mode 100644 index 0fb9e9b..0000000 --- a/micras_nav/src/costmap.cpp +++ /dev/null @@ -1,152 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_COSTMAP_CPP -#define MICRAS_NAV_COSTMAP_CPP - -#include -#include -#include -#include - -#include "micras/nav/costmap.hpp" -#include "micras/nav/grid_pose.hpp" - -namespace micras::nav { -template -Costmap::Costmap() { - for (uint8_t row = 0; row < height; row++) { - this->cells[row][0].walls[std::to_underlying(Side::LEFT)] = WallState::WALL; - this->cells[row][width - 1].walls[std::to_underlying(Side::RIGHT)] = WallState::WALL; - } - - for (uint8_t col = 0; col < width; col++) { - this->cells[0][col].walls[std::to_underlying(Side::DOWN)] = WallState::WALL; - this->cells[height - 1][col].walls[std::to_underlying(Side::UP)] = WallState::WALL; - } -} - -template -void Costmap::compute(const GridPoint& reference, uint8_t layer) { - std::queue queue; - queue.push(reference); - - while (not queue.empty()) { - GridPoint current_position = queue.front(); - queue.pop(); - - for (const Side side : all_sides) { - GridPoint front_position = current_position + side; - - if (not this->has_wall({current_position, side})) { - int16_t new_cost = this->get_cost(current_position, layer) + 1; - - if (this->get_cost(front_position, layer) > new_cost) { - this->update_cost(front_position, layer, new_cost); - queue.push(front_position); - } - } - } - } -} - -template -void Costmap::recompute(const GridPoint& reference, uint8_t layer) { - std::queue queue; - queue.push(reference); - - while (not queue.empty()) { - GridPoint current_position = queue.front(); - queue.pop(); - int16_t lowest_cost = max_cost; - - for (const Side side : all_sides) { - if (not this->has_wall({current_position, side})) { - lowest_cost = std::min(lowest_cost, this->get_cost(current_position + side, layer)); - } - } - - if (this->get_cost(current_position, layer) == lowest_cost + 1) { - continue; - } - - int16_t old_cost = this->get_cost(current_position, layer); - this->update_cost(current_position, layer, lowest_cost + 1); - - for (const Side side : all_sides) { - GridPoint front_position = current_position + side; - - if (not this->has_wall({current_position, side}) and - this->get_cost(front_position, layer) == old_cost + 1) { - queue.push(front_position); - } - } - } -} - -template -const Costmap::Cell& Costmap::get_cell(const GridPoint& position) const { - return this->cells.at(position.y).at(position.x); -} - -template -int16_t Costmap::get_cost(const GridPoint& position, uint8_t layer) const { - return this->get_cell(position).costs[layer]; -} - -template -void Costmap::update_cost(const GridPoint& position, uint8_t layer, int16_t cost) { - this->cell_on_position(position).costs.at(layer) = cost; -} - -template -bool Costmap::has_wall(const GridPose& pose, bool consider_virtual) const { - return this->get_cell(pose.position).walls[std::to_underlying(pose.orientation)] == WallState::WALL or - (consider_virtual and - this->get_cell(pose.position).walls[std::to_underlying(pose.orientation)] == WallState::VIRTUAL); -} - -template -bool Costmap::update_wall(const GridPose& pose, bool wall) { - if (this->has_wall(pose, true)) { - return false; - } - - const bool updated = wall; - this->cell_on_position(pose.position).walls[std::to_underlying(pose.orientation)] = - wall ? WallState::WALL : WallState::NO_WALL; - - GridPose front_pose = pose.front(); - - if (front_pose.position.x >= width or front_pose.position.y >= height) { - return updated; - } - - this->cell_on_position(front_pose.position).walls[std::to_underlying(pose.turned_back().orientation)] = - wall ? WallState::WALL : WallState::NO_WALL; - - return updated; -} - -template -void Costmap::add_virtual_wall(const GridPose& pose) { - this->cell_on_position(pose.position).walls[std::to_underlying(pose.orientation)] = WallState::VIRTUAL; - - GridPose front_pose = pose.front(); - - if (front_pose.position.x >= width or front_pose.position.y >= height) { - return; - } - - this->cell_on_position(front_pose.position).walls[std::to_underlying(pose.turned_back().orientation)] = - WallState::VIRTUAL; -} - -template -Costmap::Cell& Costmap::cell_on_position(const GridPoint& position) { - return this->cells.at(position.y).at(position.x); -} -} // namespace micras::nav - -#endif // MICRAS_NAV_COSTMAP_CPP diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp deleted file mode 100644 index 8b507ff..0000000 --- a/micras_nav/src/follow_wall.cpp +++ /dev/null @@ -1,152 +0,0 @@ -/** - * @file - */ - -#include "micras/core/types.hpp" -#include "micras/core/utils.hpp" -#include "micras/nav/follow_wall.hpp" -#include "micras/nav/grid_pose.hpp" -#include "micras/nav/state.hpp" -#include "micras/proxy/wall_sensors.hpp" - -namespace micras::nav { -FollowWall::FollowWall(const proxy::TWallSensors<4>& wall_sensors, const Config& config) : - wall_sensors{wall_sensors}, - pid{config.pid}, - sensor_index{config.wall_sensor_index}, - max_angular_acceleration{config.max_angular_acceleration}, - cell_size{config.cell_size}, - post_threshold{config.post_threshold}, - post_reference{config.post_reference}, - post_clearance{config.post_clearance} { } - -float FollowWall::compute_angular_correction(float elapsed_time, State& state) { - float cell_advance = state.pose.to_cell(this->cell_size).y; - const GridPose grid_pose = state.pose.to_grid(this->cell_size); - - if (grid_pose == this->last_grid_pose) { - if (this->check_posts(state.pose)) { - clear_position_error(state, this->post_reference - cell_advance); - cell_advance = this->post_reference; - this->pid.reset(); - } - } else { - if (grid_pose != this->last_grid_pose.front()) { - this->reset(); - } - } - - this->last_grid_pose = grid_pose; - this->last_pose = state.pose; - - if (cell_advance <= this->post_reference + this->post_clearance and - cell_advance >= this->post_reference - this->post_clearance / 2.0F) { - return 0.0F; - } - - if (not this->following_left and this->wall_sensors.get_wall(this->sensor_index.left)) { - this->following_left = true; - this->pid.reset(); - } - - if (not this->following_right and this->wall_sensors.get_wall(this->sensor_index.right)) { - this->following_right = true; - this->pid.reset(); - } - - float error{}; - - if (this->following_left and this->following_right) { - error = this->wall_sensors.get_sensor_error(this->sensor_index.left) - - this->wall_sensors.get_sensor_error(this->sensor_index.right); - } else if (this->following_left) { - error = 2.0F * this->wall_sensors.get_sensor_error(this->sensor_index.left); - } else if (this->following_right) { - error = -2.0F * this->wall_sensors.get_sensor_error(this->sensor_index.right); - } else { - return 0.0F; - } - - const float response = state.velocity.linear * this->pid.compute_response(error, elapsed_time); - const float clamped_response = - core::move_towards(this->last_response, response, this->max_angular_acceleration * elapsed_time); - - this->last_response = clamped_response; - return clamped_response; -} - -bool FollowWall::check_posts(const Pose& pose) { - const float delta_distance = (pose.position - this->last_pose.position).magnitude(); - - if (delta_distance <= 0.0F) { - return false; - } - - bool found_posts = false; - - if (this->following_left and - -(this->wall_sensors.get_adc_reading(this->sensor_index.left) - this->last_left_reading) / delta_distance >= - this->post_threshold) { - this->following_left = false; - found_posts = true; - } - - if (this->following_right and - -(this->wall_sensors.get_adc_reading(this->sensor_index.right) - this->last_right_reading) / delta_distance >= - this->post_threshold) { - this->following_right = false; - found_posts = true; - } - - this->last_left_reading = this->wall_sensors.get_adc_reading(this->sensor_index.left); - this->last_right_reading = this->wall_sensors.get_adc_reading(this->sensor_index.right); - - return found_posts; -} - -core::Observation FollowWall::get_observation() const { - const bool front_wall = this->wall_sensors.get_wall(this->sensor_index.left_front) and - this->wall_sensors.get_wall(this->sensor_index.right_front); - const bool disturbed = front_wall; - - return { - .left = this->wall_sensors.get_wall(this->sensor_index.left, disturbed), - .front = front_wall, - .right = this->wall_sensors.get_wall(this->sensor_index.right, disturbed), - }; -} - -void FollowWall::clear_position_error(State& state, float error) { - switch (angle_to_grid(state.pose.orientation)) { - case Side::RIGHT: - state.pose.position.x += error; - break; - case Side::UP: - state.pose.position.y += error; - break; - case Side::LEFT: - state.pose.position.x -= error; - break; - case Side::DOWN: - state.pose.position.y -= error; - break; - } -} - -void FollowWall::reset() { - this->pid.reset(); - this->last_response = 0.0F; - this->following_left = false; - this->following_right = false; - this->last_left_reading = 0.0F; - this->last_right_reading = 0.0F; -} - -bool FollowWall::get_is_following_left() const { - return this->following_left; -} - -bool FollowWall::get_is_following_right() const { - return this->following_right; -} -} // namespace micras::nav diff --git a/micras_nav/src/localizer.cpp b/micras_nav/src/localizer.cpp new file mode 100644 index 0000000..7274952 --- /dev/null +++ b/micras_nav/src/localizer.cpp @@ -0,0 +1,345 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/core/utils.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/localizer.hpp" +#include "micras/nav/measurements.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +Localizer::Localizer(const Config& config) : config{config} { + this->config.speed_window = std::clamp(config.speed_window, 1, max_speed_window); + + this->set_covariance({ + config.initial_position_deviation * config.initial_position_deviation, + config.initial_position_deviation * config.initial_position_deviation, + config.initial_orientation_deviation * config.initial_orientation_deviation, + config.initial_bias_deviation * config.initial_bias_deviation, + }); +} + +void Localizer::reset(const Pose& pose, const Measurements& measurements) { + const float bias_variance = this->get_variance(bias_index); + + this->state = {.pose = pose, .velocity = {}}; + this->last_left_angle = measurements.left_wheel_angle; + this->last_right_angle = measurements.right_wheel_angle; + + this->distances.fill(0.0F); + this->durations.fill(0.0F); + this->window_distance = 0.0F; + this->window_duration = 0.0F; + this->status = {}; + + this->set_covariance({ + this->config.initial_position_deviation * this->config.initial_position_deviation, + this->config.initial_position_deviation * this->config.initial_position_deviation, + this->config.initial_orientation_deviation * this->config.initial_orientation_deviation, + bias_variance, + }); +} + +void Localizer::predict(const Measurements& measurements, float elapsed_time) { + const float radius = this->config.model.chassis.wheel_radius; + + const float left_distance = radius * (measurements.left_wheel_angle - this->last_left_angle); + const float right_distance = radius * (measurements.right_wheel_angle - this->last_right_angle); + + this->last_left_angle = measurements.left_wheel_angle; + this->last_right_angle = measurements.right_wheel_angle; + + const float distance = (left_distance + right_distance) / 2.0F; + const float rate = this->config.model.gyroscope_scale * measurements.angular_rate - this->bias; + const float rotation = rate * elapsed_time; + + const float half_rotation = rotation / 2.0F; + const float squared = half_rotation * half_rotation; + const float chord = distance * (1.0F - squared / 6.0F + squared * squared / 120.0F); + + const float heading = this->state.pose.orientation + half_rotation; + const float cosine = std::cos(heading); + const float sine = std::sin(heading); + + this->state.pose.position.x += chord * cosine; + this->state.pose.position.y += chord * sine; + this->state.pose.orientation = core::assert_angle(this->state.pose.orientation + rotation); + + this->window_distance += distance - this->distances.at(this->window_index); + this->window_duration += elapsed_time - this->durations.at(this->window_index); + this->distances.at(this->window_index) = distance; + this->durations.at(this->window_index) = elapsed_time; + this->window_index = static_cast((this->window_index + 1) % this->config.speed_window); + + this->state.velocity = { + .linear = this->window_duration > 0.0F ? this->window_distance / this->window_duration : 0.0F, + .angular = rate, + }; + + Matrix transition{}; + + for (uint8_t i = 0; i < number_of_states; i++) { + transition.at(i).at(i) = 1.0F; + } + + transition.at(x_index).at(orientation_index) = -chord * sine; + transition.at(y_index).at(orientation_index) = chord * cosine; + transition.at(x_index).at(bias_index) = chord * sine * elapsed_time / 2.0F; + transition.at(y_index).at(bias_index) = -chord * cosine * elapsed_time / 2.0F; + transition.at(orientation_index).at(bias_index) = -elapsed_time; + + Matrix noise_input{}; + noise_input.at(x_index) = {cosine, -sine, 0.0F, 0.0F}; + noise_input.at(y_index) = {sine, cosine, 0.0F, 0.0F}; + noise_input.at(orientation_index) = {0.0F, 0.0F, 1.0F, 0.0F}; + noise_input.at(bias_index) = {0.0F, 0.0F, 0.0F, 1.0F}; + + const RobotModel::Noise& noise = this->config.model.noise; + + const float traveled = std::abs(distance); + const float longitudinal = + noise.longitudinal_slip + noise.longitudinal_slip_per_acceleration * std::abs(measurements.acceleration.x); + + this->time_since_imu += elapsed_time; + float orientation_variance = 0.0F; + + if (measurements.imu_is_new) { + orientation_variance = noise.gyroscope * noise.gyroscope * this->time_since_imu; + this->time_since_imu = 0.0F; + } + + this->propagate( + transition, noise_input, + { + longitudinal * longitudinal * traveled, + noise.lateral_slip * noise.lateral_slip * traveled, + orientation_variance, + noise.gyroscope_bias_walk * noise.gyroscope_bias_walk * elapsed_time, + } + ); +} + +void Localizer::correct_at_rest(const Measurements& measurements, float elapsed_time) { + const float wheel_difference = measurements.right_wheel_angle - measurements.left_wheel_angle; + + if (std::abs(this->state.velocity.linear) > this->config.stationary_linear_speed) { + this->rest_time = 0.0F; + return; + } + + if (this->rest_time <= 0.0F) { + this->rest_rotation = 0.0F; + this->rest_wheel_difference = wheel_difference; + } + + this->rest_time += elapsed_time; + this->rest_rotation += this->config.model.gyroscope_scale * measurements.angular_rate * elapsed_time; + + if (this->rest_time < this->config.rest_window) { + return; + } + + const RobotModel& model = this->config.model; + + const float wheel_to_rotation = model.chassis.wheel_radius / model.chassis.track_width; + const float wheel_rotation = wheel_to_rotation * (wheel_difference - this->rest_wheel_difference); + const float measured_bias = (this->rest_rotation - wheel_rotation) / this->rest_time; + + const float gyroscope_deviation = model.noise.gyroscope / std::sqrt(this->rest_time); + const float wheel_deviation = 2.0F * wheel_to_rotation * model.noise.wheel_angle / this->rest_time; + + this->update( + measured_bias - this->bias, {0.0F, 0.0F, 0.0F, 1.0F}, + gyroscope_deviation * gyroscope_deviation + wheel_deviation * wheel_deviation, this->config.stationary_gate, + this->config.max_position_correction + ); + + this->rest_time = 0.0F; +} + +const State& Localizer::get_state() const { + return this->state; +} + +const Pose& Localizer::get_pose() const { + return this->state.pose; +} + +GridPose Localizer::get_cell() const { + return this->state.pose.to_grid(this->config.model.maze.cell_size); +} + +float Localizer::get_gyroscope_bias() const { + return this->bias; +} + +float Localizer::get_position_deviation() const { + return std::sqrt(std::max(this->get_variance(x_index), this->get_variance(y_index))); +} + +float Localizer::get_orientation_deviation() const { + return std::sqrt(this->get_variance(orientation_index)); +} + +const Localizer::Status& Localizer::get_status() const { + return this->status; +} + +void Localizer::set_covariance(const Vector& variances) { + this->upper = {}; + + for (uint8_t i = 0; i < number_of_states; i++) { + this->upper.at(i).at(i) = 1.0F; + } + + this->diagonal = variances; +} + +void Localizer::propagate(const Matrix& transition, const Matrix& noise_input, const Vector& noise_variances) { + static constexpr uint8_t columns{2 * number_of_states}; + + std::array, number_of_states> work{}; + std::array weights{}; + + for (uint8_t i = 0; i < number_of_states; i++) { + for (uint8_t j = 0; j < number_of_states; j++) { + float sum = 0.0F; + + for (uint8_t k = 0; k <= j; k++) { + sum += transition.at(i).at(k) * this->upper.at(k).at(j); + } + + work.at(i).at(j) = sum; + work.at(i).at(number_of_states + j) = noise_input.at(i).at(j); + } + + weights.at(i) = this->diagonal.at(i); + weights.at(number_of_states + i) = noise_variances.at(i); + } + + for (uint8_t j = number_of_states; j-- > 0;) { + float sum = 0.0F; + + for (uint8_t k = 0; k < columns; k++) { + sum += work.at(j).at(k) * work.at(j).at(k) * weights.at(k); + } + + this->diagonal.at(j) = sum; + + for (uint8_t i = 0; i < j; i++) { + float cross = 0.0F; + + for (uint8_t k = 0; k < columns; k++) { + cross += work.at(i).at(k) * work.at(j).at(k) * weights.at(k); + } + + const float factor = sum > 0.0F ? cross / sum : 0.0F; + this->upper.at(i).at(j) = factor; + + for (uint8_t k = 0; k < columns; k++) { + work.at(i).at(k) -= factor * work.at(j).at(k); + } + } + } +} + +bool Localizer::update( + float innovation, const Vector& jacobian, float variance, float gate, float max_position_correction +) { + Vector projected{}; + Vector scaled{}; + + float innovation_variance = variance; + + for (uint8_t j = 0; j < number_of_states; j++) { + float sum = jacobian.at(j); + + for (uint8_t i = 0; i < j; i++) { + sum += this->upper.at(i).at(j) * jacobian.at(i); + } + + projected.at(j) = sum; + scaled.at(j) = this->diagonal.at(j) * sum; + innovation_variance += sum * scaled.at(j); + } + + const float normalized = innovation * innovation / innovation_variance; + + if (normalized > gate) { + this->status.rejected++; + return false; + } + + this->status.accepted++; + this->status.innovation_level += 0.01F * (normalized - this->status.innovation_level); + + Vector cross{}; + + for (uint8_t i = 0; i < number_of_states; i++) { + float sum = scaled.at(i); + + for (uint8_t j = i + 1; j < number_of_states; j++) { + sum += this->upper.at(i).at(j) * scaled.at(j); + } + + cross.at(i) = sum; + } + + const float position_step = std::hypot(cross.at(x_index), cross.at(y_index)) * std::abs(innovation); + const float orientation_step = std::abs(cross.at(orientation_index) * innovation); + + const float required = std::max( + {innovation_variance, position_step / max_position_correction, + orientation_step / this->config.max_orientation_correction} + ); + + float alpha = variance + (required - innovation_variance); + Vector gain{}; + + for (uint8_t j = 0; j < number_of_states; j++) { + const float beta = alpha; + alpha += projected.at(j) * scaled.at(j); + + const float lambda = -projected.at(j) / beta; + this->diagonal.at(j) *= beta / alpha; + gain.at(j) = scaled.at(j); + + for (uint8_t i = 0; i < j; i++) { + const float previous = this->upper.at(i).at(j); + this->upper.at(i).at(j) = previous + gain.at(i) * lambda; + gain.at(i) += scaled.at(j) * previous; + } + } + + const float scale = innovation / alpha; + + this->state.pose.position.x += gain.at(x_index) * scale; + this->state.pose.position.y += gain.at(y_index) * scale; + this->state.pose.orientation = + core::assert_angle(this->state.pose.orientation + gain.at(orientation_index) * scale); + this->bias += gain.at(bias_index) * scale; + + return true; +} + +float Localizer::get_variance(uint8_t index) const { + float variance = this->diagonal.at(index); + + for (uint8_t j = index + 1; j < number_of_states; j++) { + variance += this->upper.at(index).at(j) * this->upper.at(index).at(j) * this->diagonal.at(j); + } + + return variance; +} + +bool Localizer::is_stationary() const { + return std::abs(this->state.velocity.linear) < this->config.stationary_linear_speed and + std::abs(this->state.velocity.angular) < this->config.stationary_angular_speed; +} +} // namespace micras::nav diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp deleted file mode 100644 index 2271979..0000000 --- a/micras_nav/src/maze.cpp +++ /dev/null @@ -1,365 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_NAV_MAZE_CPP -#define MICRAS_NAV_MAZE_CPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "micras/core/types.hpp" -#include "micras/nav/costmap.hpp" -#include "micras/nav/grid_pose.hpp" -#include "micras/nav/maze.hpp" - -namespace micras::nav { -template -TMaze::TMaze(const Config& config) : - action_queuer(config.action_queuer_config), - start{config.start}, - goal{config.goal}, - cost_margin{config.cost_margin} { - this->costmap.update_wall(this->start, false); - this->costmap.update_wall(start.turned_right(), true); - - // Hardcoded walls at the end region - if constexpr (width == 16 and height == 16) { - this->costmap.update_wall({{7, 7}, Side::UP}, false); - this->costmap.update_wall({{8, 7}, Side::UP}, false); - this->costmap.update_wall({{8, 7}, Side::LEFT}, false); - this->costmap.update_wall({{8, 8}, Side::LEFT}, false); - - // this->update_wall({{7, 7}, Side::DOWN}, true); - // this->update_wall({{8, 7}, Side::DOWN}, true); - // this->update_wall({{8, 7}, Side::RIGHT}, true); - // this->update_wall({{8, 8}, Side::RIGHT}, true); - // this->update_wall({{8, 8}, Side::UP}, true); - // this->update_wall({{7, 8}, Side::UP}, true); - // this->update_wall({{7, 8}, Side::LEFT}, true); - // this->update_wall({{7, 7}, Side::LEFT}, true); - } - - for (const auto& position : this->goal) { - this->costmap.update_cost(position, std::to_underlying(Layer::EXPLORE), 0); - } - - for (const auto& position : this->goal) { - this->costmap.compute(position, std::to_underlying(Layer::EXPLORE)); - } - - this->costmap.update_cost(this->start.position, std::to_underlying(Layer::RETURN), 0); - this->costmap.compute(this->start.position, std::to_underlying(Layer::RETURN)); -} - -template -void TMaze::update_walls(const GridPose& pose, const core::Observation& observation) { - const bool new_left_wall = this->costmap.update_wall(pose.turned_left(), observation.left); - const bool new_front_wall = this->costmap.update_wall(pose, observation.front); - const bool new_right_wall = this->costmap.update_wall(pose.turned_right(), observation.right); - - if (new_left_wall) { - this->update_cell(pose.turned_left().front().position); - } - - if (new_front_wall) { - this->update_cell(pose.front().position); - } - - if (new_right_wall) { - this->update_cell(pose.turned_right().front().position); - } - - if (new_left_wall or new_front_wall or new_right_wall) { - this->update_cell(pose.position); - } -} - -template -GridPose TMaze::get_next_goal(const GridPose& pose, bool returning) { - if (returning and not this->finished_discovery) { - this->compute_minimum_cost(); - const auto& [next_goal, _] = this->get_next_bfs_goal(pose, true); - - if (next_goal != this->start) { - return next_goal; - } - - this->finished_discovery = true; - } - - int16_t current_cost = max_cost; - GridPose next_pose = {}; - - auto sides_order = { - pose.turned_right().orientation, - pose.turned_left().orientation, - pose.orientation, - pose.turned_back().orientation, - }; - - for (Side side : sides_order) { - if (this->costmap.has_wall({pose.position, side})) { - continue; - } - - GridPoint front_position = pose.position + side; - const int16_t flip_cost = pose.turned_back().orientation == side ? 1 : 0; - const int16_t front_cost = - this->costmap.get_cost( - front_position, returning ? std::to_underlying(Layer::RETURN) : std::to_underlying(Layer::EXPLORE) - ) + - flip_cost; - - if (front_cost < current_cost) { - current_cost = front_cost; - next_pose.position = front_position; - next_pose.orientation = side; - } - } - - return next_pose; -} - -template -bool TMaze::finished(const GridPoint& position, bool returning) const { - return returning ? this->start.position == position : this->goal.contains(position); -} - -template -void TMaze::compute_minimum_cost() { - this->minimum_cost = static_cast(this->get_next_bfs_goal(this->start, false).second); -} - -template -void TMaze::compute_best_route() { - std::unordered_set visited; - std::list current_route; - - visited.insert(this->start.position); - current_route.push_back(this->start.position); - - this->recursive_backtracking(this->start.position, current_route, visited); -} - -template -void TMaze::recursive_backtracking( - const GridPoint& position, std::list& route, std::unordered_set& visited -) { - if (this->goal.contains(position)) { - float current_route_time = get_route_time(route); - - if (current_route_time < this->best_route_time) { - this->best_route_time = current_route_time; - this->best_route = route; - } - - return; - } - - if (not this->best_route.empty() and - route.size() + this->heuristic(position) >= this->cost_margin * this->best_route.size()) { - return; - } - - for (const Side side : all_sides) { - if (this->costmap.has_wall({position, side}, true)) { - continue; - } - - GridPoint next_position = position + side; - - if (not TMaze::was_visited(this->costmap.get_cell(next_position)) or - visited.contains(next_position)) { - continue; - } - - visited.insert(next_position); - route.push_back(next_position); - this->recursive_backtracking(next_position, route, visited); - visited.erase(next_position); - route.pop_back(); - } -} - -template -uint16_t TMaze::heuristic(const GridPoint& position) const { - uint16_t minimum_distance = std::numeric_limits::max(); - - for (const auto& goal_position : this->goal) { - const uint16_t distance = std::abs(position.x - goal_position.x) + std::abs(position.y - goal_position.y); - - minimum_distance = std::min(distance, minimum_distance); - } - - return minimum_distance; -} - -template -const std::list& TMaze::get_best_route() const { - return this->best_route; -} - -template -std::vector TMaze::serialize() const { - std::vector buffer; - buffer.reserve(2 * this->best_route.size()); - - for (const auto& grid_point : this->best_route) { - buffer.emplace_back(grid_point.x); - buffer.emplace_back(grid_point.y); - } - - return buffer; -} - -template -void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { - this->best_route.clear(); - - for (uint32_t i = 0; i < size; i += 2) { - this->best_route.emplace_back( - // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) - GridPoint{.x = buffer[i], .y = buffer[i + 1]} - ); - } -} - -template -void TMaze::update_cell(const GridPoint& position) { - if (not this->goal.contains(position)) { - this->costmap.recompute(position, std::to_underlying(Layer::EXPLORE)); - } - - if (position != this->start.position) { - this->costmap.recompute(position, std::to_underlying(Layer::RETURN)); - } - - GridPoint dead_end_position = position; - - while (TMaze::is_dead_end(this->costmap.get_cell(dead_end_position))) { - for (Side side : {Side::UP, Side::DOWN, Side::LEFT, Side::RIGHT}) { - if (not this->costmap.has_wall({dead_end_position, side}, true)) { - this->costmap.add_virtual_wall({dead_end_position, side}); - dead_end_position = dead_end_position + side; - break; - } - } - } -} - -template -std::pair TMaze::get_next_bfs_goal(const GridPose& pose, bool discover) const { - std::queue queue; - std::array, height> distance{}; - std::pair result_pair = {this->start, 0}; - - distance.at(pose.position.y).at(pose.position.x) = 1; - - auto sides_order = { - pose.turned_right().orientation, - pose.turned_left().orientation, - pose.orientation, - pose.turned_back().orientation, - }; - - for (Side side : sides_order) { - if (not this->costmap.has_wall({pose.position, side})) { - GridPoint next_position = pose.position + side; - queue.emplace(next_position, side); - distance.at(next_position.y).at(next_position.x) = 1; - } - } - - while (not queue.empty()) { - GridPose current_pose = queue.front(); - queue.pop(); - - if ((discover and TMaze::must_visit( - this->costmap.get_cell(current_pose.position), - static_cast(std::round(this->minimum_cost * this->cost_margin)) - )) or - (not discover and this->goal.contains(current_pose.position))) { - result_pair = { - {pose.position + current_pose.orientation, current_pose.orientation}, - distance.at(current_pose.position.y).at(current_pose.position.x) - }; - break; - } - - for (const Side side : all_sides) { - if (this->costmap.has_wall({current_pose.position, side})) { - continue; - } - - const GridPoint front_position = current_pose.position + side; - - if ((discover or TMaze::was_visited(this->costmap.get_cell(front_position))) and - distance.at(front_position.y).at(front_position.x) == 0) { - queue.emplace(front_position, current_pose.orientation); - distance.at(front_position.y).at(front_position.x) = - distance.at(current_pose.position.y).at(current_pose.position.x) + 1; - } - } - } - - return result_pair; -} - -template -float TMaze::get_route_time(const std::list& route) { - this->action_queuer.recompute(route); - return this->action_queuer.get_total_time(); -} - -template -bool TMaze::is_dead_end( - const Costmap::Cell& cell -) { - uint8_t wall_count = 0; - - for (const auto& wall : cell.walls) { - if (wall == Costmap::WallState::WALL or - wall == Costmap::WallState::VIRTUAL) { - wall_count++; - } - } - - return (wall_count == 3); -} - -template -bool TMaze::was_visited( - const Costmap::Cell& cell -) { - return not( - cell.walls[std::to_underlying(Side::UP)] == - Costmap::WallState::UNKNOWN or - cell.walls[std::to_underlying(Side::DOWN)] == - Costmap::WallState::UNKNOWN or - cell.walls[std::to_underlying(Side::LEFT)] == - Costmap::WallState::UNKNOWN or - cell.walls[std::to_underlying(Side::RIGHT)] == - Costmap::WallState::UNKNOWN - ); -} - -template -bool TMaze::must_visit( - const Costmap::Cell& cell, int16_t cost_threshold -) { - return not was_visited(cell) and - (cell.costs[std::to_underlying(Layer::EXPLORE)] + cell.costs[std::to_underlying(Layer::RETURN)] <= - cost_threshold); -} -} // namespace micras::nav - -#endif // MICRAS_NAV_MAZE_CPP diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp deleted file mode 100644 index 86ab370..0000000 --- a/micras_nav/src/odometry.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/** - * @file - */ - -#include - -#include "micras/nav/odometry.hpp" -#include "micras/proxy/imu.hpp" -#include "micras/proxy/rotary_sensor.hpp" - -namespace micras::nav { -Odometry::Odometry( - const proxy::RotarySensor& left_rotary_sensor, const proxy::RotarySensor& right_rotary_sensor, - const proxy::Imu& imu, Config config -) : - left_rotary_sensor{left_rotary_sensor}, - right_rotary_sensor{right_rotary_sensor}, - imu{imu}, - wheel_radius{config.wheel_radius}, - initial_pose(config.initial_pose), - left_last_position{left_rotary_sensor.get_position()}, - right_last_position{right_rotary_sensor.get_position()}, - linear_filter{config.linear_filter}, - state{.pose = config.initial_pose, .velocity = {.linear = 0.0F, .angular = 0.0F}} { } - -void Odometry::update(float elapsed_time) { - const float left_position = this->left_rotary_sensor.get_position(); - const float right_position = this->right_rotary_sensor.get_position(); - - const float left_distance = this->wheel_radius * (left_position - this->left_last_position); - const float right_distance = this->wheel_radius * (right_position - this->right_last_position); - - this->left_last_position = left_position; - this->right_last_position = right_position; - - const float linear_distance = (left_distance + right_distance) / 2; - - this->state.velocity.linear = this->linear_filter.update(linear_distance / elapsed_time); - this->state.velocity.angular = this->imu.get_angular_velocity(proxy::Imu::Axis::Z); - - const float angular_distance = this->state.velocity.angular * elapsed_time; - - const float half_angle = angular_distance / 2; - const float linear_diagonal = - angular_distance < 0.05F ? linear_distance : std::abs(std::sin(half_angle) * linear_distance / half_angle); - - this->state.pose.position.x += linear_diagonal * std::cos(this->state.pose.orientation + half_angle); - this->state.pose.position.y += linear_diagonal * std::sin(this->state.pose.orientation + half_angle); - - this->state.pose.orientation += angular_distance; -} - -void Odometry::reset() { - this->left_last_position = this->left_rotary_sensor.get_position(); - this->right_last_position = this->right_rotary_sensor.get_position(); - this->state = {.pose = this->initial_pose, .velocity = {.linear = 0.0F, .angular = 0.0F}}; -} - -const nav::State& Odometry::get_state() const { - return this->state; -} - -nav::State& Odometry::get_state() { - return this->state; -} - -void Odometry::set_state(const nav::State& new_state) { - this->state = new_state; -} -} // namespace micras::nav diff --git a/micras_nav/src/route_compiler.cpp b/micras_nav/src/route_compiler.cpp new file mode 100644 index 0000000..b21f31a --- /dev/null +++ b/micras_nav/src/route_compiler.cpp @@ -0,0 +1,83 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/route_compiler.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +void RouteCompiler::compile( + const Route& route, const Dynamics& dynamics, const RunProfile& profile, float start_distance, + std::vector& segments +) { + const float cell_size = dynamics.get_model().maze.cell_size; + + const auto retreat = [cell_size](const LatticePose& node, float distance) { + return node.to_pose(cell_size).compose({.position = {.x = -distance, .y = 0.0F}, .orientation = 0.0F}); + }; + + const auto emit_straight = [&segments](const Pose& start, float length, bool diagonal) { + if (length > min_straight) { + segments.push_back({ + .kind = diagonal ? SegmentKind::DIAGONAL : SegmentKind::STRAIGHT, + .turn = TurnId::SS90S, + .length = length, + .start_speed = 0.0F, + .end_speed = 0.0F, + .max_speed = 0.0F, + .start = start, + }); + } + }; + + segments.clear(); + + LatticePose node = route.start; + Pose straight_start = retreat(node, start_distance); + float straight_length = start_distance; + + for (const RouteStep& step : route.steps) { + const float step_length = node.is_diagonal() ? cell_size / std::numbers::sqrt2_v : cell_size; + + for (uint8_t i = 0; i < step.run; i++) { + node = node.advanced(); + } + + straight_length += static_cast(step.run) * step_length; + + if (not step.has_turn) { + continue; + } + + const TurnShape& shape = dynamics.get_turn(profile, step.turn); + + emit_straight(straight_start, straight_length + shape.pre, node.is_diagonal()); + + segments.push_back({ + .kind = SegmentKind::TURN, + .turn = step.turn, + .length = step.side == TurnSide::LEFT ? shape.angle : -shape.angle, + .start_speed = 0.0F, + .end_speed = 0.0F, + .max_speed = 0.0F, + .start = retreat(node, -shape.pre), + }); + + node = get_turn_exit(node, step.turn, step.side); + straight_start = retreat(node, shape.post); + straight_length = shape.post; + } + + emit_straight(straight_start, straight_length + route.stop_distance, node.is_diagonal()); +} +} // namespace micras::nav diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp deleted file mode 100644 index 2428e40..0000000 --- a/micras_nav/src/speed_controller.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/** - * @file - */ - -#include -#include - -#include "micras/nav/speed_controller.hpp" -#include "micras/nav/state.hpp" - -namespace micras::nav { -SpeedController::SpeedController(const Config& config) : - linear_pid(config.linear_pid), - angular_pid(config.angular_pid), - left_feed_forward{config.left_feed_forward}, - right_feed_forward{config.right_feed_forward} { } - -std::pair SpeedController::compute_control_commands( - const Twist& current_twist, const Twist& desired_twist, float elapsed_time -) { - this->linear_pid.set_setpoint(desired_twist.linear); - this->angular_pid.set_setpoint(desired_twist.angular); - - const float linear_command = this->linear_pid.compute_response(current_twist.linear, elapsed_time); - const float angular_command = this->angular_pid.compute_response(current_twist.angular, elapsed_time); - - return {linear_command - angular_command, linear_command + angular_command}; -} - -std::pair SpeedController::compute_feed_forward_commands(const Twist& desired_twist, float elapsed_time) { - const Twist acceleration_twist{ - .linear = (desired_twist.linear - this->last_linear_speed) / elapsed_time, - .angular = (desired_twist.angular - this->last_angular_speed) / elapsed_time, - }; - - this->last_linear_speed = desired_twist.linear; - this->last_angular_speed = desired_twist.angular; - - const float left_feed_forward = feed_forward(desired_twist, acceleration_twist, this->left_feed_forward); - const float right_feed_forward = feed_forward(desired_twist, acceleration_twist, this->right_feed_forward); - - return {left_feed_forward, right_feed_forward}; -} - -float SpeedController::feed_forward(const Twist& speed, const Twist& acceleration, const Config::FeedForward& config) { - return config.linear_speed * speed.linear + config.linear_acceleration * acceleration.linear + - config.angular_speed * speed.angular + config.angular_acceleration * acceleration.angular; -} - -void SpeedController::reset() { - this->linear_pid.reset(); - this->angular_pid.reset(); -} -} // namespace micras::nav diff --git a/micras_nav/src/state.cpp b/micras_nav/src/state.cpp index 2cb22de..36b6fec 100644 --- a/micras_nav/src/state.cpp +++ b/micras_nav/src/state.cpp @@ -5,7 +5,6 @@ #include #include "micras/core/utils.hpp" -#include "micras/core/vector.hpp" #include "micras/nav/grid_pose.hpp" #include "micras/nav/state.hpp" @@ -14,34 +13,6 @@ GridPose Pose::to_grid(float cell_size) const { return { .position = GridPoint::from_vector(this->position, cell_size), .orientation = angle_to_grid(this->orientation) }; -}; - -core::Vector Pose::to_cell(float cell_size) const { - const core::Vector cell_position = this->position % cell_size; - - float cell_advance = 0.0F; - float cell_alignment = 0.0F; - - switch (angle_to_grid(this->orientation)) { - case Side::RIGHT: - cell_advance = cell_position.x; - cell_alignment = cell_size / 2.0F - cell_position.y; - break; - case Side::UP: - cell_advance = cell_position.y; - cell_alignment = cell_position.x - cell_size / 2.0F; - break; - case Side::LEFT: - cell_advance = cell_size - cell_position.x; - cell_alignment = cell_position.y - cell_size / 2.0F; - break; - case Side::DOWN: - cell_advance = cell_size - cell_position.y; - cell_alignment = cell_size / 2.0F - cell_position.x; - break; - } - - return {.x = cell_alignment, .y = cell_advance}; } Pose Pose::compose(const Pose& local) const { @@ -67,17 +38,4 @@ Pose Pose::relative(const Pose& other) const { .orientation = core::assert_angle(other.orientation - this->orientation), }; } - -RelativePose::RelativePose(const Pose& absolute_pose) : absolute_pose{&absolute_pose} { } - -Pose RelativePose::get() const { - return { - .position = this->absolute_pose->position - this->reference_pose.position, - .orientation = this->absolute_pose->orientation - this->reference_pose.orientation - }; -} - -void RelativePose::reset_reference() { - this->reference_pose = *(this->absolute_pose); -} } // namespace micras::nav diff --git a/micras_nav/src/wall_model.cpp b/micras_nav/src/wall_model.cpp new file mode 100644 index 0000000..8c953a9 --- /dev/null +++ b/micras_nav/src/wall_model.cpp @@ -0,0 +1,86 @@ +/** + * @file + */ + +#include +#include + +#include "micras/nav/state.hpp" +#include "micras/nav/wall_model.hpp" + +namespace micras::nav { +WallModel::WallModel(const Config& config) : + maze_geometry{config.model.maze}, + sensors{config.model.wall_sensors}, + range_noise{config.model.noise.wall_range}, + range_noise_per_meter{config.model.noise.wall_range_per_meter}, + min_range{config.min_range}, + max_range{config.max_range}, + edge_margin{config.edge_margin}, + confidence{config.confidence} { } + +PlaneCrossing WallModel::cross(const Pose& pose, uint8_t sensor, const RayHit& hit) const { + const RobotModel::WallSensor& mounting = this->sensors.at(sensor); + + const Pose origin = pose.compose({.position = mounting.position, .orientation = mounting.angle}); + const float direction_x = std::cos(origin.orientation); + const float direction_y = std::sin(origin.orientation); + + const float across = hit.vertical ? direction_x : direction_y; + const float along = hit.vertical ? direction_y : direction_x; + const float slope = along / across; + + const float gap = hit.face - (hit.vertical ? origin.position.x : origin.position.y); + const float start = hit.vertical ? origin.position.y : origin.position.x; + + const float lever_x = origin.position.x - pose.position.x; + const float lever_y = origin.position.y - pose.position.y; + const float swing = gap / (across * across); + + PlaneCrossing crossing{.offset = start + gap * slope - hit.base, .range = gap / across, .jacobian = {}}; + + if (hit.vertical) { + crossing.jacobian = {-slope, 1.0F, lever_x + lever_y * slope + swing}; + } else { + crossing.jacobian = {1.0F, -slope, -lever_y - lever_x * slope - swing}; + } + + return crossing; +} + +bool WallModel::is_side_looking(uint8_t sensor) const { + return std::abs(std::sin(this->sensors.at(sensor).angle)) > side_looking_sine; +} + +bool WallModel::is_footprint_clear( + const RayHit& hit, uint8_t sensor, float position_deviation, float orientation_deviation +) const { + if (not hit.valid) { + return false; + } + + const float spot = hit.range * std::tan(this->sensors.at(sensor).half_angle) / hit.cosine; + const float uncertainty = this->confidence * (position_deviation + hit.range * orientation_deviation / hit.cosine); + + const float reach = spot + uncertainty + this->edge_margin; + + return hit.offset - reach >= hit.low_end and hit.offset + reach <= hit.high_end; +} + +float WallModel::get_centered_range(uint8_t sensor) const { + const RobotModel::WallSensor& mounting = this->sensors.at(sensor); + + const float face = this->maze_geometry.cell_size / 2.0F - this->maze_geometry.wall_thickness / 2.0F; + const float sine = std::sin(mounting.angle); + + if (std::abs(sine) > 0.1F) { + return (face - std::abs(mounting.position.y)) / std::abs(sine); + } + + return (face - mounting.position.x) / std::cos(mounting.angle); +} + +float WallModel::get_range_deviation(float range) const { + return this->range_noise + this->range_noise_per_meter * range; +} +} // namespace micras::nav diff --git a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp index 6ba12e8..c360803 100644 --- a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp +++ b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp @@ -20,15 +20,23 @@ TWallSensors::TWallSensors(const Config& config) : adc{config.adc}, led_pwms{core::make_array(config.led_pwms)}, emitter_duty_cycle{config.emitter_duty_cycle}, - filters{core::make_array(config.filter)}, - base_readings{config.base_readings}, - uncertainty{config.uncertainty}, + fast_filters{core::make_array(config.fast_filter)}, + slow_filters{core::make_array(config.slow_filter)}, + reference_readings{config.reference_readings}, + reference_distances{config.reference_distances}, + noise_floor{config.noise_floor}, + max_distance{config.max_distance}, + max_reading{config.max_reading}, + wall_distance{config.wall_distance}, + wall_hysteresis{config.wall_hysteresis}, + calibration_samples{config.calibration_samples}, initialized{ this->adc.start_dma(this->buffer, this->snapshot) and this->adc.was_initialized() and config.adc.handle->Init.NbrOfConversion == num_of_sensors and std::ranges::all_of(this->led_pwms, [&config](const hal::Pwm& led_pwm) { - return led_pwm.was_initialized() and std::abs(led_pwm.get_frequency() - config.filter.sampling_frequency) < - frequency_tolerance * config.filter.sampling_frequency; + return led_pwm.was_initialized() and + std::abs(led_pwm.get_frequency() - config.fast_filter.sampling_frequency) < + frequency_tolerance * config.fast_filter.sampling_frequency; }) } { this->turn_off(); @@ -51,49 +59,99 @@ void TWallSensors::turn_off() { template void TWallSensors::update() { const uint32_t current_sequence = this->adc.read_snapshot(this->scans); + const bool is_new = current_sequence != this->sequence; - this->fresh = current_sequence != this->sequence; this->sequence = current_sequence; - if (not this->fresh) { - return; - } - for (uint8_t i = 0; i < num_of_sensors; i++) { - this->filters.at(i).update(this->get_adc_reading(i)); + Reading& reading = this->readings.at(i); + reading.is_new = is_new; + + if (not is_new) { + continue; + } + + const float intensity = this->get_intensity(i); + + const float distance = + intensity >= this->noise_floor ? + this->reference_distances.at(i) * + std::sqrt(this->reference_readings.at(i) / std::min(intensity, this->max_reading)) : + this->max_distance; + + reading.valid = distance < this->max_distance; + reading.distance = this->fast_filters.at(i).update(std::min(distance, this->max_distance)); + reading.slow_distance = this->slow_filters.at(i).update(std::min(distance, this->max_distance)); + + if (not reading.valid or reading.slow_distance > this->wall_distance + this->wall_hysteresis) { + this->walls.at(i) = false; + } else if (reading.slow_distance < this->wall_distance) { + this->walls.at(i) = true; + } + + Calibration& calibration = this->calibrations.at(i); + + if (calibration.samples_left == 0) { + continue; + } + + calibration.sum += intensity; + calibration.squared_sum += intensity * intensity; + calibration.samples_left--; + + if (calibration.samples_left == 0) { + const float mean = calibration.sum / static_cast(this->calibration_samples); + const float variance = + calibration.squared_sum / static_cast(this->calibration_samples) - mean * mean; + + calibration.spread = mean > 0.0F ? std::sqrt(std::max(variance, 0.0F)) / mean : 0.0F; + this->reference_readings.at(i) = std::max(mean, this->noise_floor); + } } } template -bool TWallSensors::is_new() const { - return this->fresh; +const typename TWallSensors::Reading& + TWallSensors::get_reading(uint8_t sensor_index) const { + return this->readings.at(sensor_index); } template -bool TWallSensors::get_wall(uint8_t sensor_index, bool disturbed) const { - return this->filters.at(sensor_index).get_last() > - this->base_readings.at(sensor_index) * this->uncertainty * (disturbed ? 1.2F : 1.0F); +bool TWallSensors::get_wall(uint8_t sensor_index) const { + return this->walls.at(sensor_index); } template -float TWallSensors::get_reading(uint8_t sensor_index) const { - return this->filters.at(sensor_index).get_last(); +float TWallSensors::get_intensity(uint8_t sensor_index) const { + return static_cast(std::abs(this->scans.at(sensor_index) - this->scans.at(sensor_index + num_of_sensors))) / + this->adc.get_max_reading(); } template -float TWallSensors::get_adc_reading(uint8_t sensor_index) const { - return static_cast(std::abs(this->scans.at(sensor_index) - this->scans.at(sensor_index + num_of_sensors))) / - this->adc.get_max_reading(); +void TWallSensors::calibrate_sensor(uint8_t sensor_index) { + this->calibrations.at(sensor_index) = { + .sum = 0.0F, + .squared_sum = 0.0F, + .samples_left = this->calibration_samples, + .spread = 0.0F, + }; } template -float TWallSensors::get_sensor_error(uint8_t sensor_index) const { - return this->get_reading(sensor_index) - this->base_readings.at(sensor_index); +bool TWallSensors::is_calibrating() const { + return std::ranges::any_of(this->calibrations, [](const Calibration& calibration) { + return calibration.samples_left > 0; + }); } template -void TWallSensors::calibrate_sensor(uint8_t sensor_index) { - this->base_readings.at(sensor_index) = this->get_reading(sensor_index); +float TWallSensors::get_reference_reading(uint8_t sensor_index) const { + return this->reference_readings.at(sensor_index); +} + +template +float TWallSensors::get_calibration_spread(uint8_t sensor_index) const { + return this->calibrations.at(sensor_index).spread; } template diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 85cf376..34eea1b 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -10,13 +10,16 @@ #include #include -#include "micras/core/butterworth_filter.hpp" #include "micras/hal/spi.hpp" namespace micras::proxy { /** * @brief Class for acquiring IMU data. * + * @note The rates are delivered as the sensor reports them, with their bias still in them. The bias + * of a gyroscope drifts with temperature and time, so it is estimated continuously by whoever + * integrates the rate, not measured once here. + * * @note The samples are read by the DMA into a buffer of this object, which therefore has to live in * memory that the DMA reaches. */ @@ -38,7 +41,6 @@ class Imu { lsm6dsv_xl_full_scale_t accelerometer_scale; lsm6dsv_filt_gy_lp1_bandwidth_t gyroscope_filter; lsm6dsv_filt_xl_lp2_bandwidth_t accelerometer_filter; - core::ButterworthFilter::Config calibration_filter; }; /** @@ -98,11 +100,6 @@ class Imu { */ float get_linear_acceleration(Axis axis) const; - /** - * @brief Define the base reading to be removed from the IMU value. - */ - void calibrate(); - /** * @brief Check if IMU was initialized. * @@ -223,21 +220,11 @@ class Imu { */ float xl_factor; - /** - * @brief Gyroscope Butterworth filter for the calibration. - */ - core::ButterworthFilter calibration_filter; - /** * @brief Flag to check if the last update brought a new angular rate. */ bool fresh{}; - /** - * @brief Flag to check if the IMU was calibrated. - */ - bool calibrated{}; - /** * @brief Flag to check if the IMU was initialized. */ diff --git a/micras_proxy/include/micras/proxy/locomotion.hpp b/micras_proxy/include/micras/proxy/locomotion.hpp index a36cfea..e3fd1cf 100644 --- a/micras_proxy/include/micras/proxy/locomotion.hpp +++ b/micras_proxy/include/micras/proxy/locomotion.hpp @@ -18,11 +18,23 @@ class Locomotion { public: /** * @brief Configuration struct for the locomotion. + * + * @note The reserved rotation is the part of the range, in percent, that the rotation command + * keeps when the two commands do not fit in it together. */ struct Config { Motor::Config left_motor; Motor::Config right_motor; hal::Gpio::Config enable_gpio; + float reserved_rotation; + }; + + /** + * @brief Linear and angular commands, in percent of the motor supply. + */ + struct Command { + float linear; + float angular; }; /** @@ -53,13 +65,17 @@ class Locomotion { /** * @brief Set the linear and angular commands of the robot. * - * @note Both wheels are scaled by the same factor when a command saturates, so the commanded - * ratio, and with it the turn radius, survives the saturation. + * @details When the two commands do not fit in the range of the motors together, it is the + * linear one that gives way: a robot that slows down stays on its path, while one that turns + * less than it was asked to leaves it. The rotation is only guaranteed up to the reserved + * part of the range, so that a rotation command gone wrong cannot stall the robot. * * @param linear Linear command of the robot. * @param angular Angular command of the robot. + * @return The commands that were applied, which differ from the ones asked for when the motors + * saturate. */ - void set_command(float linear, float angular); + Command set_command(float linear, float angular); /** * @brief Stop the motors. @@ -74,6 +90,11 @@ class Locomotion { bool was_initialized() const; private: + /** + * @brief Largest command a motor takes, in percent. + */ + static constexpr float max_command{100.0F}; + /** * @brief Left motor of the robot. */ @@ -88,6 +109,11 @@ class Locomotion { * @brief GPIO handle for the motor driver enable pin. */ hal::Gpio enable_gpio; + + /** + * @brief Part of the range that the rotation command keeps under saturation, in percent. + */ + float reserved_rotation; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 52e2425..8c9e06b 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -15,20 +15,59 @@ namespace micras::proxy { /** * @brief Class for controlling Wall Sensors. + * + * @details Each sensor is an infrared emitter next to a phototransistor, and what comes out of this + * class is the distance to whatever the emitter lights up, along its optical axis, in meters. The + * emitter has a narrow beam that lands entirely on the wall, which scatters it in every direction, + * so the light that comes back falls with the square of the distance. One reading at a known + * distance therefore calibrates a sensor: + * + * distance = reference_distance * sqrt(reference_reading / reading) + * + * @tparam num_of_sensors Number of sensors. */ template class TWallSensors { public: /** * @brief Configuration struct for wall sensors. + * + * @note The reference reading of a sensor is what it reads at its reference distance, and is + * what a calibration replaces. Both filters run at the rate the sensors produce values, not at + * the rate of the control loop, so that is the sampling frequency they have to be given. + * A reading below the noise floor, or one that works out to more than the maximum distance, + * means nothing is within range. A reading above the maximum is saturated, and is reported as + * the distance of the maximum reading. A wall is considered present when the slow distance is + * below the wall distance, and absent again when it goes above it by the hysteresis. */ struct Config { hal::AdcDma::Config adc; std::array led_pwms; float emitter_duty_cycle; - core::ButterworthFilter::Config filter; - std::array base_readings; - float uncertainty; + core::ButterworthFilter::Config fast_filter; + core::ButterworthFilter::Config slow_filter; + std::array reference_readings; + std::array reference_distances; + float noise_floor; + float max_reading; + float max_distance; + float wall_distance; + float wall_hysteresis; + uint16_t calibration_samples; + }; + + /** + * @brief Distance measured by one sensor. + * + * @note The fast distance is for everything that is a position and the slow one for deciding + * whether there is a wall. The reading is valid when the sensor sees anything above its noise, + * and it is new for a single update after the sensor produces a value. + */ + struct Reading { + float distance; + float slow_distance; + bool valid; + bool is_new; }; /** @@ -49,62 +88,78 @@ class TWallSensors { void turn_off(); /** - * @brief Update the wall sensors readings, if the emitters completed a period since the last call. + * @brief Update the wall sensors readings. + * + * @note Nothing is recomputed unless the converter completed a sequence since the last call, + * so this can be called faster than the sensors produce values. */ void update(); /** - * @brief Check whether the last update brought readings that were not seen before. + * @brief Get the distance measured by a sensor. * - * @note The readings come at the rate of the emitter timer, whatever the rate of the caller. - * - * @return True if the readings are new, false otherwise. + * @param sensor_index Index of the sensor. + * @return The reading of the sensor. */ - bool is_new() const; + const Reading& get_reading(uint8_t sensor_index) const; /** * @brief Get the observation from a sensor. * * @param sensor_index Index of the sensor. - * @param disturbed Whether or not there is another wall perpendicular to the one being measured. * @return True if the sensor detects a wall, false otherwise. */ - bool get_wall(uint8_t sensor_index, bool disturbed = false) const; + bool get_wall(uint8_t sensor_index) const; /** - * @brief Get the reading from a sensor. + * @brief Get the light a sensor receives from its emitter, with the ambient light removed. * * @param sensor_index Index of the sensor. - * @return Reading from the sensor. + * @return Reading from the sensor from 0 to 1. */ - float get_reading(uint8_t sensor_index) const; + float get_intensity(uint8_t sensor_index) const; /** - * @brief Get the ADC reading from a sensor. + * @brief Start calibrating a sensor, with the robot placed at the reference distance. + * + * @note The readings are averaged over the configured number of samples, which takes that many + * updates with a new value. * * @param sensor_index Index of the sensor. - * @return ADC reading from the sensor from 0 to 1. */ - float get_adc_reading(uint8_t sensor_index) const; + void calibrate_sensor(uint8_t sensor_index); /** - * @brief Get the deviation of a wall sensor reading from its calibrated baseline. + * @brief Check if a calibration is in progress. + * + * @return True while any sensor is still averaging. + */ + bool is_calibrating() const; + + /** + * @brief Get the reference reading of a sensor, which is the result of its last calibration. * * @param sensor_index Index of the sensor. - * @return The reading error relative to the baseline; positive if above baseline. + * @return The reading at the reference distance, from 0 to 1. */ - float get_sensor_error(uint8_t sensor_index) const; + float get_reference_reading(uint8_t sensor_index) const; /** - * @brief Calibrate a wall sensor base reading. + * @brief Get how much the readings varied during the last calibration of a sensor. + * + * @note A spread that is not small means the robot moved, or something else was wrong, while + * the sensor was being calibrated. + * + * @param sensor_index Index of the sensor. + * @return The standard deviation of the readings, as a fraction of their mean. */ - void calibrate_sensor(uint8_t sensor_index); + float get_calibration_spread(uint8_t sensor_index) const; /** * @brief Check if the ADC was initialized, its scan matches the buffer layout, and the emitters - * run at the rate the filter was designed for. + * run at the rate the filters were designed for. * - * @note The rate of the emitters lives in the peripheral configuration and the one of the filter + * @note The rate of the emitters lives in the peripheral configuration and the one of the filters * in the constants, and nothing else would notice the day only one of them changes. * * @return True if the initialization was successful, false otherwise. @@ -113,10 +168,20 @@ class TWallSensors { private: /** - * @brief Largest relative difference between the rate of the emitters and the one of the filter. + * @brief Largest relative difference between the rate of the emitters and the one of the filters. */ static constexpr float frequency_tolerance{0.01F}; + /** + * @brief Averaging of the readings of one sensor during its calibration. + */ + struct Calibration { + float sum; + float squared_sum; + uint16_t samples_left; + float spread; + }; + /** * @brief ADC DMA handle. */ @@ -168,24 +233,69 @@ class TWallSensors { uint32_t sequence{}; /** - * @brief Flag to check if the last update brought new readings. + * @brief Butterworth filters for the fast distances. + */ + std::array fast_filters; + + /** + * @brief Butterworth filters for the slow distances. + */ + std::array slow_filters; + + /** + * @brief Reading of each sensor at its reference distance. + */ + std::array reference_readings; + + /** + * @brief Distance each sensor was calibrated at. + */ + std::array reference_distances; + + /** + * @brief Reading below which there is only noise. + */ + float noise_floor; + + /** + * @brief Distance beyond which nothing is considered within range. + */ + float max_distance; + + /** + * @brief Reading above which the sensor is saturated. + */ + float max_reading; + + /** + * @brief Slow distance below which a wall is present. + */ + float wall_distance; + + /** + * @brief How far above the wall distance the slow distance has to go for the wall to be absent. + */ + float wall_hysteresis; + + /** + * @brief Number of readings averaged by a calibration. */ - bool fresh{}; + uint16_t calibration_samples; /** - * @brief Butterworth filter for the ADC readings. + * @brief Distance measured by each sensor. */ - std::array filters; + std::array readings{}; /** - * @brief Measured wall values during calibration. + * @brief Whether each sensor detects a wall. */ - std::array base_readings; + std::array walls{}; /** - * @brief Ratio of the base reading to still consider as seeing a wall. + * @brief Calibration of each sensor. */ - float uncertainty; + std::array calibrations{}; /** * @brief Flag to check if the ADC was initialized. diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 5aae772..430045f 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -18,8 +18,7 @@ Imu::Imu(const Config& config) : mdps_to_radps * 4.375F * (1 << (config.gyroscope_scale == LSM6DSV_4000dps ? 5 : static_cast(config.gyroscope_scale))) }, - xl_factor{mg_to_mps2 * (0.061F * (1 << static_cast(config.accelerometer_scale)))}, - calibration_filter{config.calibration_filter} { + xl_factor{mg_to_mps2 * (0.061F * (1 << static_cast(config.accelerometer_scale)))} { this->dev_ctx.read_reg = platform_read; this->dev_ctx.write_reg = platform_write; this->dev_ctx.mdelay = proxy::Stopwatch::sleep_ms; @@ -109,10 +108,6 @@ void Imu::read_response() { } this->fresh = true; - - if (not this->calibrated) { - this->calibration_filter.update(std::get<2>(this->angular_velocity)); - } } bool Imu::is_new() const { @@ -128,7 +123,7 @@ float Imu::get_angular_velocity(Axis axis) const { return std::get<1>(this->angular_velocity); case Axis::Z: - return std::get<2>(this->angular_velocity) - this->calibration_filter.get_last(); + return std::get<2>(this->angular_velocity); default: return 0.0F; @@ -178,10 +173,6 @@ int32_t Imu::platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint return transferred ? 0 : -1; } -void Imu::calibrate() { - this->calibrated = true; -} - bool Imu::was_initialized() const { return this->initialized; } diff --git a/micras_proxy/src/locomotion.cpp b/micras_proxy/src/locomotion.cpp index 3e175af..d1bc3f3 100644 --- a/micras_proxy/src/locomotion.cpp +++ b/micras_proxy/src/locomotion.cpp @@ -9,7 +9,10 @@ namespace micras::proxy { Locomotion::Locomotion(const Config& config) : - left_motor{config.left_motor}, right_motor{config.right_motor}, enable_gpio{config.enable_gpio} { + left_motor{config.left_motor}, + right_motor{config.right_motor}, + enable_gpio{config.enable_gpio}, + reserved_rotation{config.reserved_rotation} { this->stop(); this->disable(); } @@ -27,19 +30,18 @@ void Locomotion::set_wheel_command(float left_command, float right_command) { this->right_motor.set_command(right_command); } -void Locomotion::set_command(float linear, float angular) { - float left_command = linear - angular; - float right_command = linear + angular; +Locomotion::Command Locomotion::set_command(float linear, float angular) { + const float angular_limit = std::max(this->reserved_rotation, max_command - std::abs(linear)); - const float peak = std::max(std::abs(left_command), std::abs(right_command)); + angular = std::clamp(angular, -angular_limit, angular_limit); - if (peak > 100.0F) { - const float scale = 100.0F / peak; - left_command *= scale; - right_command *= scale; - } + const float linear_limit = max_command - std::abs(angular); - this->set_wheel_command(left_command, right_command); + linear = std::clamp(linear, -linear_limit, linear_limit); + + this->set_wheel_command(linear - angular, linear + angular); + + return {.linear = linear, .angular = angular}; } void Locomotion::stop() { From 75d75518989095f3c2e2a2c55d7add74c1db6630 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 03:22:12 -0300 Subject: [PATCH 492/561] =?UTF-8?q?=E2=9C=A8=20Add=20constant=20expression?= =?UTF-8?q?=20math=20and=20the=20filter=20delay=20to=20micras=5Fcore?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The elementary functions of the standard library only become constant expressions in C++26, and only GCC accepts them earlier as an extension. Whatever is computed when the firmware is compiled goes through these instead, so the same sources build with clang and with the compilers of the simulators. The clothoid is the path of a robot whose angular speed ramps at a constant linear speed, which is what a slalom turn is made of, and its series is short enough to be evaluated at run time too. The group delay of the Butterworth filter is what anyone who turns a filtered reading into a position has to account for. --- .../micras/core/butterworth_filter.hpp | 14 ++ micras_core/include/micras/core/math.hpp | 167 ++++++++++++++++++ 2 files changed, 181 insertions(+) create mode 100644 micras_core/include/micras/core/math.hpp diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp index ed17392..53bf19b 100644 --- a/micras_core/include/micras/core/butterworth_filter.hpp +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -7,6 +7,7 @@ #include #include +#include namespace micras::core { /** @@ -63,6 +64,19 @@ class ButterworthFilter { std::array feedback; }; + /** + * @brief Get the delay the filter adds to a signal that changes slowly compared with the cutoff. + * + * @note This is the group delay at low frequency, `sqrt(2) / (2 * pi * cutoff)`. Whoever turns a + * filtered reading into a position has to account for the distance traveled during it. + * + * @param cutoff_frequency Cutoff frequency in Hz. + * @return The delay in seconds. + */ + static constexpr float get_delay(float cutoff_frequency) { + return std::numbers::sqrt2_v / (2.0F * std::numbers::pi_v * cutoff_frequency); + } + /** * @brief Compute the discrete coefficients of the filter. * diff --git a/micras_core/include/micras/core/math.hpp b/micras_core/include/micras/core/math.hpp new file mode 100644 index 0000000..0310d53 --- /dev/null +++ b/micras_core/include/micras/core/math.hpp @@ -0,0 +1,167 @@ +/** + * @file + */ + +#ifndef MICRAS_CORE_MATH_HPP +#define MICRAS_CORE_MATH_HPP + +#include +#include +#include + +/** + * @brief Elementary functions that are constant expressions on every compiler. + * + * @note The functions of the standard library only become constant expressions in C++26, and only + * GCC accepts them earlier as an extension. Everything computed when the firmware is compiled, such + * as the shape of each turn, goes through these instead, so that the same sources build with clang + * and with the compilers of the simulators. They are also usable at run time, where the series are + * short enough to be cheaper than a table. + */ +namespace micras::core::math { +/** + * @brief Absolute value. + * + * @tparam T Floating point type. + * @param value Value. + * @return The absolute value. + */ +template +constexpr T abs(T value) { + return value < T{0} ? -value : value; +} + +/** + * @brief Square root by Newton iterations. + * + * @tparam T Floating point type. + * @param value Non negative value. + * @return The square root of the value, or zero for a non positive value. + */ +template +constexpr T sqrt(T value) { + if (value <= T{0}) { + return T{0}; + } + + T estimate = value > T{1} ? value : T{1}; + + for (uint8_t i = 0; i < 64; i++) { + const T next = (estimate + value / estimate) / T{2}; + + if (abs(next - estimate) <= estimate * T{1e-16}) { + return next; + } + + estimate = next; + } + + return estimate; +} + +/** + * @brief Wrap an angle to the range [-pi, pi]. + * + * @tparam T Floating point type. + * @param angle Angle in radians. + * @return The equivalent angle in [-pi, pi]. + */ +template +constexpr T wrap_angle(T angle) { + constexpr T two_pi = T{2} * std::numbers::pi_v; + + const auto turns = static_cast(angle / two_pi + (angle < T{0} ? T{-0.5} : T{0.5})); + + return angle - static_cast(turns) * two_pi; +} + +/** + * @brief Sine by its Taylor series, after folding the angle into [-pi/2, pi/2]. + * + * @tparam T Floating point type. + * @param angle Angle in radians. + * @return The sine of the angle. + */ +template +constexpr T sin(T angle) { + angle = wrap_angle(angle); + + if (angle > std::numbers::pi_v / T{2}) { + angle = std::numbers::pi_v - angle; + } else if (angle < -std::numbers::pi_v / T{2}) { + angle = -std::numbers::pi_v - angle; + } + + const T squared = angle * angle; + T term = angle; + T sum = angle; + + for (uint8_t order = 1; order < 12; order++) { + term *= -squared / static_cast((2 * order) * (2 * order + 1)); + sum += term; + } + + return sum; +} + +/** + * @brief Cosine, as the sine of the complementary angle. + * + * @tparam T Floating point type. + * @param angle Angle in radians. + * @return The cosine of the angle. + */ +template +constexpr T cos(T angle) { + return sin(angle + std::numbers::pi_v / T{2}); +} + +/** + * @brief Point reached by a clothoid that starts straight along the x axis. + * + * @tparam T Floating point type. + */ +template +struct ClothoidPoint { + T x; + T y; +}; + +/** + * @brief Evaluate a clothoid, the curve whose curvature grows linearly with the distance traveled. + * + * @details The path of a robot that moves at a constant speed while its angular speed ramps up is a + * clothoid. With a heading of `sharpness * s^2 / 2` after a distance `s`, its coordinates are the + * Fresnel integrals `x = int cos(sharpness * t^2 / 2) dt` and `y = int sin(sharpness * t^2 / 2) dt`. + * They are evaluated here by their power series in the heading, which converges for every argument + * and needs about ten terms for the quarter turn that is the most a ramp of a slalom turn covers. + * + * @tparam T Floating point type. + * @param distance Distance traveled along the curve, in meters. + * @param sharpness Rate of change of the curvature with the distance, in 1/m^2. + * @param terms Number of terms of the series. + * @return The coordinates of the point, in the frame of the start of the curve. + */ +template +constexpr ClothoidPoint clothoid(T distance, T sharpness, uint8_t terms = 14) { + const T heading = sharpness * distance * distance / T{2}; + const T squared = heading * heading; + + T even_term = T{1}; + T odd_term = heading; + T x_sum = T{0}; + T y_sum = T{0}; + + for (uint8_t order = 0; order < terms; order++) { + x_sum += even_term / static_cast(4 * order + 1); + y_sum += odd_term / static_cast(4 * order + 3); + + even_term *= -squared / static_cast((2 * order + 1) * (2 * order + 2)); + odd_term *= -squared / static_cast((2 * order + 2) * (2 * order + 3)); + } + + return {.x = distance * x_sum, .y = distance * y_sum}; +} +} // namespace micras::core::math + +#endif // MICRAS_CORE_MATH_HPP From 10a32c8e352def05ea18a357f52c11b6a67e937c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 03:22:12 -0300 Subject: [PATCH 493/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Choose=20the=20mon?= =?UTF-8?q?itored=20PID=20controller=20in=20its=20configuration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The terms of the controller were only published for a variable monitor when the caller passed a flag that no caller passed, so the monitor showed five constants. There is one set of those variables for every instance, which makes it a property of the instance: a field of the configuration selects the one that is watched. The overload that takes the derivative never cleared the first run flag, the last response was stored and never read, and the description had the sign of the derivative term wrong: it acts on the controlled variable, not on the error. --- .../include/micras/core/pid_controller.hpp | 31 ++++++++++++------- micras_core/src/pid_controller.cpp | 18 +++++------ 2 files changed, 27 insertions(+), 22 deletions(-) diff --git a/micras_core/include/micras/core/pid_controller.hpp b/micras_core/include/micras/core/pid_controller.hpp index 6452847..dde7a4b 100644 --- a/micras_core/include/micras/core/pid_controller.hpp +++ b/micras_core/include/micras/core/pid_controller.hpp @@ -7,12 +7,20 @@ namespace micras::core { /** - * @brief Implementation of simple PID controller. Response = kp * (error + ki * integral(error) kd * d/dt(error)) + * @brief Implementation of simple PID controller. + * + * @details The response is `kp * (error + ki * integral(error) - kd * d/dt(state))`. The derivative + * acts on the controlled variable and not on the error, so a step of the setpoint does not kick it. */ class PidController { public: /** * @brief Configuration struct for the PID controller. + * + * @note A negative saturation or maximum integral disables the respective limit. The monitored + * flag makes the controller publish its terms to the file scope variables of its source file, + * where a variable monitor can read them. There is one set of those for every instance, so only + * one controller at a time should have it set. */ struct Config { float kp{}; @@ -21,6 +29,7 @@ class PidController { float setpoint{}; float saturation{-1.0F}; float max_integral{-1.0F}; + bool monitored{false}; }; /** @@ -47,21 +56,19 @@ class PidController { * * @param state Current value of the controlled variable. * @param elapsed_time Time since the last update. - * @param save Whether to save the variables for calibration. * @return Response of the controller. */ - float compute_response(float state, float elapsed_time, bool save = false); + float compute_response(float state, float elapsed_time); /** * @brief Update PID with new state and return response. * * @param state Current value of the controlled variable. - * @param state_change Derivative of the controlled variable. * @param elapsed_time Time since the last update. - * @param save Whether to save the variables for calibration. + * @param state_change Derivative of the controlled variable. * @return Response of the controller. */ - float compute_response(float state, float elapsed_time, float state_change, bool save = false); + float compute_response(float state, float elapsed_time, float state_change); private: /** @@ -94,6 +101,11 @@ class PidController { */ float max_integral; + /** + * @brief Whether the terms of the controller are published for a variable monitor. + */ + bool monitored; + /** * @brief Flag indicating whether this is the first run of the controller. */ @@ -105,14 +117,9 @@ class PidController { float error_acc = 0; /** - * @brief Previous error for derivative term. + * @brief Previous state for derivative term. */ float prev_state = 0; - - /** - * @brief Last response returned by the controller. - */ - float last_response = 0; }; } // namespace micras::core diff --git a/micras_core/src/pid_controller.cpp b/micras_core/src/pid_controller.cpp index ac20ac7..dd08569 100644 --- a/micras_core/src/pid_controller.cpp +++ b/micras_core/src/pid_controller.cpp @@ -23,7 +23,8 @@ PidController::PidController(Config config) : kd{config.kd}, setpoint{config.setpoint}, saturation{config.saturation}, - max_integral{config.max_integral} { } + max_integral{config.max_integral}, + monitored{config.monitored} { } void PidController::set_setpoint(float setpoint) { this->setpoint = setpoint; @@ -32,23 +33,22 @@ void PidController::set_setpoint(float setpoint) { void PidController::reset() { this->error_acc = 0; this->prev_state = 0; - this->last_response = 0; this->first_run = true; } -float PidController::compute_response(float state, float elapsed_time, bool save) { - if (first_run) { +float PidController::compute_response(float state, float elapsed_time) { + if (this->first_run) { this->prev_state = state; - this->first_run = false; } const float state_change = (state - this->prev_state) / elapsed_time; - return this->compute_response(state, elapsed_time, state_change, save); + return this->compute_response(state, elapsed_time, state_change); } -float PidController::compute_response(float state, float elapsed_time, float state_change, bool save) { +float PidController::compute_response(float state, float elapsed_time, float state_change) { const float error = this->setpoint - state; this->prev_state = state; + this->first_run = false; float response = this->kp * (error + this->ki * this->error_acc - this->kd * state_change); @@ -69,9 +69,7 @@ float PidController::compute_response(float state, float elapsed_time, float sta response = std::clamp(response, -this->saturation, this->saturation); } - this->last_response = response; - - if (save) { + if (this->monitored) { test_error = error; test_proportional = this->kp * error; test_integrative = this->kp * this->ki * this->error_acc; From 43e652fe9fd98c6eeb00f79e8a7f50263681fc79 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 03:22:44 -0300 Subject: [PATCH 494/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20the=20template=20?= =?UTF-8?q?implementations=20to=20impl/*.tpp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A template was implemented in a .cpp file under src/ that its header included from the bottom, with a NOLINT for the suspicious include and another for the include cycle it created, and the source globs compiled every one of those files a second time as an empty translation unit. They are included, not compiled, so they are named and placed for that: impl/*.tpp next to the header that owns them, which no longer include that header back. The folder is the one Asio and PCL use for the implementation of a public header, and the extension is one that no library gives another meaning, as Boost does with .ipp for what is not a template. The format target and the documentation pick the new extension up. --- micras_core/include/micras/core/fsm.hpp | 2 +- .../{src/fsm.cpp => include/micras/core/impl/fsm.tpp} | 8 +++----- micras_proxy/include/micras/proxy/argb.hpp | 2 +- micras_proxy/include/micras/proxy/dip_switch.hpp | 2 +- .../{src/argb.cpp => include/micras/proxy/impl/argb.tpp} | 7 +++---- .../micras/proxy/impl/dip_switch.tpp} | 7 +++---- .../micras/proxy/impl/torque_sensors.tpp} | 7 +++---- .../micras/proxy/impl/wall_sensors.tpp} | 7 +++---- micras_proxy/include/micras/proxy/torque_sensors.hpp | 2 +- micras_proxy/include/micras/proxy/wall_sensors.hpp | 2 +- 10 files changed, 20 insertions(+), 26 deletions(-) rename micras_core/{src/fsm.cpp => include/micras/core/impl/fsm.tpp} (88%) rename micras_proxy/{src/argb.cpp => include/micras/proxy/impl/argb.tpp} (94%) rename micras_proxy/{src/dip_switch.cpp => include/micras/proxy/impl/dip_switch.tpp} (84%) rename micras_proxy/{src/torque_sensors.cpp => include/micras/proxy/impl/torque_sensors.tpp} (92%) rename micras_proxy/{src/wall_sensors.cpp => include/micras/proxy/impl/wall_sensors.tpp} (94%) diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 66d8989..5434d64 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -117,6 +117,6 @@ class TFsm { }; } // namespace micras::core -#include "../src/fsm.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) +#include "micras/core/impl/fsm.tpp" // IWYU pragma: export #endif // MICRAS_CORE_FSM_HPP diff --git a/micras_core/src/fsm.cpp b/micras_core/include/micras/core/impl/fsm.tpp similarity index 88% rename from micras_core/src/fsm.cpp rename to micras_core/include/micras/core/impl/fsm.tpp index 18638c0..3f9ebe0 100644 --- a/micras_core/src/fsm.cpp +++ b/micras_core/include/micras/core/impl/fsm.tpp @@ -2,16 +2,14 @@ * @file */ -#ifndef MICRAS_CORE_FSM_CPP -#define MICRAS_CORE_FSM_CPP +#ifndef MICRAS_CORE_FSM_TPP +#define MICRAS_CORE_FSM_TPP #include #include #include #include -#include "micras/core/fsm.hpp" - namespace micras::core { template TFsm::TFsm(uint8_t initial_state_id) : current_state_id{initial_state_id} { } @@ -40,4 +38,4 @@ void TFsm::update() { } } // namespace micras::core -#endif // MICRAS_CORE_FSM_CPP +#endif // MICRAS_CORE_FSM_TPP diff --git a/micras_proxy/include/micras/proxy/argb.hpp b/micras_proxy/include/micras/proxy/argb.hpp index d7b62df..921b70a 100644 --- a/micras_proxy/include/micras/proxy/argb.hpp +++ b/micras_proxy/include/micras/proxy/argb.hpp @@ -165,6 +165,6 @@ class TArgb { }; } // namespace micras::proxy -#include "../src/argb.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) +#include "micras/proxy/impl/argb.tpp" // IWYU pragma: export #endif // MICRAS_PROXY_ARGB_HPP diff --git a/micras_proxy/include/micras/proxy/dip_switch.hpp b/micras_proxy/include/micras/proxy/dip_switch.hpp index 84e814f..84ba872 100644 --- a/micras_proxy/include/micras/proxy/dip_switch.hpp +++ b/micras_proxy/include/micras/proxy/dip_switch.hpp @@ -63,6 +63,6 @@ class TDipSwitch { }; } // namespace micras::proxy -#include "../src/dip_switch.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) +#include "micras/proxy/impl/dip_switch.tpp" // IWYU pragma: export #endif // MICRAS_PROXY_DIP_SWITCH_HPP diff --git a/micras_proxy/src/argb.cpp b/micras_proxy/include/micras/proxy/impl/argb.tpp similarity index 94% rename from micras_proxy/src/argb.cpp rename to micras_proxy/include/micras/proxy/impl/argb.tpp index 4be7358..64d930f 100644 --- a/micras_proxy/src/argb.cpp +++ b/micras_proxy/include/micras/proxy/impl/argb.tpp @@ -2,11 +2,10 @@ * @file */ -#ifndef MICRAS_PROXY_ARGB_CPP -#define MICRAS_PROXY_ARGB_CPP +#ifndef MICRAS_PROXY_ARGB_TPP +#define MICRAS_PROXY_ARGB_TPP #include -#include "micras/proxy/argb.hpp" namespace micras::proxy { template @@ -78,4 +77,4 @@ bool TArgb::was_initialized() const { } } // namespace micras::proxy -#endif // MICRAS_PROXY_ARGB_CPP +#endif // MICRAS_PROXY_ARGB_TPP diff --git a/micras_proxy/src/dip_switch.cpp b/micras_proxy/include/micras/proxy/impl/dip_switch.tpp similarity index 84% rename from micras_proxy/src/dip_switch.cpp rename to micras_proxy/include/micras/proxy/impl/dip_switch.tpp index 2a1a203..817892a 100644 --- a/micras_proxy/src/dip_switch.cpp +++ b/micras_proxy/include/micras/proxy/impl/dip_switch.tpp @@ -2,13 +2,12 @@ * @file */ -#ifndef MICRAS_PROXY_DIP_SWITCH_CPP -#define MICRAS_PROXY_DIP_SWITCH_CPP +#ifndef MICRAS_PROXY_DIP_SWITCH_TPP +#define MICRAS_PROXY_DIP_SWITCH_TPP #include #include "micras/core/utils.hpp" #include "micras/hal/gpio.hpp" -#include "micras/proxy/dip_switch.hpp" namespace micras::proxy { template @@ -32,4 +31,4 @@ uint8_t TDipSwitch::get_switches_value() const { } } // namespace micras::proxy -#endif // MICRAS_PROXY_DIP_SWITCH_CPP +#endif // MICRAS_PROXY_DIP_SWITCH_TPP diff --git a/micras_proxy/src/torque_sensors.cpp b/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp similarity index 92% rename from micras_proxy/src/torque_sensors.cpp rename to micras_proxy/include/micras/proxy/impl/torque_sensors.tpp index 51edfaa..b033e0f 100644 --- a/micras_proxy/src/torque_sensors.cpp +++ b/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp @@ -2,14 +2,13 @@ * @file */ -#ifndef MICRAS_PROXY_TORQUE_SENSORS_CPP -#define MICRAS_PROXY_TORQUE_SENSORS_CPP +#ifndef MICRAS_PROXY_TORQUE_SENSORS_TPP +#define MICRAS_PROXY_TORQUE_SENSORS_TPP #include #include "micras/core/butterworth_filter.hpp" #include "micras/core/utils.hpp" -#include "micras/proxy/torque_sensors.hpp" namespace micras::proxy { template @@ -66,4 +65,4 @@ bool TTorqueSensors::was_initialized() const { } } // namespace micras::proxy -#endif // MICRAS_PROXY_TORQUE_SENSORS_CPP +#endif // MICRAS_PROXY_TORQUE_SENSORS_TPP diff --git a/micras_proxy/src/wall_sensors.cpp b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp similarity index 94% rename from micras_proxy/src/wall_sensors.cpp rename to micras_proxy/include/micras/proxy/impl/wall_sensors.tpp index e2b098e..68e660f 100644 --- a/micras_proxy/src/wall_sensors.cpp +++ b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp @@ -2,8 +2,8 @@ * @file */ -#ifndef MICRAS_PROXY_WALL_SENSORS_CPP -#define MICRAS_PROXY_WALL_SENSORS_CPP +#ifndef MICRAS_PROXY_WALL_SENSORS_TPP +#define MICRAS_PROXY_WALL_SENSORS_TPP #include #include @@ -11,7 +11,6 @@ #include "micras/core/butterworth_filter.hpp" #include "micras/core/utils.hpp" #include "micras/hal/pwm.hpp" -#include "micras/proxy/wall_sensors.hpp" namespace micras::proxy { template @@ -84,4 +83,4 @@ bool TWallSensors::was_initialized() const { } } // namespace micras::proxy -#endif // MICRAS_PROXY_WALL_SENSORS_CPP +#endif // MICRAS_PROXY_WALL_SENSORS_TPP diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 600372f..30232a8 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -134,6 +134,6 @@ class TTorqueSensors { }; } // namespace micras::proxy -#include "../src/torque_sensors.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) +#include "micras/proxy/impl/torque_sensors.tpp" // IWYU pragma: export #endif // MICRAS_PROXY_TORQUE_SENSORS_HPP diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 1a58dd3..075a6b3 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -147,6 +147,6 @@ class TWallSensors { }; } // namespace micras::proxy -#include "../src/wall_sensors.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) +#include "micras/proxy/impl/wall_sensors.tpp" // IWYU pragma: export #endif // MICRAS_PROXY_WALL_SENSORS_HPP From 662cce0bb69b6abd6e209558941850ce863a1b69 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 03:22:56 -0300 Subject: [PATCH 495/561] =?UTF-8?q?=E2=9C=A8=20Let=20a=20PWM=20output=20be?= =?UTF-8?q?=20inverted=20and=20an=20ADC=20keep=20whole=20sequences?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit An inverted output is active for the last part of the period instead of the first. On a center aligned timer that centers the pulse on the overflow instead of the underflow, which lets two groups of channels of one timer take turns with no change to the generated configuration. The duty cycle keeps meaning the fraction of the period the output is active for, so zero is off for both kinds, and the emergency stop writes the compare value that silences each of them. The field has no default, so that every configuration says which kind it is. The duty cycle is now clamped. A negative one used to be converted to an unsigned compare value, which only the absolute value taken by the motor proxy kept from happening. The DMA rewrites the buffer of a converter continuously, so reading it could mix the values of two sequences. A converter can now be started with a second buffer that the transfer complete interrupt copies every whole sequence to, a few words at a time, together with a counter that tells a reader whether it is new. --- micras_hal/include/micras/hal/adc_dma.hpp | 75 +++++++++++++++++++++++ micras_hal/include/micras/hal/pwm.hpp | 13 +++- micras_hal/stm32/src/adc_dma.cpp | 67 ++++++++++++++++++++ micras_hal/stm32/src/mcu.cpp | 8 ++- micras_hal/stm32/src/pwm.cpp | 21 ++++++- 5 files changed, 180 insertions(+), 4 deletions(-) diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 2aa5430..5221f8f 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -5,6 +5,7 @@ #ifndef MICRAS_HAL_ADC_DMA_HPP #define MICRAS_HAL_ADC_DMA_HPP +#include #include #include #include @@ -35,6 +36,19 @@ class AdcDma { */ explicit AdcDma(const Config& config); + /** + * @brief Special member functions, of which only the destructor exists. + * + * @note The interrupt finds a converter by its address, so an object cannot be copied or moved. + */ + ///@{ + ~AdcDma(); + AdcDma(const AdcDma&) = delete; + AdcDma(AdcDma&&) = delete; + AdcDma& operator=(const AdcDma&) = delete; + AdcDma& operator=(AdcDma&&) = delete; + ///@} + /** * @brief Enable ADC, start conversion of regular group and transfer result through DMA. * @@ -51,6 +65,33 @@ class AdcDma { */ bool start_dma(std::span buffer); + /** + * @brief Start the conversions as start_dma does, keeping a copy of every complete sequence. + * + * @details The DMA rewrites the buffer continuously, so reading it while it is being written + * can mix values of two different sequences. Every time the DMA reaches the end of the buffer, + * the transfer complete interrupt copies it to the snapshot, which is therefore always one whole + * sequence. The copy is a few words long, so the interrupt costs a few tens of cycles. + * + * @note Both buffers are borrowed and have to outlive the conversions. + * + * @param buffer 16 bit destination buffer of the DMA. + * @param snapshot Buffer of the same size that receives the copies. + * @return True if the conversion was started, false otherwise. + */ + bool start_dma(std::span buffer, std::span snapshot); + + /** + * @brief Read the last complete sequence. + * + * @note The snapshot may be replaced while it is being read, which is detected with the + * sequence counter and answered by reading it again. + * + * @param destination Buffer of the size of the snapshot that receives it. + * @return The number of sequences completed so far, which tells whether this one is new. + */ + uint32_t read_snapshot(std::span destination) const; + /** * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode). */ @@ -80,7 +121,26 @@ class AdcDma { */ bool was_initialized() const; + /** + * @brief Copy the buffer of the converter that completed a sequence to its snapshot. + * + * @note To be called by the conversion complete callback only. + * + * @param handle Handle of the converter that completed a sequence. + */ + static void on_sequence_complete(const ADC_HandleTypeDef* handle); + private: + /** + * @brief Largest number of converters that can keep a snapshot. + */ + static constexpr uint8_t max_instances{4}; + + /** + * @brief Converters that keep a snapshot, for the interrupt to find the one that completed. + */ + static std::array instances; + /** * @brief Maximum ADC reading. */ @@ -96,6 +156,21 @@ class AdcDma { */ ADC_HandleTypeDef* handle; + /** + * @brief Destination buffer of the DMA. + */ + std::span buffer; + + /** + * @brief Copy of the last complete sequence. + */ + std::span snapshot; + + /** + * @brief Number of sequences completed so far. + */ + volatile uint32_t sequence{}; + /** * @brief Flag to check if the ADC was calibrated and started. */ diff --git a/micras_hal/include/micras/hal/pwm.hpp b/micras_hal/include/micras/hal/pwm.hpp index a513a70..986d4ce 100644 --- a/micras_hal/include/micras/hal/pwm.hpp +++ b/micras_hal/include/micras/hal/pwm.hpp @@ -17,11 +17,17 @@ class Pwm { public: /** * @brief PWM configuration struct. + * + * @note An inverted output is active for the last part of the period instead of the first. On a + * center aligned timer that centers its pulse on the overflow instead of the underflow, which + * is how two groups of channels of one timer are made to take turns. The duty cycle keeps + * meaning the fraction of the period the output is active for, so zero is always off. */ struct Config { void (*init_function)(); TIM_HandleTypeDef* handle; uint32_t timer_channel; + bool inverted; }; /** @@ -34,7 +40,7 @@ class Pwm { /** * @brief Set the PWM duty cycle. * - * @param duty_cycle Duty cycle value. + * @param duty_cycle Duty cycle value in percent, which is clamped to the range from 0 to 100. */ void set_duty_cycle(float duty_cycle); @@ -69,6 +75,11 @@ class Pwm { */ uint32_t channel; + /** + * @brief Whether the output is active for the last part of the period. + */ + bool inverted; + /** * @brief Flag to check if the PWM was started. */ diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 2ac9269..77d0e51 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -2,13 +2,29 @@ * @file */ +#include +#include #include #include #include #include "micras/hal/adc_dma.hpp" +extern "C" { +/** + * @brief Callback of the vendor HAL for the end of a DMA transfer of a converter. + * + * @param hadc Handle of the converter. + */ +// NOLINTNEXTLINE(readability-identifier-naming) the name is fixed by the vendor HAL +void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) { + micras::hal::AdcDma::on_sequence_complete(hadc); +} +} + namespace micras::hal { +std::array AdcDma::instances{}; + AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { if (this->handle->State == HAL_ADC_STATE_RESET) { @@ -26,6 +42,10 @@ AdcDma::AdcDma(const Config& config) : this->initialized = status == HAL_OK; } +AdcDma::~AdcDma() { + std::ranges::replace(instances, this, static_cast(nullptr)); +} + bool AdcDma::start_dma(std::span buffer) { if (HAL_ADC_Start_DMA(this->handle, buffer.data(), buffer.size()) != HAL_OK) { this->initialized = false; @@ -39,6 +59,53 @@ bool AdcDma::start_dma(std::span buffer) { return this->start_dma({std::bit_cast(buffer.data()), buffer.size()}); } +bool AdcDma::start_dma(std::span buffer, std::span snapshot) { + if (snapshot.size() != buffer.size()) { + this->initialized = false; + return false; + } + + this->buffer = buffer; + this->snapshot = snapshot; + + auto* const slot = std::ranges::find(instances, nullptr); + + if (slot == instances.end()) { + this->initialized = false; + return false; + } + + *slot = this; + + return this->start_dma(buffer); +} + +uint32_t AdcDma::read_snapshot(std::span destination) const { + uint32_t before = this->sequence; + + while (true) { + std::ranges::copy(this->snapshot, destination.begin()); + + const uint32_t after = this->sequence; + + if (after == before) { + return after; + } + + before = after; + } +} + +void AdcDma::on_sequence_complete(const ADC_HandleTypeDef* handle) { + for (AdcDma* instance : instances) { + if (instance != nullptr and instance->handle == handle) { + std::ranges::copy(instance->buffer, instance->snapshot.begin()); + instance->sequence = instance->sequence + 1; + return; + } + } +} + void AdcDma::stop_dma() { HAL_ADC_Stop_DMA(this->handle); } diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp index 381445c..8901728 100644 --- a/micras_hal/stm32/src/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -55,6 +55,11 @@ static constexpr uint32_t watchdog_max_reload{0xFFF}; */ static constexpr uint32_t watchdog_timeout_us{1000}; +/** + * @brief Compare value that keeps an inverted PWM output inactive, above any period. + */ +static constexpr uint32_t inactive_compare{0xFFFFFFFF}; + void Mcu::init(const Config& config) { SCB_EnableICache(); @@ -75,7 +80,8 @@ void Mcu::init(const Config& config) { void Mcu::emergency_stop(std::span pwm_outputs, std::span enable_gpios) { for (const auto& pwm_output : pwm_outputs) { - __HAL_TIM_SET_COMPARE(pwm_output.handle, pwm_output.timer_channel, 0); + const uint32_t compare = pwm_output.inverted ? inactive_compare : 0; + __HAL_TIM_SET_COMPARE(pwm_output.handle, pwm_output.timer_channel, compare); } for (const auto& enable_gpio : enable_gpios) { diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index 7cdc5cb..abf573a 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -2,6 +2,7 @@ * @file */ +#include #include #include @@ -30,16 +31,32 @@ static uint32_t get_timer_clock_frequency(const TIM_TypeDef* instance) { return clock_config.APB1CLKDivider == RCC_APB1_DIV1 ? pclk1 : 2 * pclk1; } -Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel} { +/** + * @brief Mask that turns the identifier of one of the first four channels into the position of its + * bits in the capture and compare enable register. + */ +static constexpr uint32_t channel_shift_mask{0x1F}; + +Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel}, inverted{config.inverted} { if (this->handle->State == HAL_TIM_STATE_RESET) { config.init_function(); } + if (this->inverted) { + this->handle->Instance->CCER |= TIM_CCER_CC1P << (this->channel & channel_shift_mask); + } + + this->set_duty_cycle(0.0F); this->initialized = HAL_TIM_PWM_Start(this->handle, this->channel) == HAL_OK; - __HAL_TIM_SET_COMPARE(this->handle, this->channel, 0); } void Pwm::set_duty_cycle(float duty_cycle) { + duty_cycle = std::clamp(duty_cycle, 0.0F, 100.0F); + + if (this->inverted) { + duty_cycle = 100.0F - duty_cycle; + } + const float scaled = duty_cycle * static_cast(__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) * 0.01F; // NOLINTNEXTLINE(bugprone-incorrect-roundings) From ad0b7a34002a6dd327f4bebb54be443a23dc73f7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 03:23:14 -0300 Subject: [PATCH 496/561] =?UTF-8?q?=E2=9C=A8=20Add=20the=20watchdog=20and?= =?UTF-8?q?=20tick=20proxies?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The application refreshed and reconfigured the watchdog through micras::hal, which no simulator can provide, and which the layering asks the application not to reach. The watchdog is now a proxy, with a scoped extension of its timeout for the few operations that legitimately stall the core, such as a flash erase. The tick paces a loop at a fixed period. Every tick is scheduled from the previous one rather than from when the wait was called, so the period does not drift with what the loop body did, and an overrun is reported as missed ticks instead of being absorbed. The deadlines are kept in cycles of the core clock, which resolves a period of a hundred microseconds to a few parts per million, where the stopwatch in microseconds would jitter by a percent. The loop stays a plain loop, with no interrupt involved. --- micras_proxy/include/micras/proxy/tick.hpp | 67 +++++++++++++ .../include/micras/proxy/watchdog.hpp | 93 +++++++++++++++++++ micras_proxy/src/tick.cpp | 26 ++++++ micras_proxy/src/watchdog.cpp | 31 +++++++ 4 files changed, 217 insertions(+) create mode 100644 micras_proxy/include/micras/proxy/tick.hpp create mode 100644 micras_proxy/include/micras/proxy/watchdog.hpp create mode 100644 micras_proxy/src/tick.cpp create mode 100644 micras_proxy/src/watchdog.cpp diff --git a/micras_proxy/include/micras/proxy/tick.hpp b/micras_proxy/include/micras/proxy/tick.hpp new file mode 100644 index 0000000..7106e21 --- /dev/null +++ b/micras_proxy/include/micras/proxy/tick.hpp @@ -0,0 +1,67 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_TICK_HPP +#define MICRAS_PROXY_TICK_HPP + +#include + +namespace micras::proxy { +/** + * @brief Class that paces a loop at a fixed period. + * + * @details Every tick is scheduled from the previous one and not from when the wait was called, so + * the period does not drift with the time the loop body takes, and a body that overruns is reported + * rather than absorbed. The deadlines are kept in cycles of the core clock, which resolves a + * period of a hundred microseconds to a few parts per million, where a timebase in microseconds + * would jitter by a percent. + * + * @note The loop stays a plain loop: the wait spins on the cycle counter and no interrupt is + * involved. A simulator implements the wait by advancing its physics by one period. + */ +class Tick { +public: + /** + * @brief Configuration struct for the tick. + */ + struct Config { + uint32_t period_us; + }; + + /** + * @brief Construct a new Tick object, whose first tick is one period from now. + * + * @param config Configuration for the tick. + */ + explicit Tick(const Config& config); + + /** + * @brief Wait for the next tick. + * + * @return The number of periods since the previous tick, which is one unless the caller took + * longer than a period, in which case the ticks that were missed are included. + */ + uint32_t wait(); + + /** + * @brief Get the time since the last tick. + * + * @return The time in microseconds, which is how long the body of the loop has taken so far. + */ + uint32_t elapsed_time_us() const; + +private: + /** + * @brief Period in cycles of the core clock. + */ + uint32_t period; + + /** + * @brief Value of the cycle counter at the last tick. + */ + uint32_t last_tick; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_TICK_HPP diff --git a/micras_proxy/include/micras/proxy/watchdog.hpp b/micras_proxy/include/micras/proxy/watchdog.hpp new file mode 100644 index 0000000..5d0d1c5 --- /dev/null +++ b/micras_proxy/include/micras/proxy/watchdog.hpp @@ -0,0 +1,93 @@ +/** + * @file + */ + +#ifndef MICRAS_PROXY_WATCHDOG_HPP +#define MICRAS_PROXY_WATCHDOG_HPP + +#include + +namespace micras::proxy { +/** + * @brief Class for keeping the watchdog of the microcontroller fed. + * + * @note Once started the watchdog cannot be stopped, so anything that hangs for longer than its + * timeout resets the microcontroller, which brings every actuator back to its inactive state. + */ +class Watchdog { +public: + /** + * @brief Configuration struct for the watchdog. + */ + struct Config { + uint32_t timeout_ms; + }; + + /** + * @brief Temporary widening of the timeout, which ends with the life of the object. + * + * @note Meant for the few operations that legitimately stall the core for longer than the + * control loop budget, such as erasing a flash sector, and only with the robot stopped. + */ + class Extension { + public: + /** + * @brief Widen the timeout of a watchdog. + * + * @param watchdog The watchdog. + * @param timeout_ms The timeout while the extension lives, in milliseconds. + */ + Extension(Watchdog& watchdog, uint32_t timeout_ms); + + /** + * @brief Bring the timeout back to the configured one. + */ + ~Extension(); + + /** + * @brief Special member functions deleted, since an extension ends exactly once. + */ + ///@{ + Extension(const Extension&) = delete; + Extension(Extension&&) = delete; + Extension& operator=(const Extension&) = delete; + Extension& operator=(Extension&&) = delete; + ///@} + + private: + /** + * @brief Watchdog whose timeout was widened. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) the extension cannot outlive the watchdog + Watchdog& watchdog; + }; + + /** + * @brief Construct a new Watchdog object, starting the watchdog. + * + * @param config Configuration for the watchdog. + */ + explicit Watchdog(const Config& config); + + /** + * @brief Tell the watchdog that the program is alive. + */ + void refresh(); + + /** + * @brief Widen the timeout for as long as the returned object lives. + * + * @param timeout_ms The timeout during the extension, in milliseconds. + * @return The extension. + */ + [[nodiscard]] Extension extend(uint32_t timeout_ms); + +private: + /** + * @brief Timeout outside of any extension, in milliseconds. + */ + uint32_t timeout_ms; +}; +} // namespace micras::proxy + +#endif // MICRAS_PROXY_WATCHDOG_HPP diff --git a/micras_proxy/src/tick.cpp b/micras_proxy/src/tick.cpp new file mode 100644 index 0000000..17850d0 --- /dev/null +++ b/micras_proxy/src/tick.cpp @@ -0,0 +1,26 @@ +/** + * @file + */ + +#include + +#include "micras/hal/timer.hpp" +#include "micras/proxy/tick.hpp" + +namespace micras::proxy { +Tick::Tick(const Config& config) : + period{hal::Timer::to_cycles(config.period_us)}, last_tick{hal::Timer::get_counter()} { } + +uint32_t Tick::wait() { + while (hal::Timer::get_counter() - this->last_tick < this->period) { } + + const uint32_t ticks = (hal::Timer::get_counter() - this->last_tick) / this->period; + this->last_tick += ticks * this->period; + + return ticks; +} + +uint32_t Tick::elapsed_time_us() const { + return hal::Timer::to_microseconds(hal::Timer::get_counter() - this->last_tick); +} +} // namespace micras::proxy diff --git a/micras_proxy/src/watchdog.cpp b/micras_proxy/src/watchdog.cpp new file mode 100644 index 0000000..66d3e1a --- /dev/null +++ b/micras_proxy/src/watchdog.cpp @@ -0,0 +1,31 @@ +/** + * @file + */ + +#include + +#include "micras/hal/mcu.hpp" +#include "micras/proxy/watchdog.hpp" + +namespace micras::proxy { +Watchdog::Extension::Extension(Watchdog& watchdog, uint32_t timeout_ms) : watchdog{watchdog} { + hal::Mcu::set_watchdog_timeout(timeout_ms); +} + +Watchdog::Extension::~Extension() { + hal::Mcu::set_watchdog_timeout(this->watchdog.timeout_ms); +} + +Watchdog::Watchdog(const Config& config) : timeout_ms{config.timeout_ms} { + hal::Mcu::set_watchdog_timeout(this->timeout_ms); +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) there is one watchdog, and this object is it +void Watchdog::refresh() { + hal::Mcu::refresh_watchdog(); +} + +Watchdog::Extension Watchdog::extend(uint32_t timeout_ms) { + return Extension{*this, timeout_ms}; +} +} // namespace micras::proxy From 39abc31bbc5c16df1ff52bbd49a56a8de6657778 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 04:34:45 -0300 Subject: [PATCH 497/561] =?UTF-8?q?=F0=9F=90=9B=20Keep=20the=20low=20pass?= =?UTF-8?q?=20filter=20accurate=20at=20cutoffs=20far=20below=20its=20rate?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The direct form computes each output as the small difference of two large terms, and gets its unity gain from three rounded coefficients adding up exactly. Single precision stops doing both when the cutoff is a thousand times below the sampling rate: the 7.64 Hz filter of the battery sampled at 10 kHz settles 0.35 % away from its input and does not see a change of 5 mV in 12 V at all. The recursion is now written for the change of the output, which is the same transfer function with a gain of one by construction. The same filter comes out good to 0.001 %, at the cost of nothing, so the rate of the control loop is no longer something the filters have an opinion about. --- .../micras/core/butterworth_filter.hpp | 31 ++++++++++++---- micras_core/src/butterworth_filter.cpp | 37 +++++-------------- 2 files changed, 32 insertions(+), 36 deletions(-) diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp index 53bf19b..56add89 100644 --- a/micras_core/include/micras/core/butterworth_filter.hpp +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -95,6 +95,15 @@ class ButterworthFilter { /** * @brief Produce a new value from measured data. * + * @details The recursion is written for the change of the output instead of the output, + * `y[k] = y[k-1] + r[k]` with `r[k] = a2 * r[k-1] + b0 * (x[k] + 2 * x[k-1] + x[k-2] - 4 * y[k-1])`, + * which is the same transfer function. The direct form computes the output as the small + * difference of two large terms, and gets its unity gain from three rounded coefficients + * adding up exactly, which single precision stops doing when the cutoff is a thousand times + * below the sampling rate: a 7.64 Hz filter sampled at 10 kHz comes out with a gain error of + * 0.35 % and blind to changes of 0.04 %. In this form the gain is one by construction and the + * same filter is good to 0.001 %. + * * @param x0 Last measure. * @return Filtered value. */ @@ -109,24 +118,30 @@ class ButterworthFilter { private: /** - * @brief Last input values of the filter. + * @brief Last two input values of the filter, most recent first. + */ + std::array inputs{}; + + /** + * @brief Last output value of the filter. */ - std::array x_array{}; + float output{}; /** - * @brief Last output values of the filter. + * @brief Change of the output in the last update. */ - std::array y_array{}; + float rate{}; /** - * @brief Coefficients of the filter related to the output value, oldest first. + * @brief Share of the rate that is kept from one update to the next, which is the feedback + * coefficient of the oldest output. */ - std::array a_array{}; + float damping; /** - * @brief Coefficients of the filter related to the input value, oldest first. + * @brief Gain from the inputs to the rate, which is the first feed forward coefficient. */ - std::array b_array{}; + float gain; }; } // namespace micras::core diff --git a/micras_core/src/butterworth_filter.cpp b/micras_core/src/butterworth_filter.cpp index f6fe9e2..ba6b03d 100644 --- a/micras_core/src/butterworth_filter.cpp +++ b/micras_core/src/butterworth_filter.cpp @@ -2,9 +2,7 @@ * @file */ -#include #include -#include #include #include "micras/core/butterworth_filter.hpp" @@ -31,40 +29,23 @@ ButterworthFilter::Coefficients ButterworthFilter::compute_coefficients(const Co ButterworthFilter::ButterworthFilter(const Config& config) { const Coefficients coefficients = compute_coefficients(config); - std::get<0>(this->a_array) = std::get<1>(coefficients.feedback); - std::get<1>(this->a_array) = std::get<0>(coefficients.feedback); - - std::get<0>(this->b_array) = std::get<2>(coefficients.feed_forward); - std::get<1>(this->b_array) = std::get<1>(coefficients.feed_forward); - std::get<2>(this->b_array) = std::get<0>(coefficients.feed_forward); + this->damping = std::get<1>(coefficients.feedback); + this->gain = std::get<0>(coefficients.feed_forward); } float ButterworthFilter::update(float x0) { - std::get<0>(this->x_array) = std::get<1>(this->x_array); - std::get<1>(this->x_array) = std::get<2>(this->x_array); - std::get<2>(this->x_array) = x0; - - float x_b_dot = 0; - - for (uint8_t i = 0; i < filter_order + 1; i++) { - x_b_dot += this->x_array.at(i) * this->b_array.at(i); - } - - float y_a_dot = 0; - - for (uint8_t i = 0; i < filter_order; i++) { - y_a_dot += this->y_array.at(i) * this->a_array.at(i); - } + const float input_sum = x0 + 2.0F * std::get<0>(this->inputs) + std::get<1>(this->inputs); - const float y0 = x_b_dot - y_a_dot; + this->rate = this->damping * this->rate + this->gain * (input_sum - 4.0F * this->output); + this->output += this->rate; - std::get<0>(this->y_array) = std::get<1>(this->y_array); - std::get<1>(this->y_array) = y0; + std::get<1>(this->inputs) = std::get<0>(this->inputs); + std::get<0>(this->inputs) = x0; - return y0; + return this->output; } float ButterworthFilter::get_last() const { - return std::get<1>(this->y_array); + return this->output; } } // namespace micras::core From 1dec503990f097ca56b1f5aa53dc78ced880ba33 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 04:35:00 -0300 Subject: [PATCH 498/561] =?UTF-8?q?=F0=9F=90=9B=20Start=20an=20inverted=20?= =?UTF-8?q?PWM=20output=20off=20instead=20of=20fully=20on?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The compare register of a channel is preloaded, so the value a constructor writes to it waits for the next update of the timer, and until then the register holds the zero it was reset to. For a normal output that is a duty cycle of zero. For an inverted one it is a duty cycle of one hundred, so the output started fully on for up to a period of the timer. The first value is now written around the preload. --- micras_hal/include/micras/hal/pwm.hpp | 6 +++++- micras_hal/stm32/src/pwm.cpp | 23 +++++++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/micras_hal/include/micras/hal/pwm.hpp b/micras_hal/include/micras/hal/pwm.hpp index 986d4ce..7830c83 100644 --- a/micras_hal/include/micras/hal/pwm.hpp +++ b/micras_hal/include/micras/hal/pwm.hpp @@ -31,7 +31,11 @@ class Pwm { }; /** - * @brief Construct a new Pwm object. + * @brief Construct a new Pwm object, with its output at a duty cycle of zero. + * + * @note The compare register is preloaded, so a value written to it waits for the next update + * of the timer. The first one is written around the preload: an inverted output would otherwise + * start fully on, since zero is what the register holds until then. * * @param config Configuration for the PWM. */ diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index abf573a..0575eb0 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -37,6 +37,26 @@ static uint32_t get_timer_clock_frequency(const TIM_TypeDef* instance) { */ static constexpr uint32_t channel_shift_mask{0x1F}; +/** + * @brief Make a value written to the compare register of a channel take effect at once. + * + * @param handle Timer handle. + * @param channel Timer channel. + */ +static void disable_compare_preload(TIM_HandleTypeDef* handle, uint32_t channel) { + __HAL_TIM_DISABLE_OCxPRELOAD(handle, channel); +} + +/** + * @brief Make a value written to the compare register of a channel wait for the next update. + * + * @param handle Timer handle. + * @param channel Timer channel. + */ +static void enable_compare_preload(TIM_HandleTypeDef* handle, uint32_t channel) { + __HAL_TIM_ENABLE_OCxPRELOAD(handle, channel); +} + Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel}, inverted{config.inverted} { if (this->handle->State == HAL_TIM_STATE_RESET) { config.init_function(); @@ -46,7 +66,10 @@ Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_cha this->handle->Instance->CCER |= TIM_CCER_CC1P << (this->channel & channel_shift_mask); } + disable_compare_preload(this->handle, this->channel); this->set_duty_cycle(0.0F); + enable_compare_preload(this->handle, this->channel); + this->initialized = HAL_TIM_PWM_Start(this->handle, this->channel) == HAL_OK; } From 453575b2da53453ee431cb5be9f77cd660f022c1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 04:35:19 -0300 Subject: [PATCH 499/561] =?UTF-8?q?=E2=9C=A8=20Let=20the=20DMA=20carry=20o?= =?UTF-8?q?ut=20an=20SPI=20transfer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A transfer on SPI3 blocked the caller for as long as it took, 35 us for the 17 bytes of a sample of the inertial measurement unit, which is a third of the period the control loop is heading to. A device can now start a transfer and look at its state later. It is selected when the transfer starts and unselected by the interrupt that ends it, so the bus is free for the other devices on it as soon as the last bit is out, and a transfer that ended in a bus error reads as failed instead of as data. SPI3 gains two DMA streams, their interrupts and its own, since the vendor HAL closes a transfer from the end of transfer interrupt of the peripheral. The data cache is off, so the buffers need no maintenance, only to be in memory that the DMA reaches. --- micras_hal/include/micras/hal/spi.hpp | 76 +++++++++++++++++++++++++++ micras_hal/stm32/src/spi.cpp | 74 ++++++++++++++++++++++++++ 2 files changed, 150 insertions(+) diff --git a/micras_hal/include/micras/hal/spi.hpp b/micras_hal/include/micras/hal/spi.hpp index 4dfbf15..93941d0 100644 --- a/micras_hal/include/micras/hal/spi.hpp +++ b/micras_hal/include/micras/hal/spi.hpp @@ -5,6 +5,7 @@ #ifndef MICRAS_HAL_SPI_HPP #define MICRAS_HAL_SPI_HPP +#include #include #include @@ -33,6 +34,19 @@ class Spi { uint32_t clock_phase; }; + /** + * @brief State of the transfer started by start_transfer. + * + * @note A transfer that could not be started and one that ended in a bus error both read as + * failed, and the received data is valid only when complete. + */ + enum class Transfer : uint8_t { + NONE = 0, + RUNNING = 1, + COMPLETE = 2, + FAILED = 3, + }; + /** * @brief Construct a new Spi object. * @@ -40,6 +54,20 @@ class Spi { */ explicit Spi(const Config& config); + /** + * @brief Special member functions, of which only the destructor exists. + * + * @note The interrupt that ends a transfer finds its device by address, so an object cannot be + * copied or moved. + */ + ///@{ + ~Spi(); + Spi(const Spi&) = delete; + Spi(Spi&&) = delete; + Spi& operator=(const Spi&) = delete; + Spi& operator=(Spi&&) = delete; + ///@} + /** * @brief Activate the chip select, reconfiguring the bus for this device if needed. * @@ -80,6 +108,29 @@ class Spi { */ bool transmit_receive(std::span transmitted, std::span received); + /** + * @brief Start a transfer in both directions that the DMA carries out, without waiting for it. + * + * @details The device is selected here and unselected by the interrupt that ends the transfer, + * so the bus is free for the other devices on it as soon as the last bit is out, and the caller + * has nothing to do but look at the state of the transfer later. + * + * @note Both buffers are borrowed until the transfer ends, and have to be in memory that the DMA + * reaches, which on the parts with tightly coupled memory excludes it. + * + * @param transmitted Data to transmit. + * @param received Buffer to receive data into, at least as large as the transmitted one. + * @return True if the transfer was started, false otherwise. + */ + bool start_transfer(std::span transmitted, std::span received); + + /** + * @brief Get the state of the last transfer started by start_transfer. + * + * @return State of the transfer. + */ + Transfer get_transfer() const; + /** * @brief Check if the SPI was successfully initialized. * @@ -87,7 +138,27 @@ class Spi { */ bool was_initialized() const; + /** + * @brief Unselect the device whose transfer ended and record how it ended. + * + * @note To be called by the transfer callbacks only. + * + * @param handle Handle of the bus whose transfer ended. + * @param succeeded Whether the transfer ended without a bus error. + */ + static void on_transfer_end(const SPI_HandleTypeDef* handle, bool succeeded); + private: + /** + * @brief Largest number of transfers that can be running at once, which is one per bus. + */ + static constexpr uint8_t max_transfers{6}; + + /** + * @brief Devices with a transfer running, for the interrupt to find the one that ended. + */ + static std::array transferring; + /** * @brief Handle for the SPI. */ @@ -113,6 +184,11 @@ class Spi { */ uint32_t clock_phase; + /** + * @brief State of the last transfer started by start_transfer. + */ + volatile Transfer transfer{Transfer::NONE}; + /** * @brief Flag to check if the SPI was initialized. */ diff --git a/micras_hal/stm32/src/spi.cpp b/micras_hal/stm32/src/spi.cpp index a862910..661bca4 100644 --- a/micras_hal/stm32/src/spi.cpp +++ b/micras_hal/stm32/src/spi.cpp @@ -2,13 +2,39 @@ * @file */ +#include +#include #include #include #include "micras/hal/spi.hpp" #include "micras/hal/timer.hpp" +extern "C" { +/** + * @brief Callback of the vendor HAL for the end of a transfer in both directions. + * + * @param hspi Handle of the bus. + */ +// NOLINTNEXTLINE(readability-identifier-naming) the name is fixed by the vendor HAL +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef* hspi) { + micras::hal::Spi::on_transfer_end(hspi, true); +} + +/** + * @brief Callback of the vendor HAL for a transfer that ended in a bus or DMA error. + * + * @param hspi Handle of the bus. + */ +// NOLINTNEXTLINE(readability-identifier-naming) the name is fixed by the vendor HAL +void HAL_SPI_ErrorCallback(SPI_HandleTypeDef* hspi) { + micras::hal::Spi::on_transfer_end(hspi, false); +} +} + namespace micras::hal { +std::array Spi::transferring{}; + Spi::Spi(const Config& config) : handle{config.handle}, cs_gpio{config.cs_gpio}, @@ -24,6 +50,15 @@ Spi::Spi(const Config& config) : this->initialized = this->handle->State == HAL_SPI_STATE_READY; } +Spi::~Spi() { + if (this->transfer == Transfer::RUNNING) { + HAL_SPI_Abort(this->handle); + this->unselect_device(); + } + + std::ranges::replace(transferring, this, static_cast(nullptr)); +} + bool Spi::select_device() { const uint32_t start = Timer::get_counter(); const uint32_t limit = Timer::to_cycles(1000 * this->timeout); @@ -73,6 +108,45 @@ bool Spi::transmit_receive(std::span transmitted, std::spanhandle, buffer, received.data(), transmitted.size(), this->timeout) == HAL_OK; } +bool Spi::start_transfer(std::span transmitted, std::span received) { + auto* const slot = std::ranges::find(transferring, nullptr); + + if (received.size() < transmitted.size() or slot == transferring.end() or not this->select_device()) { + this->transfer = Transfer::FAILED; + return false; + } + + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-const-cast) the HAL takes a mutable pointer it only reads + auto* buffer = const_cast(transmitted.data()); + + *slot = this; + this->transfer = Transfer::RUNNING; + + if (HAL_SPI_TransmitReceive_DMA(this->handle, buffer, received.data(), transmitted.size()) != HAL_OK) { + *slot = nullptr; + this->transfer = Transfer::FAILED; + this->unselect_device(); + return false; + } + + return true; +} + +Spi::Transfer Spi::get_transfer() const { + return this->transfer; +} + +void Spi::on_transfer_end(const SPI_HandleTypeDef* handle, bool succeeded) { + for (Spi*& device : transferring) { + if (device != nullptr and device->handle == handle) { + device->unselect_device(); + device->transfer = succeeded ? Transfer::COMPLETE : Transfer::FAILED; + device = nullptr; + return; + } + } +} + bool Spi::was_initialized() const { return this->initialized; } From 87b20247b9cbaa4701aa53b9acc859f58eeaa0d4 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 04:35:58 -0300 Subject: [PATCH 500/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Read=20the=20inert?= =?UTF-8?q?ial=20measurement=20unit=20in=20one=20burst=20that=20the=20DMA?= =?UTF-8?q?=20carries=20out?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit An update asked the sensor whether it had data, then read the angular rate, then the acceleration: three blocking transactions, about 50 us. The status, the angular rate and the acceleration are contiguous registers, so one transfer of 17 bytes reads them all, and the DMA carries it out between two updates. An update takes the sample the last one asked for and asks for the next, which costs the time to start a transfer, and tells whether the angular rate it found is new. The sensor runs at 8 kHz, the rate of the control loop, in its high accuracy mode, where the data rates are round numbers that hold within 1 % instead of following the oscillator of each part. The bench test runs at the rate of the control loop and counts the new samples it gets, which is how a sensor slower than it should be, or a transfer that does not fit in an iteration, shows. --- micras_proxy/include/micras/proxy/imu.hpp | 71 ++++++++++++++++++++++- micras_proxy/src/imu.cpp | 54 ++++++++++++----- 2 files changed, 110 insertions(+), 15 deletions(-) diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index ca96093..a193929 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -16,14 +16,22 @@ namespace micras::proxy { /** * @brief Class for acquiring IMU data. + * + * @note The samples are read by the DMA into a buffer of this object, which therefore has to live in + * memory that the DMA reaches. */ class Imu { public: /** * @brief IMU configuration struct. + * + * @note The data rates of the high accuracy mode, which are the ones that are round numbers, + * exist only with both sensors in that mode. */ struct Config { hal::Spi::Config spi; + lsm6dsv_gy_mode_t gyroscope_mode; + lsm6dsv_xl_mode_t accelerometer_mode; lsm6dsv_data_rate_t gyroscope_data_rate; lsm6dsv_data_rate_t accelerometer_data_rate; lsm6dsv_gy_full_scale_t gyroscope_scale; @@ -50,10 +58,25 @@ class Imu { explicit Imu(const Config& config); /** - * @brief Update the IMU data. + * @brief Take the sample that the last call asked for, and ask for the next one. + * + * @details The status, the angular rate and the acceleration are read in one transfer that the + * DMA carries out while the caller does something else, so a call costs the time to start a + * transfer and never the time the transfer takes. The price is that a sample is one call old + * when it is used. */ void update(); + /** + * @brief Check whether the last update brought an angular rate that was not seen before. + * + * @note The sensor samples on its own clock, so a caller running faster than the output data + * rate, or close to it, gets the same sample more than once. + * + * @return True if the angular rate is a new sample, false otherwise. + */ + bool is_new() const; + /** * @brief Get the IMU angular velocity over an axis. * @@ -90,6 +113,11 @@ class Imu { */ bool check_whoami(); + /** + * @brief Convert the registers that the last transfer brought. + */ + void read_response(); + /** * @brief Read data from the IMU. * @@ -112,6 +140,32 @@ class Imu { */ static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp, uint16_t len); + /** + * @brief Bit of the register address that makes a transfer a read. + */ + static constexpr uint8_t read_flag{0x80}; + + /** + * @brief Number of registers from the status to the last byte of the acceleration. + */ + static constexpr uint8_t burst_size{16}; + + /** + * @brief Offsets of the angular rate and of the acceleration from the status register. + */ + ///@{ + static constexpr uint8_t angular_rate_offset{4}; + static constexpr uint8_t acceleration_offset{10}; + ///@} + + /** + * @brief Bits of the status register that tell which sensor has a new sample. + */ + ///@{ + static constexpr uint8_t accelerometer_ready{0x01}; + static constexpr uint8_t gyroscope_ready{0x02}; + ///@} + /** * @brief Conversion constants. */ @@ -128,6 +182,16 @@ class Imu { */ stmdev_ctx_t dev_ctx{}; + /** + * @brief Address of the status register, followed by the padding that clocks the registers out. + */ + std::array command{LSM6DSV_STATUS_REG | read_flag}; + + /** + * @brief Registers received by the last transfer, after the byte that answers the address. + */ + std::array response{}; + /** * @brief Current angular velocity on each axis. */ @@ -153,6 +217,11 @@ class Imu { */ core::ButterworthFilter calibration_filter; + /** + * @brief Flag to check if the last update brought a new angular rate. + */ + bool fresh{}; + /** * @brief Flag to check if the IMU was calibrated. */ diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index e70e109..ad20f6c 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -33,6 +33,9 @@ Imu::Imu(const Config& config) : lsm6dsv_sw_por(&(this->dev_ctx)); lsm6dsv_block_data_update_set(&(this->dev_ctx), PROPERTY_ENABLE); + lsm6dsv_gy_mode_set(&(this->dev_ctx), config.gyroscope_mode); + lsm6dsv_xl_mode_set(&(this->dev_ctx), config.accelerometer_mode); + lsm6dsv_gy_data_rate_set(&(this->dev_ctx), config.gyroscope_data_rate); lsm6dsv_xl_data_rate_set(&(this->dev_ctx), config.accelerometer_data_rate); @@ -57,30 +60,53 @@ bool Imu::check_whoami() { } void Imu::update() { - std::array raw_data{}; + const hal::Spi::Transfer transfer = this->spi.get_transfer(); + this->fresh = false; - lsm6dsv_data_ready_t data_ready{}; - lsm6dsv_flag_data_ready_get(&this->dev_ctx, &data_ready); + if (transfer == hal::Spi::Transfer::RUNNING) { + return; + } - if (data_ready.drdy_xl) { - lsm6dsv_acceleration_raw_get(&dev_ctx, raw_data.data()); - std::get<0>(this->linear_acceleration) = std::get<0>(raw_data) * xl_factor; - std::get<1>(this->linear_acceleration) = std::get<1>(raw_data) * xl_factor; - std::get<2>(this->linear_acceleration) = std::get<2>(raw_data) * xl_factor; + if (transfer == hal::Spi::Transfer::COMPLETE) { + this->read_response(); } - if (data_ready.drdy_gy) { - lsm6dsv_angular_rate_raw_get(&dev_ctx, raw_data.data()); - std::get<0>(this->angular_velocity) = std::get<0>(raw_data) * gy_factor; - std::get<1>(this->angular_velocity) = std::get<1>(raw_data) * gy_factor; - std::get<2>(this->angular_velocity) = std::get<2>(raw_data) * gy_factor; + this->spi.start_transfer(this->command, this->response); +} + +void Imu::read_response() { + const auto get_axis = [this](uint8_t offset, uint8_t axis) { + const uint8_t index = 1 + offset + 2 * axis; + return static_cast(this->response.at(index) | (this->response.at(index + 1) << 8)); + }; + + const uint8_t status = std::get<1>(this->response); + + if ((status & accelerometer_ready) != 0) { + for (uint8_t axis = 0; axis < 3; axis++) { + this->linear_acceleration.at(axis) = get_axis(acceleration_offset, axis) * this->xl_factor; + } } + if ((status & gyroscope_ready) == 0) { + return; + } + + for (uint8_t axis = 0; axis < 3; axis++) { + this->angular_velocity.at(axis) = get_axis(angular_rate_offset, axis) * this->gy_factor; + } + + this->fresh = true; + if (not this->calibrated) { this->calibration_filter.update(std::get<2>(this->angular_velocity)); } } +bool Imu::is_new() const { + return this->fresh; +} + float Imu::get_angular_velocity(Axis axis) const { switch (axis) { case Axis::X: @@ -120,7 +146,7 @@ int32_t Imu::platform_read(void* handle, uint8_t reg, uint8_t* bufp, uint16_t le return -1; } - reg |= 0x80; + reg |= read_flag; const bool transferred = spi->transmit({®, 1}) and spi->receive({bufp, len}); spi->unselect_device(); From 6251e461b03a20f214f20636d891f194b6c1026a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 04:37:59 -0300 Subject: [PATCH 501/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Fire=20the=20wall?= =?UTF-8?q?=20sensor=20emitters=20in=20two=20groups,=20twice=20as=20often?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The four emitters were on together for half of a 1 ms period, and the readings were taken straight from the buffer the DMA keeps rewriting. They now fire in two groups, one centered on each of the two scans of a period, so every scan reads two sensors lit and two dark, and neither the two sensors that look forward nor the two of each side light the same patch of wall at once. The readings come from the copy the converter keeps of every whole pair of scans, and an update tells whether it brought a new one, so that the filter runs at the rate of the emitters whatever the rate of the caller. The period becomes 500 us, four periods of the control loop. What sets it is the phototransistor, which takes some tens of microseconds to follow its emitter on a 1 kOhm load: an emitter is on for 75 us before the scan of its group starts and for the 75 us that the scan has to fit in, and off for 175 us before it is read dark. The scan takes 66 us, with 8 samples of 32.5 cycles per channel instead of 16 of 64.5, at the 20 MHz the converter really runs at: the kernel clock is divided by two after the prescaler, so the old scan took 234 us of the 250 us it had. A duty cycle of 30 % also brings the series resistors of the emitters, 100 mW parts that dissipated 140 mW, inside their rating. Since the rate of the emitters lives in the peripheral configuration and the one of the filter in the constants, the proxy compares the two when it starts, and fails its initialization the day only one of them changes. --- micras_hal/include/micras/hal/pwm.hpp | 10 +++ micras_hal/stm32/src/pwm.cpp | 9 +++ .../micras/proxy/impl/wall_sensors.tpp | 31 ++++++-- .../include/micras/proxy/wall_sensors.hpp | 71 +++++++++++++++---- 4 files changed, 103 insertions(+), 18 deletions(-) diff --git a/micras_hal/include/micras/hal/pwm.hpp b/micras_hal/include/micras/hal/pwm.hpp index 7830c83..87c00ea 100644 --- a/micras_hal/include/micras/hal/pwm.hpp +++ b/micras_hal/include/micras/hal/pwm.hpp @@ -61,6 +61,16 @@ class Pwm { */ void set_frequency(uint32_t frequency); + /** + * @brief Get the frequency of the PWM signal from the registers of the timer. + * + * @note A timer that counts up and down takes twice its autoreload value to complete a period, + * where one that counts in a single direction takes the autoreload value plus one. + * + * @return Frequency value in Hz. + */ + float get_frequency() const; + /** * @brief Check if the PWM was successfully started. * diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index 0575eb0..c4b7869 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -97,6 +97,15 @@ void Pwm::set_frequency(uint32_t frequency) { __HAL_TIM_SET_COUNTER(this->handle, 0); } +float Pwm::get_frequency() const { + const uint32_t autoreload = __HAL_TIM_GET_AUTORELOAD(this->handle); + const bool center_aligned = (this->handle->Instance->CR1 & TIM_CR1_CMS) != 0; + const uint32_t period = center_aligned ? 2 * autoreload : autoreload + 1; + const auto ticks = static_cast(this->handle->Instance->PSC + 1) * static_cast(period); + + return static_cast(get_timer_clock_frequency(this->handle->Instance)) / ticks; +} + bool Pwm::was_initialized() const { return this->initialized; } diff --git a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp index 68e660f..6ba12e8 100644 --- a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp +++ b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp @@ -5,6 +5,8 @@ #ifndef MICRAS_PROXY_WALL_SENSORS_TPP #define MICRAS_PROXY_WALL_SENSORS_TPP +#include +#include #include #include @@ -17,12 +19,17 @@ template TWallSensors::TWallSensors(const Config& config) : adc{config.adc}, led_pwms{core::make_array(config.led_pwms)}, + emitter_duty_cycle{config.emitter_duty_cycle}, filters{core::make_array(config.filter)}, base_readings{config.base_readings}, uncertainty{config.uncertainty}, initialized{ - this->adc.start_dma(this->buffer) and this->adc.was_initialized() and - config.adc.handle->Init.NbrOfConversion == num_of_sensors + this->adc.start_dma(this->buffer, this->snapshot) and this->adc.was_initialized() and + config.adc.handle->Init.NbrOfConversion == num_of_sensors and + std::ranges::all_of(this->led_pwms, [&config](const hal::Pwm& led_pwm) { + return led_pwm.was_initialized() and std::abs(led_pwm.get_frequency() - config.filter.sampling_frequency) < + frequency_tolerance * config.filter.sampling_frequency; + }) } { this->turn_off(); } @@ -30,7 +37,7 @@ TWallSensors::TWallSensors(const Config& config) : template void TWallSensors::turn_on() { for (auto& led_pwm : this->led_pwms) { - led_pwm.set_duty_cycle(50.0F); + led_pwm.set_duty_cycle(this->emitter_duty_cycle); } } @@ -43,11 +50,25 @@ void TWallSensors::turn_off() { template void TWallSensors::update() { + const uint32_t current_sequence = this->adc.read_snapshot(this->scans); + + this->fresh = current_sequence != this->sequence; + this->sequence = current_sequence; + + if (not this->fresh) { + return; + } + for (uint8_t i = 0; i < num_of_sensors; i++) { this->filters.at(i).update(this->get_adc_reading(i)); } } +template +bool TWallSensors::is_new() const { + return this->fresh; +} + template bool TWallSensors::get_wall(uint8_t sensor_index, bool disturbed) const { return this->filters.at(sensor_index).get_last() > @@ -61,9 +82,7 @@ float TWallSensors::get_reading(uint8_t sensor_index) const { template float TWallSensors::get_adc_reading(uint8_t sensor_index) const { - return static_cast( - std::abs(this->buffer.at(sensor_index) - this->buffer.at(sensor_index + num_of_sensors)) - ) / + return static_cast(std::abs(this->scans.at(sensor_index) - this->scans.at(sensor_index + num_of_sensors))) / this->adc.get_max_reading(); } diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 075a6b3..52e2425 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -25,6 +25,7 @@ class TWallSensors { struct Config { hal::AdcDma::Config adc; std::array led_pwms; + float emitter_duty_cycle; core::ButterworthFilter::Config filter; std::array base_readings; float uncertainty; @@ -48,10 +49,19 @@ class TWallSensors { void turn_off(); /** - * @brief Update the wall sensors readings. + * @brief Update the wall sensors readings, if the emitters completed a period since the last call. */ void update(); + /** + * @brief Check whether the last update brought readings that were not seen before. + * + * @note The readings come at the rate of the emitter timer, whatever the rate of the caller. + * + * @return True if the readings are new, false otherwise. + */ + bool is_new() const; + /** * @brief Get the observation from a sensor. * @@ -91,13 +101,22 @@ class TWallSensors { void calibrate_sensor(uint8_t sensor_index); /** - * @brief Check if the ADC was initialized and its scan matches the buffer layout. + * @brief Check if the ADC was initialized, its scan matches the buffer layout, and the emitters + * run at the rate the filter was designed for. + * + * @note The rate of the emitters lives in the peripheral configuration and the one of the filter + * in the constants, and nothing else would notice the day only one of them changes. * * @return True if the initialization was successful, false otherwise. */ bool was_initialized() const; private: + /** + * @brief Largest relative difference between the rate of the emitters and the one of the filter. + */ + static constexpr float frequency_tolerance{0.01F}; + /** * @brief ADC DMA handle. */ @@ -109,21 +128,49 @@ class TWallSensors { std::array led_pwms; /** - * @brief Buffer to store the ADC values, holding one emitter on and one emitter off scan. + * @brief Duty cycle of the emitters while the sensors are on. + */ + float emitter_duty_cycle; + + /** + * @brief Buffer the DMA writes to, holding one emitter on and one emitter off scan. * * @details The emitter PWM timer is center aligned and triggers the ADC on its update event, so - * the conversion sequence runs twice per emitter period: once at the underflow, while the - * emitters are on, and once at the overflow, while they are off. Two consecutive scans - * therefore fill the two halves of this buffer with a matching pair, and the difference between - * the halves is the reflected signal with the ambient light canceled. Taking the absolute - * value makes the result independent of which half currently holds which phase. + * the conversion sequence runs twice per emitter period: once at the underflow and once at the + * overflow. The emitters fire in two groups, one centered on each of those instants, so every + * scan reads half of the sensors lit and the other half dark, no sensor is ever lit by the + * emitter of its neighbor, and two consecutive scans hold a lit and a dark reading of every + * sensor. Their difference is the reflected signal with the ambient light canceled, and taking + * its absolute value makes the result independent of which half currently holds which scan. * * @note This depends on the ADC scanning exactly num_of_sensors channels, on the emitter timer - * being center aligned with its trigger on the update event, and on a scan fitting inside half - * an emitter period. The first of those is checked by the constructor; the other two live in the - * peripheral configuration. + * being center aligned with its trigger on the update event, on the two groups being told + * apart by the inverted flag of their PWM configuration, and on the emitter being on for at + * least the settling time of the receiver before the scan starts and the duration of the scan + * after it. The first of those is checked by the constructor; the others live in the + * configuration. + */ + std::array buffer{}; + + /** + * @brief Copy of the buffer taken when a pair of scans completes, so that a pair is never torn. + */ + std::array snapshot{}; + + /** + * @brief The pair of scans the readings are computed from. + */ + std::array scans{}; + + /** + * @brief Number of pairs of scans completed when the readings were last computed. + */ + uint32_t sequence{}; + + /** + * @brief Flag to check if the last update brought new readings. */ - std::array buffer; + bool fresh{}; /** * @brief Butterworth filter for the ADC readings. From cab4f77856025d5523ce2772403a56ed0df542ac Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 20 Sep 2026 06:18:03 -0300 Subject: [PATCH 502/561] =?UTF-8?q?=F0=9F=93=9D=20Say=20what=20the=20filte?= =?UTF-8?q?r=20math=20accelerator=20can=20really=20hold?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The notes said that the accelerator holds one filter configuration at a time, so that a second filter means reprogramming its coefficients. Its reference manual says otherwise: the local memory holds the coefficients of several filters, chosen by a base address, and signals that share a cutoff can be interleaved through one filter. What it runs one at a time is a function, and what it loses across a change is the history, which has to be loaded again. The conclusion for filters that run sample by sample does not change, but the reason given was not the real one. The cutoffs quoted for the range of the fixed point format were also those of a sampling rate the robot no longer has. --- micras_hal/include/micras/hal/fmac.hpp | 11 ++++++++--- .../include/micras/proxy/fmac_filter.hpp | 16 +++++++++------- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/micras_hal/include/micras/hal/fmac.hpp b/micras_hal/include/micras/hal/fmac.hpp index d8d6687..2e42757 100644 --- a/micras_hal/include/micras/hal/fmac.hpp +++ b/micras_hal/include/micras/hal/fmac.hpp @@ -15,9 +15,14 @@ namespace micras::hal { /** * @brief Class to handle the filter math accelerator on STM32 microcontrollers. * - * @note The accelerator works in q1.15 fixed point and holds one filter configuration at a time, - * so a second filter cannot run without reprogramming the coefficient buffer. Both limits are - * properties of the peripheral, not of this wrapper. + * @note The accelerator works in q1.15 fixed point and runs one function at a time. Its local + * memory of 256 words can hold the coefficients of several filters, chosen by the base address of + * the coefficient buffer, and N signals that share a cutoff can be interleaved through one filter + * whose coefficients are spaced N taps apart. What it does not keep is the history of a filter + * across a change of configuration: stopping it resets the pointers into its buffers, so the last + * inputs and outputs have to be loaded again, which for one sample at a time costs more than the + * filter does. This wrapper uses none of that. It configures one filter and runs it sample by + * sample. */ class Fmac { public: diff --git a/micras_proxy/include/micras/proxy/fmac_filter.hpp b/micras_proxy/include/micras/proxy/fmac_filter.hpp index 7421293..6671174 100644 --- a/micras_proxy/include/micras/proxy/fmac_filter.hpp +++ b/micras_proxy/include/micras/proxy/fmac_filter.hpp @@ -21,16 +21,18 @@ namespace micras::proxy { * * - The accelerator works in q1.15, so every coefficient has to fall inside [-1, 1) and be large * enough not to quantize to zero. A second order Butterworth has a feed forward coefficient of - * roughly (fc / fs)^2 / 4, so at a cutoff far below the sampling rate the coefficients underflow. - * At a 960 Hz sampling rate the useful range starts around a 100 Hz cutoff, well above the 5 Hz - * and 10 Hz cutoffs the sensors of this robot use. was_initialized reports that refusal instead - * of filtering with silently truncated coefficients. + * roughly (pi * fc / fs)^2 and a feedback coefficient close to -2, so the useful range starts + * around a cutoff of a tenth of the sampling rate. Halving the coefficients and doubling the + * output with the gain of the accelerator, which this class does not do, brings that to about a + * fiftieth, still well above what the sensors of this robot use. was_initialized reports that + * refusal instead of filtering with silently truncated coefficients. * - Samples and results are q1.15 too, so the input has to be normalized to [-1, 1). That suits a * ratiometric reading such as an ADC fraction, and not a physical quantity such as a rate in * radians per second. - * - The accelerator holds one filter configuration at a time. Several instances cannot run - * concurrently without reprogramming the coefficient buffer between samples, which costs far - * more than the handful of multiply accumulates it would save. + * - The accelerator runs one function at a time, so one instance of this class owns it. Several + * filters would have to share a cutoff and be interleaved through one set of coefficients spaced + * apart, or have their history loaded again at every switch, which costs far more than the + * handful of multiply accumulates it would save. Neither is what this class does. */ class FmacFilter { public: From af0535773691e314ef3ccbddac15b105d10039a8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 25 Sep 2026 20:27:29 -0300 Subject: [PATCH 503/561] =?UTF-8?q?=F0=9F=90=9B=20Correct=20the=20sensing,?= =?UTF-8?q?=20the=20estimate=20and=20the=20control=20the=20simulation=20fo?= =?UTF-8?q?und=20wrong?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Each of these is an error the simulation demonstrated and that would affect the robot the same way; targets/micras/FIRMWARE_CHANGES.md in the simulator has the evidence for each. Sensing. The wall sensors model the receiver beside the emitter, the parallax of the front ones and the angle a range meets the wall at, which along a diagonal read the walls 16 % short. The wall model starts at 35 mm and the localizer only corrects with ranges out to 120 mm, where the floor starts to lengthen them. The fan reports how much of its speed it has reached, and the run waits for it. Estimate. The edge tracker times a wall edge by the reading and only names it from the map, instead of losing it whenever the estimate ran ahead. The odometry rolls on a radius the load flattens, since with the fan the tires carry four times the load. The tires slide to the outside of a curve, 4.8 mm/s per m/s^2 of lateral acceleration in the simulation, which neither the wheels nor the gyroscope see: the prediction now slides the pose by that much. Control. The time scale settles instead of alternating between one and a fraction, and the controller points the robot into a curve by the angle at which it moves along the path while sliding, instead of letting an error across the path build up to turn it there. The physical constants of robot.hpp follow the board, the CAD and the datasheets, with the lateral compliance of the tires as a new one, still to be measured. --- micras_hal/stm32/src/timer.cpp | 2 +- micras_nav/include/micras/nav/controller.hpp | 11 +- .../include/micras/nav/impl/localizer.tpp | 114 +++++++++++------- micras_nav/include/micras/nav/localizer.hpp | 68 +++++++++-- micras_nav/include/micras/nav/robot_model.hpp | 36 +++++- micras_nav/include/micras/nav/wall_model.hpp | 21 +++- micras_nav/src/controller.cpp | 11 +- micras_nav/src/localizer.cpp | 14 ++- micras_nav/src/wall_model.cpp | 12 +- micras_proxy/include/micras/proxy/fan.hpp | 7 ++ .../micras/proxy/impl/wall_sensors.tpp | 65 +++++++++- .../include/micras/proxy/wall_sensors.hpp | 60 ++++++++- micras_proxy/src/fan.cpp | 4 + 13 files changed, 352 insertions(+), 73 deletions(-) diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index 7d9bde4..dfa2878 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -3,8 +3,8 @@ */ #include - #include + #include "micras/hal/timer.hpp" namespace micras::hal { diff --git a/micras_nav/include/micras/nav/controller.hpp b/micras_nav/include/micras/nav/controller.hpp index 9ec648e..367772f 100644 --- a/micras_nav/include/micras/nav/controller.hpp +++ b/micras_nav/include/micras/nav/controller.hpp @@ -27,6 +27,12 @@ namespace micras::nav { * proportional to the error across the path, which turns the robot back towards it at a rate set by * the distance traveled rather than by time. It works wherever the pose is known, walls or not. * + * In a curve the tires slide to the outside at a speed the lateral compliance of the model gives, + * so the robot has to point into the curve by that speed over its own, which is the compliance + * times the angular speed of the reference, to move along the path. That angle is added to the + * orientation the feedback aims at, and its rate to the angular speed, instead of being left for + * an error across the path to build up. + * * @note The gains are not tuned: they follow from the model of the drive train and from the natural * frequency and damping asked of each axis. */ @@ -106,7 +112,10 @@ class Controller { * @note The feed forward alone may ask for more voltage than there is. Slowing the clock of * the reference scales its linear and angular speeds together, so the robot stays on the same * path and only takes longer, instead of cutting the turn it is in. With the limits of a run - * derived from the same model this should stay at one. + * derived from the same model this should stay at one. The reference this is computed from was + * already played at the scale in force, so the scale is corrected from that one rather than + * computed afresh, and settles where the demand meets the voltage available instead of + * alternating between one and a fraction every iteration. * * @return The factor to apply to the elapsed time of the reference, in (0, 1]. */ diff --git a/micras_nav/include/micras/nav/impl/localizer.tpp b/micras_nav/include/micras/nav/impl/localizer.tpp index 32fcf3a..662dee0 100644 --- a/micras_nav/include/micras/nav/impl/localizer.tpp +++ b/micras_nav/include/micras/nav/impl/localizer.tpp @@ -7,6 +7,7 @@ #include #include +#include #include "micras/nav/maze.hpp" #include "micras/nav/measurements.hpp" @@ -21,7 +22,7 @@ void Localizer::correct( ) { if (std::abs(this->state.velocity.angular) > this->config.max_angular_speed) { for (EdgeTracker& tracker : this->edge_trackers) { - tracker.tracking = false; + tracker.locked = false; } return; @@ -47,7 +48,7 @@ void Localizer::correct( this->track_edge(sensor, reading, sampled, hit, wall_model, maze); } - if (not reading.valid or not hit.valid or hit.state != WallState::WALL or + if (not reading.valid or not hit.valid or hit.state != WallState::WALL or hit.range > this->config.max_range or not wall_model.is_footprint_clear( hit, sensor, this->get_position_deviation(), this->get_orientation_deviation() )) { @@ -57,7 +58,8 @@ void Localizer::correct( const float deviation = wall_model.get_range_deviation(hit.range); this->update( - reading.distance - hit.range, {hit.jacobian.at(0), hit.jacobian.at(1), hit.jacobian.at(2), 0.0F}, + wall_model.get_range(reading.distance, sensor, hit) - hit.range, + {hit.jacobian.at(0), hit.jacobian.at(1), hit.jacobian.at(2), 0.0F}, deviation * deviation * this->config.range_correlation, gate, this->config.max_position_correction ); } @@ -70,64 +72,92 @@ void Localizer::track_edge( ) { EdgeTracker& tracker = this->edge_trackers.at(sensor); - const auto is_on = [this, &reading](float range) { - return reading.valid and std::abs(reading.distance - range) < - this->config.range_tolerance + this->config.relative_range_tolerance * range; + const auto tolerance = [this](float range) { + return this->config.range_tolerance + this->config.relative_range_tolerance * range; }; - const bool on_wall = - hit.valid and hit.state == WallState::WALL and hit.range < this->config.edge_range and is_on(hit.range); - const bool was_tracking = tracker.tracking; - const bool was_on_wall = tracker.on_wall; + if (this->state.velocity.linear <= this->config.edge_speed) { + tracker.locked = false; + return; + } - const RayHit previous = tracker.hit; + if (tracker.locked) { + const PlaneCrossing crossing = wall_model.cross(sampled, sensor, tracker.hit); + const bool reading_on = reading.valid and reading.distance < crossing.range + tolerance(crossing.range); + + if (crossing.range > this->config.edge_range) { + tracker.locked = false; + } else if (reading_on != tracker.reading_on) { + tracker.reading_on = reading_on; + + const float heading = this->state.pose.orientation; + const float travel = + this->state.velocity.linear * (tracker.hit.vertical ? std::sin(heading) : std::cos(heading)); + const float face = this->config.model.maze.wall_thickness * std::abs(crossing.slope); + const std::optional edge = this->find_edge(maze, tracker.hit.wall, travel > 0.0F, reading_on, face); + + if (edge.has_value() and std::abs(*edge - crossing.offset) <= this->config.edge_window and + this->update( + *edge - crossing.offset, + {crossing.jacobian.at(0), crossing.jacobian.at(1), crossing.jacobian.at(2), 0.0F}, + this->config.edge_deviation * this->config.edge_deviation, this->config.gate, + this->config.max_edge_correction + )) { + this->status.edges++; + } + } + } - tracker.tracking = this->state.velocity.linear > this->config.edge_speed; - tracker.on_wall = on_wall; + const bool on_wall = hit.valid and hit.state == WallState::WALL and hit.range < this->config.edge_range and + std::abs(reading.distance - hit.range) < tolerance(hit.range) and reading.valid; if (on_wall) { + tracker.locked = true; + tracker.reading_on = true; tracker.hit = hit; } +} - if (not was_tracking or not tracker.tracking or was_on_wall == on_wall) { - return; - } - - const RayHit& wall = on_wall ? hit : previous; +template +std::optional Localizer::find_edge( + const TMaze& maze, const GridPose& wall, bool upwards, bool start, float face +) const { + const bool vertical = wall.orientation == Side::LEFT or wall.orientation == Side::RIGHT; + const float cell_size = this->config.model.maze.cell_size; + const float half_wall = this->config.model.maze.wall_thickness / 2.0F; + const float sign = upwards ? 1.0F : -1.0F; - const PlaneCrossing crossing = wall_model.cross(sampled, sensor, wall); + Side step = upwards ? Side::UP : Side::DOWN; - if (not on_wall and (is_on(crossing.range) or (reading.valid and reading.distance < crossing.range))) { - return; + if (not vertical) { + step = upwards ? Side::RIGHT : Side::LEFT; } - const float heading = this->state.pose.orientation; - const float travel = this->state.velocity.linear * (wall.vertical ? std::sin(heading) : std::cos(heading)); - const bool leaving_upwards = (travel > 0.0F) == (not on_wall); + GridPose segment = wall; + bool in_gap = false; - const Side towards = - wall.vertical ? (leaving_upwards ? Side::UP : Side::DOWN) : (leaving_upwards ? Side::RIGHT : Side::LEFT); - const GridPose beyond{.position = wall.wall.position + towards, .orientation = wall.wall.orientation}; + for (uint8_t shift = 1; shift <= edge_search_cells; shift++) { + segment.position = segment.position + step; - if (maze.get_wall(beyond) == WallState::WALL) { - return; - } + const WallState state = + TMaze::contains(segment.position) ? maze.get_wall(segment) : WallState::UNKNOWN; - const float half_wall = this->config.model.maze.wall_thickness / 2.0F; - const float edge = leaving_upwards ? this->config.model.maze.cell_size + half_wall : -half_wall; - const float innovation = edge - crossing.offset; + if (state == WallState::UNKNOWN) { + return std::nullopt; + } - if (std::abs(innovation) > this->config.edge_window) { - return; - } + const bool open = state == WallState::NO_WALL; + in_gap = in_gap or open; + + if (start ? (in_gap and not open) : open) { + const auto cells = static_cast(shift); + const float boundary = (upwards ? cells : 1.0F - cells) * cell_size; - if (this->update( - innovation, {crossing.jacobian.at(0), crossing.jacobian.at(1), crossing.jacobian.at(2), 0.0F}, - this->config.edge_deviation * this->config.edge_deviation, this->config.gate, - this->config.max_edge_correction - )) { - this->status.edges++; + return start ? boundary - sign * (half_wall + face) : boundary + sign * half_wall; + } } + + return std::nullopt; } } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/localizer.hpp b/micras_nav/include/micras/nav/localizer.hpp index 05cacef..fc3c1b2 100644 --- a/micras_nav/include/micras/nav/localizer.hpp +++ b/micras_nav/include/micras/nav/localizer.hpp @@ -7,6 +7,7 @@ #include #include +#include #include "micras/nav/maze.hpp" #include "micras/nav/measurements.hpp" @@ -58,7 +59,10 @@ class Localizer { * share it: the variance of each is multiplied by it, so that what a second of ranges is worth * does not depend on how many there are in it. The speed window is the number of iterations the linear * speed is measured over, which trades noise for delay. The rest window is the time over which - * the gyroscope is compared with the encoders to measure its bias. + * the gyroscope is compared with the encoders to measure its bias. The maximum range is the + * longest range a reading corrects the pose from: past it the beam of the emitter grows wide + * enough to land partly on the floor, the reading loses light the model of the sensor does not + * know about, and the range comes out long by several percent. * * The ends of the side walls are used as references along the path when use_edges is set. An * end is accepted while the robot moves forward faster than the edge speed, since the only time @@ -82,6 +86,7 @@ class Localizer { float stationary_angular_speed; float range_delay; float range_correlation; + float max_range; float rest_window; bool use_edges; float edge_deviation; @@ -125,11 +130,22 @@ class Localizer { /** * @brief Advance the estimate by one iteration. * + * @note The wheels and the gyroscope give the motion along the heading. The tires also slide to + * the outside of a curve, in proportion to the lateral acceleration, which neither of them sees, + * so that slide comes from the lateral compliance of the model. + * * @param measurements The current measurements. * @param elapsed_time Time since the last iteration, in seconds. */ void predict(const Measurements& measurements, float elapsed_time); + /** + * @brief Set how much of the fan downforce presses the tires, which sets the radius they roll on. + * + * @param downforce The share of the fan downforce, from 0 with the fan off to 1. + */ + void set_downforce(float downforce); + /** * @brief Correct the estimate with the ranges of the wall sensors that can be trusted. * @@ -217,6 +233,11 @@ class Localizer { */ using Matrix = std::array; + /** + * @brief Number of walls a line is followed over to find an edge. + */ + static constexpr uint8_t edge_search_cells{4}; + /** * @brief Indexes of the states. */ @@ -229,19 +250,30 @@ class Localizer { ///@} /** - * @brief What a side looking sensor was seeing at its previous sample. + * @brief The line of walls a side looking sensor follows, and what its reading last said of it. * - * @note The end of a wall shows up as a change between two consecutive samples, from a reading - * on the wall to a reading past it or the other way around, so both are remembered. + * @note A sensor locks on a line once its axis, cast from the estimated pose, meets a wall of it + * and the reading agrees. From then on the reading is compared with the plane of that line, and + * whether it is on the plane or past it is remembered, until the robot stops or turns. */ struct EdgeTracker { - bool tracking; - bool on_wall; + bool locked; + bool reading_on; RayHit hit; }; /** - * @brief Look for the end of a side wall in the reading of a sensor, and use it as a reference. + * @brief Look for the end or the start of a side wall in the reading of a sensor, and use it as + * a reference along the path. + * + * @note Only a change of the reading places an edge, and the map only says which edge it is: + * from the wall the sensor is locked on, the first end of the line of walls ahead, or the first + * start after a gap, in the direction the axis moves along it. So an edge is found whether the + * estimate is ahead of the robot or behind it. The reading changes where half of the spot is on + * the wall, with the axis of the sensor on the edge itself. Toward the start of a wall the beam, + * which always points ahead, also lights the end face of the wall, so the start is seen early, by + * the thickness of the wall times the slope of the axis to it. Leaving a wall that face is in the + * shade. An edge the map does not know, because a wall on the way is unknown, is not used. * * @tparam width The width of the maze in cells. * @tparam height The height of the maze in cells. @@ -258,6 +290,23 @@ class Localizer { const TMaze& maze ); + /** + * @brief Find the edge ahead along a line of walls. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + * @param maze The map of the walls. + * @param wall A known wall of the line. + * @param upwards Whether to look toward higher coordinates along the line. + * @param start Whether to look for the start of a wall after a gap, rather than for an end. + * @param face How far ahead of a start its end face is seen. + * @return The coordinate of the edge along the line, from the base of the wall, if the map + * knows every wall on the way. + */ + template + std::optional + find_edge(const TMaze& maze, const GridPose& wall, bool upwards, bool start, float face) const; + /** * @brief Set the covariance to a diagonal one. * @@ -326,6 +375,11 @@ class Localizer { */ Vector diagonal{}; + /** + * @brief Radius the wheels roll on, under the load on their tires. + */ + float rolling_radius; + /** * @brief Wheel angles at the last iteration. */ diff --git a/micras_nav/include/micras/nav/robot_model.hpp b/micras_nav/include/micras/nav/robot_model.hpp index db6fd61..1e8f85a 100644 --- a/micras_nav/include/micras/nav/robot_model.hpp +++ b/micras_nav/include/micras/nav/robot_model.hpp @@ -43,12 +43,15 @@ struct RobotModel { * @brief Mass properties and outline of the robot. * * @note The outline is the rectangle that encloses everything that can touch a wall, given by - * its half width and by how far it extends ahead of and behind the axle. + * its half width and by how far it extends ahead of and behind the axle. The rolling compliance + * is how much the radius a wheel rolls on shrinks for each newton on its tire, as the tire + * flattens under the load. */ struct Chassis { float mass; float yaw_inertia; float wheel_radius; + float rolling_compliance; float track_width; float half_width; float front_length; @@ -59,11 +62,17 @@ struct RobotModel { * @brief What limits the force the tires can transmit. * * @note The fan downforce is the extra normal force, in newtons, with the fan at the speed it - * is run at. + * is run at, and the fan offset is how far ahead of the axle it pulls, in meters. Pulling ahead + * of the axle, the fan tips the robot onto the front edge of its board, which then carries the + * share of the downforce that the lever of the offset over the front length gives it. The + * lateral compliance is how fast the tires slide sideways, to the outside of a curve, per unit + * of lateral acceleration, in m/s per m/s^2: a tire only pushes sideways by slipping a little. */ struct Traction { float friction_coefficient; float fan_downforce; + float fan_offset; + float lateral_compliance; }; /** @@ -118,6 +127,27 @@ struct RobotModel { float wall_range_per_meter; }; + /** + * @brief Get the part of the fan downforce that the tires carry. + * + * @return The downforce on the tires in newtons. + */ + constexpr float tire_downforce() const { + return this->traction.fan_downforce * (1.0F - this->traction.fan_offset / this->chassis.front_length); + } + + /** + * @brief Get the radius the wheels roll on under the load on their tires. + * + * @param downforce The share of the fan downforce there is, from 0 with the fan off to 1. + * @return The rolling radius in meters. + */ + constexpr float rolling_radius(float downforce) const { + const float load = (this->chassis.mass * gravity + downforce * this->tire_downforce()) / 2.0F; + + return this->chassis.wheel_radius - this->chassis.rolling_compliance * load; + } + /** * @brief Get the largest acceleration the tires can transmit, in any direction. * @@ -126,7 +156,7 @@ struct RobotModel { */ constexpr float traction_acceleration(bool fan_on) const { return this->traction.friction_coefficient * - (gravity + (fan_on ? this->traction.fan_downforce / this->chassis.mass : 0.0F)); + (gravity + (fan_on ? this->tire_downforce() / this->chassis.mass : 0.0F)); } /** diff --git a/micras_nav/include/micras/nav/wall_model.hpp b/micras_nav/include/micras/nav/wall_model.hpp index f99572a..0b66682 100644 --- a/micras_nav/include/micras/nav/wall_model.hpp +++ b/micras_nav/include/micras/nav/wall_model.hpp @@ -50,12 +50,14 @@ struct RayHit { * @brief Where the optical axis of a sensor crosses the plane of a wall, extended past its ends. * * @note The offset is measured along the wall like the one of a RayHit, the range is the distance - * from the sensor to the plane along the axis, and the jacobian is the derivative of the offset with + * from the sensor to the plane along the axis, the slope is how far the axis advances along the wall + * for each meter it closes on the plane, and the jacobian is the derivative of the offset with * respect to the x, y and orientation of the robot. */ struct PlaneCrossing { float offset; float range; + float slope; std::array jacobian; }; @@ -147,6 +149,23 @@ class WallModel { */ float get_centered_range(uint8_t sensor) const; + /** + * @brief Get the range to a wall from the distance a sensor reads for it. + * + * @details A wall reflects diffusely, so the light a sensor gets back from it falls with the + * cosine of the angle between the axis and the perpendicular of the wall, and a reading is turned + * into a distance as if the wall were met at the angle of the calibration: square on for a front + * sensor and at 45 degrees for a diagonal one. Met at another angle, a wall reads as one nearer + * or farther by the square root of the ratio of the two cosines. A diagonal sensor that meets a + * wall square on, as it does all along a diagonal, reads it 16 % short. + * + * @param distance The distance read by the sensor. + * @param sensor The index of the sensor. + * @param hit What the axis of the sensor meets. + * @return The range to the face of the wall along the axis. + */ + float get_range(float distance, uint8_t sensor, const RayHit& hit) const; + /** * @brief Get the standard deviation of a range reading. * diff --git a/micras_nav/src/controller.cpp b/micras_nav/src/controller.cpp index bfc0599..0030b63 100644 --- a/micras_nav/src/controller.cpp +++ b/micras_nav/src/controller.cpp @@ -38,7 +38,7 @@ Controller::Command Controller::update(const Reference& reference, const State& const float available = model.drive.supply_voltage * (1.0F - this->config.voltage_reserve); const float demand = std::abs(forward_feed_forward) + std::abs(rotation_feed_forward); - this->time_scale = demand > available ? available / demand : 1.0F; + this->time_scale = demand > 0.0F ? std::min(1.0F, this->time_scale * available / demand) : 1.0F; const Pose seen = reference.pose.relative(estimate.pose); @@ -49,15 +49,18 @@ Controller::Command Controller::update(const Reference& reference, const State& blend * std::clamp( -this->config.steering_gain * seen.position.y, -this->config.max_steering, this->config.max_steering ); - const float orientation_error = - std::clamp(-seen.orientation + steering, -this->config.angular.max_error, this->config.angular.max_error); + const float slide_angle = model.traction.lateral_compliance * reference.twist.angular; + const float slide_rate = model.traction.lateral_compliance * reference.acceleration.angular; + const float orientation_error = std::clamp( + -seen.orientation + slide_angle + steering, -this->config.angular.max_error, this->config.angular.max_error + ); const float forward_feedback = this->linear_gains.proportional * along_error + this->linear_gains.derivative * (reference.twist.linear - estimate.velocity.linear); const float rotation_feedback = this->angular_gains.proportional * orientation_error + - this->angular_gains.derivative * (reference.twist.angular - estimate.velocity.angular); + this->angular_gains.derivative * (reference.twist.angular + slide_rate - estimate.velocity.angular); this->status = { .along_error = -seen.position.x, diff --git a/micras_nav/src/localizer.cpp b/micras_nav/src/localizer.cpp index 7274952..cdd1fcf 100644 --- a/micras_nav/src/localizer.cpp +++ b/micras_nav/src/localizer.cpp @@ -14,7 +14,7 @@ #include "micras/nav/state.hpp" namespace micras::nav { -Localizer::Localizer(const Config& config) : config{config} { +Localizer::Localizer(const Config& config) : config{config}, rolling_radius{config.model.rolling_radius(0.0F)} { this->config.speed_window = std::clamp(config.speed_window, 1, max_speed_window); this->set_covariance({ @@ -47,7 +47,7 @@ void Localizer::reset(const Pose& pose, const Measurements& measurements) { } void Localizer::predict(const Measurements& measurements, float elapsed_time) { - const float radius = this->config.model.chassis.wheel_radius; + const float radius = this->rolling_radius; const float left_distance = radius * (measurements.left_wheel_angle - this->last_left_angle); const float right_distance = radius * (measurements.right_wheel_angle - this->last_right_angle); @@ -67,8 +67,10 @@ void Localizer::predict(const Measurements& measurements, float elapsed_time) { const float cosine = std::cos(heading); const float sine = std::sin(heading); - this->state.pose.position.x += chord * cosine; - this->state.pose.position.y += chord * sine; + const float slide = -this->config.model.traction.lateral_compliance * distance * rate; + + this->state.pose.position.x += chord * cosine - slide * sine; + this->state.pose.position.y += chord * sine + slide * cosine; this->state.pose.orientation = core::assert_angle(this->state.pose.orientation + rotation); this->window_distance += distance - this->distances.at(this->window_index); @@ -328,6 +330,10 @@ bool Localizer::update( return true; } +void Localizer::set_downforce(float downforce) { + this->rolling_radius = this->config.model.rolling_radius(downforce); +} + float Localizer::get_variance(uint8_t index) const { float variance = this->diagonal.at(index); diff --git a/micras_nav/src/wall_model.cpp b/micras_nav/src/wall_model.cpp index 8c953a9..d3f60d7 100644 --- a/micras_nav/src/wall_model.cpp +++ b/micras_nav/src/wall_model.cpp @@ -37,7 +37,9 @@ PlaneCrossing WallModel::cross(const Pose& pose, uint8_t sensor, const RayHit& h const float lever_y = origin.position.y - pose.position.y; const float swing = gap / (across * across); - PlaneCrossing crossing{.offset = start + gap * slope - hit.base, .range = gap / across, .jacobian = {}}; + PlaneCrossing crossing{ + .offset = start + gap * slope - hit.base, .range = gap / across, .slope = slope, .jacobian = {} + }; if (hit.vertical) { crossing.jacobian = {-slope, 1.0F, lever_x + lever_y * slope + swing}; @@ -80,6 +82,14 @@ float WallModel::get_centered_range(uint8_t sensor) const { return (face - mounting.position.x) / std::cos(mounting.angle); } +float WallModel::get_range(float distance, uint8_t sensor, const RayHit& hit) const { + const float angle = this->sensors.at(sensor).angle; + const float sine = std::abs(std::sin(angle)); + const float calibrated = sine > 0.1F ? sine : std::abs(std::cos(angle)); + + return distance * std::sqrt(hit.cosine / calibrated); +} + float WallModel::get_range_deviation(float range) const { return this->range_noise + this->range_noise_per_meter * range; } diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index 630b802..930640a 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -63,6 +63,13 @@ class Fan { */ void stop(); + /** + * @brief Check if the fan has finished ramping to the speed it was asked for. + * + * @return True once the current speed is the target one, which a stopped fan always is. + */ + bool is_at_speed() const; + /** * @brief Check if the driver is reporting a fault. * diff --git a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp index c360803..0fb240c 100644 --- a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp +++ b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp @@ -9,6 +9,7 @@ #include #include #include +#include #include "micras/core/butterworth_filter.hpp" #include "micras/core/utils.hpp" @@ -24,6 +25,8 @@ TWallSensors::TWallSensors(const Config& config) : slow_filters{core::make_array(config.slow_filter)}, reference_readings{config.reference_readings}, reference_distances{config.reference_distances}, + receiver_offset{config.receiver_offset}, + receiver_half_angle{config.receiver_half_angle}, noise_floor{config.noise_floor}, max_distance{config.max_distance}, max_reading{config.max_reading}, @@ -39,9 +42,63 @@ TWallSensors::TWallSensors(const Config& config) : frequency_tolerance * config.fast_filter.sampling_frequency; }) } { + if (this->receiver_offset > 0.0F) { + const float step = this->max_distance / static_cast(4 * shape_points); + float peak = step; + + for (float distance = step; distance < this->max_distance; distance += step) { + if (this->shape(distance) > this->shape(peak)) { + peak = distance; + } + } + + const float ratio = std::pow(this->max_distance / peak, 1.0F / static_cast(shape_points - 1)); + float distance = peak; + + for (uint8_t i = 0; i < shape_points; i++) { + this->shape_distances.at(i) = distance; + this->shape_scales.at(i) = 1.0F / std::sqrt(this->shape(distance)); + distance *= ratio; + } + } + this->turn_off(); } +template +float TWallSensors::shape(float distance) const { + const float angle = std::atan(this->receiver_offset / distance) / this->receiver_half_angle; + return std::exp2(-angle * angle) / (distance * distance); +} + +template +float TWallSensors::to_distance(uint8_t sensor_index, float intensity) const { + const float reference_distance = this->reference_distances.at(sensor_index); + const float ratio = this->reference_readings.at(sensor_index) / intensity; + + if (this->receiver_offset <= 0.0F) { + return reference_distance * std::sqrt(ratio); + } + + const float scale = std::sqrt(ratio) / std::sqrt(this->shape(reference_distance)); + + if (scale <= this->shape_scales.front()) { + return this->shape_distances.front(); + } + + if (scale >= this->shape_scales.back()) { + return this->max_distance; + } + + const auto upper = std::ranges::upper_bound(this->shape_scales, scale); + const auto index = static_cast(std::distance(this->shape_scales.begin(), upper)); + const float low = this->shape_scales.at(index - 1); + const float fraction = (scale - low) / (this->shape_scales.at(index) - low); + + return this->shape_distances.at(index - 1) + + fraction * (this->shape_distances.at(index) - this->shape_distances.at(index - 1)); +} + template void TWallSensors::turn_on() { for (auto& led_pwm : this->led_pwms) { @@ -73,11 +130,9 @@ void TWallSensors::update() { const float intensity = this->get_intensity(i); - const float distance = - intensity >= this->noise_floor ? - this->reference_distances.at(i) * - std::sqrt(this->reference_readings.at(i) / std::min(intensity, this->max_reading)) : - this->max_distance; + const float distance = intensity >= this->noise_floor ? + this->to_distance(i, std::min(intensity, this->max_reading)) : + this->max_distance; reading.valid = distance < this->max_distance; reading.distance = this->fast_filters.at(i).update(std::min(distance, this->max_distance)); diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 8c9e06b..c81eebf 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -19,10 +19,16 @@ namespace micras::proxy { * @details Each sensor is an infrared emitter next to a phototransistor, and what comes out of this * class is the distance to whatever the emitter lights up, along its optical axis, in meters. The * emitter has a narrow beam that lands entirely on the wall, which scatters it in every direction, - * so the light that comes back falls with the square of the distance. One reading at a known - * distance therefore calibrates a sensor: + * so the light that comes back falls with the square of the distance. The receiver sits beside the + * emitter, not on its axis, and only takes in light close to its own axis, so it sees the lit spot + * at an angle that grows as the wall comes closer and loses light to its directivity: * - * distance = reference_distance * sqrt(reference_reading / reading) + * reading = k * 2^(-(atan(receiver_offset / distance) / receiver_half_angle)^2) / distance^2 + * + * One reading at a known distance therefore calibrates a sensor, by fixing k. With no receiver + * offset this is the inverse square law, distance = reference_distance * sqrt(reference_reading / + * reading). With one, the reading peaks at a short distance and falls again closer than that, so a + * reading above the peak is reported as the distance of the peak, as a saturated one is. * * @tparam num_of_sensors Number of sensors. */ @@ -38,7 +44,9 @@ class TWallSensors { * A reading below the noise floor, or one that works out to more than the maximum distance, * means nothing is within range. A reading above the maximum is saturated, and is reported as * the distance of the maximum reading. A wall is considered present when the slow distance is - * below the wall distance, and absent again when it goes above it by the hysteresis. + * below the wall distance, and absent again when it goes above it by the hysteresis. The + * receiver offset is the distance from the optical axis of the emitter to the receiver, and the + * receiver half angle the angle off its own axis at which the receiver's sensitivity halves. */ struct Config { hal::AdcDma::Config adc; @@ -48,6 +56,8 @@ class TWallSensors { core::ButterworthFilter::Config slow_filter; std::array reference_readings; std::array reference_distances; + float receiver_offset; + float receiver_half_angle; float noise_floor; float max_reading; float max_distance; @@ -172,6 +182,28 @@ class TWallSensors { */ static constexpr float frequency_tolerance{0.01F}; + /** + * @brief Number of distances the shape of the reading is tabulated at. + */ + static constexpr uint8_t shape_points{64}; + + /** + * @brief Get how the reading varies with the distance, up to the calibrated constant. + * + * @param distance Distance to the wall in meters. + * @return The reading at that distance divided by k. + */ + float shape(float distance) const; + + /** + * @brief Get the distance a reading corresponds to. + * + * @param sensor_index Index of the sensor. + * @param intensity Reading from 0 to 1, above the noise floor. + * @return Distance in meters. + */ + float to_distance(uint8_t sensor_index, float intensity) const; + /** * @brief Averaging of the readings of one sensor during its calibration. */ @@ -252,6 +284,26 @@ class TWallSensors { */ std::array reference_distances; + /** + * @brief Distance from the axis of each emitter to its receiver. + */ + float receiver_offset; + + /** + * @brief Angle off its axis at which the sensitivity of the receiver halves. + */ + float receiver_half_angle; + + /** + * @brief Distances the shape of the reading is tabulated at, from its peak to the maximum distance. + */ + std::array shape_distances{}; + + /** + * @brief One over the square root of the shape at each tabulated distance, which grows with it. + */ + std::array shape_scales{}; + /** * @brief Reading below which there is only noise. */ diff --git a/micras_proxy/src/fan.cpp b/micras_proxy/src/fan.cpp index a1d7728..745e7ce 100644 --- a/micras_proxy/src/fan.cpp +++ b/micras_proxy/src/fan.cpp @@ -41,6 +41,10 @@ float Fan::update() { return this->current_speed; } +bool Fan::is_at_speed() const { + return this->current_speed == this->target_speed; +} + void Fan::stop() { this->target_speed = 0.0F; this->current_speed = 0.0F; From 804c3e8593fe31cd944143f20f2f2b530488bcde Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 25 Sep 2026 20:27:47 -0300 Subject: [PATCH 504/561] =?UTF-8?q?=E2=9C=A8=20Plan=20with=20labels=20and?= =?UTF-8?q?=20add=20the=20turns=20of=20two=20bends?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The planner holds labels: a way of reaching a node, with its cost, the turn that reached it and how fast that turn is driven, since those fix the speed the next straight starts at. A label is dropped only when another one of the same node is at least as fast for every route that can follow, which keeps the search exact with far fewer labels than there are ways of reaching a node. The search advances by edges instead of nodes, so a bounded number of them fits in an iteration. Eight turns now enter a cell and leave it through one of its other three walls: SD45E, SD45T and SD135T from a straight, DS45T, DD0E, DS45E, DD90E and DS135B from a diagonal. Each is two bends joined by a straight. A turn of two bends is too long a search for the compiler, so the simulator's turn designer writes config/two_bend_turns.hpp, and the compiler places each design between its nodes and checks, in a static_assert, that it clears every post and every wall it does not cross by the margin. The fast runs of ten mazes with every switch on are 3.5 % faster in total. The explorer asks the planner, with every wall it has not seen taken as absent, for the cells still worth visiting, until the map proves the fastest route known. --- micras_nav/include/micras/nav/explorer.hpp | 4 +- .../include/micras/nav/impl/explorer.tpp | 4 +- .../include/micras/nav/impl/planner.tpp | 624 ++++++++++++------ micras_nav/include/micras/nav/lattice.hpp | 36 +- micras_nav/include/micras/nav/planner.hpp | 417 +++++++++--- micras_nav/include/micras/nav/turn_table.hpp | 545 +++++++++++++-- micras_nav/src/route_compiler.cpp | 4 +- 7 files changed, 1289 insertions(+), 345 deletions(-) diff --git a/micras_nav/include/micras/nav/explorer.hpp b/micras_nav/include/micras/nav/explorer.hpp index 72b4b6a..77ed5c0 100644 --- a/micras_nav/include/micras/nav/explorer.hpp +++ b/micras_nav/include/micras/nav/explorer.hpp @@ -68,10 +68,10 @@ class TExplorer { * @brief Advance the planning. * * @param maze The map of the walls. - * @param max_nodes The largest number of nodes the planner may expand in this call. + * @param max_edges The largest number of edges the planner may try in this call. * @return True if the targets changed. */ - bool update(const TMaze& maze, uint32_t max_nodes); + bool update(const TMaze& maze, uint32_t max_edges); /** * @brief Check if an answer for the current map exists yet. diff --git a/micras_nav/include/micras/nav/impl/explorer.tpp b/micras_nav/include/micras/nav/impl/explorer.tpp index 175b845..c7cfcb0 100644 --- a/micras_nav/include/micras/nav/impl/explorer.tpp +++ b/micras_nav/include/micras/nav/impl/explorer.tpp @@ -30,7 +30,7 @@ void TExplorer::reset() { } template -bool TExplorer::update(const TMaze& maze, uint32_t max_nodes) { +bool TExplorer::update(const TMaze& maze, uint32_t max_edges) { if (this->profiles.empty()) { this->answered = true; return false; @@ -55,7 +55,7 @@ bool TExplorer::update(const TMaze& maze, uint32_t this->planning = true; } - if (not this->planner.step(max_nodes)) { + if (not this->planner.step(max_edges)) { return false; } diff --git a/micras_nav/include/micras/nav/impl/planner.tpp b/micras_nav/include/micras/nav/impl/planner.tpp index 049dd1d..8f94896 100644 --- a/micras_nav/include/micras/nav/impl/planner.tpp +++ b/micras_nav/include/micras/nav/impl/planner.tpp @@ -36,14 +36,55 @@ void TPlanner::begin( this->turn_speeds.at(i) = this->dynamics.get_turn_speed(profile, static_cast(i)); } - for (auto& runs : this->edges) { - for (auto& turns : runs) { - turns.fill({.cost = -1.0F, .speed_ratio = 0}); + this->number_of_usable_turns.fill(0); + + for (uint8_t i = 0; i < number_of_turns; i++) { + const auto turn = static_cast(i); + const uint8_t diagonal = get_primitive(turn).diagonal_entry ? 1 : 0; + + if (this->is_usable(turn)) { + this->usable_turns.at(diagonal).at(this->number_of_usable_turns.at(diagonal)) = turn; + this->number_of_usable_turns.at(diagonal)++; + } + } + + for (auto& costs : this->edge_costs) { + costs.fill(-1.0F); + } + + for (auto& row : this->gaps) { + row.fill(-std::numeric_limits::infinity()); + } + + this->preparing = true; + this->prepare_first = 0; + this->prepare_second = 0; + this->prepare_run = 0; + this->expanding = none; + + this->goal_length = 1; + + for (const GridPoint& cell : maze.get_goal()) { + for (const Side side : {Side::RIGHT, Side::UP, Side::LEFT, Side::DOWN}) { + GridPose pose{.position = cell, .orientation = side}; + uint8_t length = 1; + + while (TMaze::contains(pose.front().position) and maze.is_goal(pose.front().position)) { + pose = pose.front(); + length++; + } + + this->goal_length = std::max(this->goal_length, length); } } - this->cleared = 0; + this->heads.fill(none); this->queue_size = 0; + this->allocated = 0; + this->free_list = none; + this->held = 0; + this->peak = 0; + this->exact = true; this->number_of_terminals = 0; const GridPose& start_pose = maze.get_start(); @@ -62,36 +103,50 @@ void TPlanner::begin( .y = static_cast(center.point.y + step.y / 2)}, .heading = heading, }; + + if (is_inside(this->start) and this->is_traversable(this->start)) { + this->insert({ + .cost = 0.0F, + .node = encode(this->start), + .parent = none, + .next = none, + .position = none, + .arrival = rest, + .speed_ratio = full_speed, + .run = 0, + .side = TurnSide::LEFT, + }); + } } template -bool TPlanner::step(uint32_t max_nodes) { - while (this->cleared < number_of_states and max_nodes > 0) { - const uint16_t end = std::min(this->cleared + states_per_node_budget, number_of_states); - - std::fill( - this->costs.begin() + this->cleared, this->costs.begin() + end, std::numeric_limits::infinity() - ); - std::fill(this->positions.begin() + this->cleared, this->positions.begin() + end, not_queued); +bool TPlanner::step(uint32_t max_edges) { + for (; max_edges > 0; max_edges--) { + if (this->preparing) { + this->prepare(); + continue; + } - this->cleared = end; - max_nodes--; + if (this->expanding == none) { + if (this->queue_size == 0) { + break; + } - if (this->cleared == number_of_states and is_inside(this->start) and this->is_traversable(this->start)) { - this->relax(encode({.node = this->start, .arrival = Arrival::REST}), 0.0F, 0, full_speed); - } - } + const uint16_t index = this->pop(); - for (uint32_t i = 0; i < max_nodes and this->queue_size > 0; i++) { - const uint16_t index = this->pop(); + if (this->number_of_terminals == number_of_candidates and + this->labels.at(index).cost >= this->terminals.at(number_of_candidates - 1).cost) { + this->queue_size = 0; + break; + } - if (this->number_of_terminals == number_of_candidates and - this->costs.at(index) >= this->terminals.at(number_of_candidates - 1).cost) { - this->queue_size = 0; - break; + this->expanding = index; + this->expansion_entry = decode(this->labels.at(index).node); + this->expansion_run = 0; + this->expansion_turn = 0; } - this->expand(index); + this->advance_expansion(); } return this->is_finished(); @@ -99,7 +154,17 @@ bool TPlanner::step(uint32_t max_nodes) { template bool TPlanner::is_finished() const { - return this->cleared == number_of_states and this->queue_size == 0; + return not this->preparing and this->expanding == none and this->queue_size == 0; +} + +template +bool TPlanner::is_exact() const { + return this->exact; +} + +template +uint16_t TPlanner::get_peak_labels() const { + return this->peak; } template @@ -121,42 +186,9 @@ void TPlanner::get_route(uint8_t index, Route& route) const { {.run = terminal.run, .has_turn = terminal.has_turn, .turn = terminal.turn, .side = terminal.side} ); - uint16_t index_of_state = terminal.state; - - while (true) { - const State state = decode(index_of_state); - const LatticePose& node = state.node; - const Arrival arrival = state.arrival; - - if (arrival == Arrival::REST) { - break; - } - - const uint16_t link = this->links.at(index_of_state); - const auto run = static_cast(link & 0x3FU); - const auto previous = static_cast((link >> 6U) & 0x0FU); - const TurnSide side = (link >> 10U) != 0 ? TurnSide::RIGHT : TurnSide::LEFT; - const TurnId turn = to_turn(arrival); - - route.steps.push_back({.run = run, .has_turn = true, .turn = turn, .side = side}); - - const TurnPrimitive& primitive = get_primitive(turn); - const uint8_t rotation = - side == TurnSide::LEFT ? primitive.rotation : LatticePose::number_of_headings - primitive.rotation; - const auto entry_heading = static_cast( - (node.heading + LatticePose::number_of_headings - rotation) % LatticePose::number_of_headings - ); - const LatticePoint offset = from_canonical(primitive.exit, primitive.diagonal_entry, entry_heading, side); - const LatticePoint step = LatticePose{.point = {}, .heading = entry_heading}.step(); - - const LatticePose origin{ - .point = - {.x = static_cast(node.point.x - offset.x - run * step.x), - .y = static_cast(node.point.y - offset.y - run * step.y)}, - .heading = entry_heading, - }; - - index_of_state = encode({.node = origin, .arrival = previous}); + for (uint16_t i = terminal.label; this->labels.at(i).arrival != rest; i = this->labels.at(i).parent) { + const Label& label = this->labels.at(i); + route.steps.push_back({.run = label.run, .has_turn = true, .turn = to_turn(label.arrival), .side = label.side}); } std::ranges::reverse(route.steps); @@ -188,23 +220,18 @@ void TPlanner::for_each_wall(const Route& route, F&& function) { } template -constexpr TPlanner::Arrival TPlanner::to_arrival(TurnId turn) { - constexpr std::array arrivals{ - Arrival::SS90S, Arrival::SS90L, Arrival::SS180, Arrival::SD45, - Arrival::SD135, Arrival::DS45, Arrival::DS135, Arrival::DD90, - }; +constexpr bool TPlanner::ends_diagonal(uint8_t arrival) { + if (arrival == rest) { + return false; + } - return arrivals.at(std::to_underlying(turn)); + const TurnPrimitive& primitive = get_primitive(to_turn(arrival)); + return (primitive.rotation % 2 == 1) != primitive.diagonal_entry; } template -constexpr TurnId TPlanner::to_turn(Arrival arrival) { - constexpr std::array turns{ - TurnId::SS90S, TurnId::SS90S, TurnId::SS90L, TurnId::SS180, TurnId::DS45, - TurnId::DS135, TurnId::SD45, TurnId::SD135, TurnId::DD90, - }; - - return turns.at(std::to_underlying(arrival)); +constexpr TurnId TPlanner::to_turn(uint8_t arrival) { + return static_cast(arrival - 1); } template @@ -214,49 +241,45 @@ constexpr bool TPlanner::is_inside(const LatticePose& node) { } template -constexpr uint16_t TPlanner::encode(const State& state) { - const LatticePoint& point = state.node.point; - const uint8_t heading = state.node.heading; +constexpr uint16_t TPlanner::encode(const LatticePose& node) { + const LatticePoint& point = node.point; const int32_t wall = point.on_vertical_wall() ? (point.x / 2) * height + (point.y - 1) / 2 : (width + 1) * height + ((point.x - 1) / 2) * (height + 1) + point.y / 2; - const int32_t sub = heading % 2 == 0 ? (heading / 4) * 6 + std::to_underlying(state.arrival) : - 12 + (heading / 2) * 3 + (std::to_underlying(state.arrival) - 6); + const int32_t sub = node.heading % 2 == 0 ? node.heading / 4 : 2 + node.heading / 2; - return static_cast(wall * states_per_wall + sub); + return static_cast(wall * headings_per_wall + sub); } template -constexpr TPlanner::State TPlanner::decode(uint16_t index) { - const uint16_t wall = index / states_per_wall; - const uint16_t sub = index % states_per_wall; +constexpr LatticePose TPlanner::decode(uint16_t index) { + const uint16_t wall = index / headings_per_wall; + const uint16_t sub = index % headings_per_wall; - State state{}; + LatticePose node{}; if (wall < (width + 1) * height) { - state.node.point = { - .x = static_cast(2 * (wall / height)), .y = static_cast(2 * (wall % height) + 1) - }; + node.point = {.x = static_cast(2 * (wall / height)), .y = static_cast(2 * (wall % height) + 1)}; } else { const uint16_t horizontal = wall - (width + 1) * height; - state.node.point = { + node.point = { .x = static_cast(2 * (horizontal / (height + 1)) + 1), .y = static_cast(2 * (horizontal % (height + 1))), }; } - if (sub < 12) { - state.node.heading = static_cast((state.node.point.on_vertical_wall() ? 0 : 2) + 4 * (sub / 6)); - state.arrival = static_cast(sub % 6); - } else { - state.node.heading = static_cast(2 * ((sub - 12) / 3) + 1); - state.arrival = static_cast(6 + (sub - 12) % 3); - } + node.heading = sub < 2 ? static_cast((node.point.on_vertical_wall() ? 0 : 2) + 4 * sub) : + static_cast(2 * (sub - 2) + 1); - return state; + return node; +} + +template +bool TPlanner::is_usable(TurnId turn) const { + return this->dynamics.get_turn(this->run_profile, turn).valid; } template @@ -297,40 +320,39 @@ float TPlanner::get_stop_distance(const LatticePose& node) const } template -float TPlanner::get_speed(uint16_t index, Arrival arrival) const { - if (arrival == Arrival::REST) { +float TPlanner::get_speed(uint8_t arrival, uint8_t speed_ratio) const { + if (arrival == rest) { return 0.0F; } - return this->turn_speeds.at(std::to_underlying(to_turn(arrival))) * - static_cast(this->speed_ratios.at(index)) / static_cast(full_speed); + return this->turn_speeds.at(arrival - 1) * static_cast(speed_ratio) / static_cast(full_speed); } template -float TPlanner::get_offset(Arrival arrival) const { - return arrival == Arrival::REST ? this->start_distance : - this->dynamics.get_turn(this->run_profile, to_turn(arrival)).post; +float TPlanner::get_offset(uint8_t arrival) const { + return arrival == rest ? this->start_distance : this->dynamics.get_turn(this->run_profile, to_turn(arrival)).post; } template TPlanner::Edge - TPlanner::get_edge(uint16_t index, Arrival arrival, uint8_t run, TurnId turn) { - const bool at_full_speed = arrival == Arrival::REST or this->speed_ratios.at(index) == full_speed; - Edge& cached = this->edges.at(std::to_underlying(arrival)).at(run).at(std::to_underlying(turn)); - - if (at_full_speed and cached.cost >= 0.0F) { - return cached; + TPlanner::get_edge(uint8_t arrival, uint8_t speed_ratio, uint8_t run, TurnId turn) { + const bool at_full_speed = arrival == rest or speed_ratio == full_speed; + const int16_t pair = edge_pairs.index.at(arrival).at(std::to_underlying(turn)); + float& cached_cost = this->edge_costs.at(pair).at(run); + uint8_t& cached_speed_ratio = this->edge_speed_ratios.at(pair).at(run); + + if (at_full_speed and cached_cost >= 0.0F) { + return {.cost = cached_cost, .speed_ratio = cached_speed_ratio}; } const float cell_size = this->dynamics.get_model().maze.cell_size; - const bool diagonal = std::to_underlying(arrival) >= std::to_underlying(Arrival::SD45); - const float step = diagonal ? cell_size / std::numbers::sqrt2_v : cell_size; + const float step = ends_diagonal(arrival) ? cell_size / std::numbers::sqrt2_v : cell_size; const TurnShape& shape = this->dynamics.get_turn(this->run_profile, turn); const float distance = this->get_offset(arrival) + static_cast(run) * step + shape.pre; const float nominal_speed = this->turn_speeds.at(std::to_underlying(turn)); - float start_speed = this->get_speed(index, arrival); + float start_speed = this->get_speed(arrival, speed_ratio); float penalty = 0.0F; const float brakeable = SpeedProfile::get_brakeable_speed(distance, nominal_speed, this->limits); @@ -351,43 +373,171 @@ TPlanner::Edge }; if (at_full_speed) { - cached = edge; + cached_cost = edge.cost; + cached_speed_ratio = edge.speed_ratio; } return edge; } template -void TPlanner::expand(uint16_t index) { - const State state = decode(index); +bool TPlanner::is_comparable(uint8_t first, uint8_t second) const { + const auto is_reachable = [this](uint8_t arrival) { return arrival == rest or this->is_usable(to_turn(arrival)); }; + + return first != second and ends_diagonal(first) == ends_diagonal(second) and is_reachable(first) and + is_reachable(second); +} + +template +void TPlanner::prepare() { + while (this->prepare_first < number_of_arrivals and + not this->is_comparable(this->prepare_first, this->prepare_second)) { + this->prepare_second++; + + if (this->prepare_second == number_of_arrivals) { + this->prepare_second = 0; + this->prepare_first++; + } + } + + if (this->prepare_first == number_of_arrivals) { + this->preparing = false; + return; + } + + this->compare_arrivals(this->prepare_first, this->prepare_second, this->prepare_run); + this->prepare_run++; + + const uint8_t longest = ends_diagonal(this->prepare_first) ? max_run : max_straight_run; + + if (this->prepare_run > longest or std::isinf(this->gaps.at(this->prepare_first).at(this->prepare_second))) { + this->prepare_run = 0; + this->prepare_second++; + + if (this->prepare_second == number_of_arrivals) { + this->prepare_second = 0; + this->prepare_first++; + } + } +} + +template +void TPlanner::compare_arrivals(uint8_t first, uint8_t second, uint8_t run) { + float& gap = this->gaps.at(first).at(second); + + const bool diagonal = ends_diagonal(first); + const uint8_t kind = diagonal ? 1 : 0; + + for (uint8_t i = 0; i < this->number_of_usable_turns.at(kind); i++) { + const TurnId turn = this->usable_turns.at(kind).at(i); + const TurnPrimitive& primitive = get_primitive(turn); + const auto arrival = static_cast(std::to_underlying(turn) + 1); + + const Edge kept = this->get_edge(first, full_speed, run, turn); + const Edge dropped = this->get_edge(second, full_speed, run, turn); + + if (kept.speed_ratio < dropped.speed_ratio) { + gap = std::numeric_limits::infinity(); + return; + } + + const TurnShape& shape = this->dynamics.get_turn(this->run_profile, turn); + const float kept_speed = this->get_speed(arrival, kept.speed_ratio); + const float dropped_speed = this->get_speed(arrival, dropped.speed_ratio); + + gap = + std::max(gap, (kept.cost - shape.length() / kept_speed) - (dropped.cost - shape.length() / dropped_speed)); + + for (uint8_t j = 0; j < primitive.number_of_gates; j++) { + const float remaining = shape.pre + shape.length() - std::max(shape.gates.at(j), shape.pre); + gap = std::max(gap, (kept.cost - remaining / kept_speed) - (dropped.cost - remaining / dropped_speed)); + } + } + + if (diagonal) { + return; + } - const bool diagonal = state.node.is_diagonal(); const float cell_size = this->dynamics.get_model().maze.cell_size; - LatticePose entry = state.node; + for (uint8_t cells = 1; cells <= this->goal_length; cells++) { + const float stop_distance = (static_cast(cells) - 0.5F) * cell_size; + const float kept_line = this->get_offset(first) + static_cast(run) * cell_size; + const float dropped_line = this->get_offset(second) + static_cast(run) * cell_size; - for (uint8_t run = 0; run <= max_run; run++) { - if (run > 0) { - entry = entry.advanced(); + const SpeedProfile kept{kept_line + stop_distance, this->get_speed(first, full_speed), 0.0F, this->limits}; + const SpeedProfile dropped{ + dropped_line + stop_distance, this->get_speed(second, full_speed), 0.0F, this->limits + }; - if (not is_inside(entry) or not this->is_traversable(entry)) { - break; + gap = std::max(gap, kept.time_at(kept_line) - dropped.time_at(dropped_line)); + } +} + +template +bool TPlanner::dominates(const Label& first, const Label& second) { + if (first.arrival == second.arrival) { + if (first.speed_ratio < second.speed_ratio) { + return false; + } + + if (first.arrival == rest) { + return first.cost <= second.cost; + } + + const float length = this->dynamics.get_turn(this->run_profile, to_turn(first.arrival)).length(); + + return first.cost - length / this->get_speed(first.arrival, first.speed_ratio) <= + second.cost - length / this->get_speed(second.arrival, second.speed_ratio); + } + + if (first.speed_ratio != full_speed) { + return false; + } + + float second_cost = second.cost; + + if (second.speed_ratio != full_speed) { + const float length = this->dynamics.get_turn(this->run_profile, to_turn(second.arrival)).length(); + + second_cost -= length / this->get_speed(second.arrival, second.speed_ratio) - + length / this->get_speed(second.arrival, full_speed); + } + + return first.cost + this->gaps.at(first.arrival).at(second.arrival) <= second_cost; +} + +template +void TPlanner::advance_expansion() { + const Label label = this->labels.at(this->expanding); + const bool diagonal = this->expansion_entry.is_diagonal(); + const uint8_t kind = diagonal ? 1 : 0; + + if (this->expansion_turn == 0) { + if (this->expansion_run > 0) { + this->expansion_entry = this->expansion_entry.advanced(); + + if (not is_inside(this->expansion_entry) or not this->is_traversable(this->expansion_entry)) { + this->expanding = none; + return; } } - if (this->is_goal(entry.cell_ahead())) { + if (this->is_goal(this->expansion_entry.cell_ahead())) { if (not diagonal) { - const float line = this->get_offset(state.arrival) + static_cast(run) * cell_size; - const float stop_distance = this->get_stop_distance(entry); + const float cell_size = this->dynamics.get_model().maze.cell_size; + const float line = + this->get_offset(label.arrival) + static_cast(this->expansion_run) * cell_size; + const float stop_distance = this->get_stop_distance(this->expansion_entry); const SpeedProfile approach{ - line + stop_distance, this->get_speed(index, state.arrival), 0.0F, this->limits + line + stop_distance, this->get_speed(label.arrival, label.speed_ratio), 0.0F, this->limits }; this->add_terminal({ - .cost = this->costs.at(index) + approach.time_at(line), - .state = index, - .run = run, + .cost = label.cost + approach.time_at(line), + .label = this->expanding, + .run = this->expansion_run, .has_turn = false, .turn = TurnId::SS90S, .side = TurnSide::LEFT, @@ -396,35 +546,34 @@ void TPlanner::expand(uint16_t index) { }); } - break; + this->expanding = none; + return; } + } - if (diagonal) { - this->relax_turn(index, state, entry, run, TurnId::DS45); - this->relax_turn(index, state, entry, run, TurnId::DS135); - this->relax_turn(index, state, entry, run, TurnId::DD90); - } else { - this->relax_turn(index, state, entry, run, TurnId::SS90S); - this->relax_turn(index, state, entry, run, TurnId::SS90L); - this->relax_turn(index, state, entry, run, TurnId::SS180); + if (this->expansion_turn < this->number_of_usable_turns.at(kind)) { + this->relax_turn( + this->expanding, this->expansion_entry, this->expansion_run, + this->usable_turns.at(kind).at(this->expansion_turn) + ); + this->expansion_turn++; + } - if (this->run_profile.diagonal) { - this->relax_turn(index, state, entry, run, TurnId::SD45); - this->relax_turn(index, state, entry, run, TurnId::SD135); - } + if (this->expansion_turn >= this->number_of_usable_turns.at(kind)) { + this->expansion_turn = 0; + this->expansion_run++; + + if (this->expansion_run > max_run) { + this->expanding = none; } } } template -void TPlanner::relax_turn( - uint16_t index, const State& state, const LatticePose& entry, uint8_t run, TurnId turn -) { +void TPlanner::relax_turn(uint16_t index, const LatticePose& entry, uint8_t run, TurnId turn) { const TurnPrimitive& primitive = get_primitive(turn); - if (not this->dynamics.get_turn(this->run_profile, turn).valid) { - return; - } + Edge edge{.cost = -1.0F, .speed_ratio = 0}; for (const TurnSide side : {TurnSide::LEFT, TurnSide::RIGHT}) { if (primitive.diagonal_entry and side != entry.diagonal_turn_side()) { @@ -472,8 +621,13 @@ void TPlanner::relax_turn( continue; } - const Edge edge = this->get_edge(index, state.arrival, run, turn); - const float cost = this->costs.at(index) + edge.cost; + const Label& label = this->labels.at(index); + + if (edge.cost < 0.0F) { + edge = this->get_edge(label.arrival, label.speed_ratio, run, turn); + } + + const float cost = label.cost + edge.cost; if (finish_gate >= 0) { if (exit.is_diagonal() or not this->is_goal(exit.cell_ahead())) { @@ -483,14 +637,13 @@ void TPlanner::relax_turn( const TurnShape& shape = this->dynamics.get_turn(this->run_profile, turn); const float remaining = shape.pre + shape.length() - std::max(shape.gates.at(static_cast(finish_gate)), shape.pre); - const float speed = this->turn_speeds.at(std::to_underlying(turn)) * static_cast(edge.speed_ratio) / - static_cast(full_speed); + const float speed = this->get_speed(std::to_underlying(turn) + 1, edge.speed_ratio); const float stop_distance = this->get_stop_distance(exit); this->add_terminal({ .cost = cost - remaining / speed, - .state = index, + .label = index, .run = run, .has_turn = true, .turn = turn, @@ -502,16 +655,17 @@ void TPlanner::relax_turn( continue; } - const uint16_t target = encode({.node = exit, .arrival = to_arrival(turn)}); - - if (cost < this->costs.at(target)) { - const auto link = static_cast( - static_cast(run) | (static_cast(std::to_underlying(state.arrival)) << 6U) | - ((side == TurnSide::RIGHT ? 1U : 0U) << 10U) - ); - - this->relax(target, cost, link, edge.speed_ratio); - } + this->insert({ + .cost = cost, + .node = encode(exit), + .parent = index, + .next = none, + .position = none, + .arrival = static_cast(std::to_underlying(turn) + 1), + .speed_ratio = edge.speed_ratio, + .run = run, + .side = side, + }); } } @@ -534,79 +688,177 @@ void TPlanner::add_terminal(const Terminal& terminal) { } template -void TPlanner::relax(uint16_t index, float cost, uint16_t link, uint8_t speed_ratio) { - this->costs.at(index) = cost; - this->links.at(index) = link; - this->speed_ratios.at(index) = speed_ratio; +void TPlanner::insert(Label label) { + uint16_t previous = none; + uint16_t current = this->heads.at(label.node); - if (this->positions.at(index) == not_queued) { - this->positions.at(index) = this->queue_size; - this->queue.at(this->queue_size) = index; - this->queue_size++; + while (current != none) { + const Label& other = this->labels.at(current); + const uint16_t next = other.next; + + if (this->dominates(other, label)) { + return; + } + + if (other.position != none and this->dominates(label, other)) { + if (previous == none) { + this->heads.at(label.node) = next; + } else { + this->labels.at(previous).next = next; + } + + this->remove(other.position); + this->labels.at(current).next = this->free_list; + this->free_list = current; + this->held--; + } else { + previous = current; + } + + current = next; + } + + const uint16_t index = this->allocate(label.cost); + + if (index == none) { + return; + } + + label.next = this->heads.at(label.node); + this->labels.at(index) = label; + this->heads.at(label.node) = index; + this->push(index); +} + +template +uint16_t TPlanner::allocate(float cost) { + if (this->free_list == none and this->allocated == max_labels) { + this->exact = false; + + uint16_t costliest = none; + + for (uint16_t position = this->queue_size / 2; position < this->queue_size; position++) { + const uint16_t index = this->queue.at(position); + + if (costliest == none or this->labels.at(index).cost > this->labels.at(costliest).cost) { + costliest = index; + } + } + + if (costliest == none or this->labels.at(costliest).cost <= cost) { + return none; + } + + this->release(costliest); + } + + uint16_t index = this->free_list; + + if (index != none) { + this->free_list = this->labels.at(index).next; + } else { + index = this->allocated; + this->allocated++; } - this->sift_up(this->positions.at(index)); + this->held++; + this->peak = std::max(this->peak, this->held); + + return index; +} + +template +void TPlanner::release(uint16_t index) { + Label& label = this->labels.at(index); + uint16_t* link = &this->heads.at(label.node); + + while (*link != index) { + link = &this->labels.at(*link).next; + } + + *link = label.next; + this->remove(label.position); + label.next = this->free_list; + this->free_list = index; + this->held--; +} + +template +void TPlanner::push(uint16_t index) { + this->queue.at(this->queue_size) = index; + this->labels.at(index).position = this->queue_size; + this->queue_size++; + this->sift_up(this->queue_size - 1); } template uint16_t TPlanner::pop() { const uint16_t top = this->queue.at(0); + this->remove(0); + return top; +} +template +void TPlanner::remove(uint16_t position) { + this->labels.at(this->queue.at(position)).position = none; this->queue_size--; - this->positions.at(top) = not_queued; - if (this->queue_size > 0) { - this->queue.at(0) = this->queue.at(this->queue_size); - this->positions.at(this->queue.at(0)) = 0; - this->sift_down(0); + if (position == this->queue_size) { + return; } - return top; + const uint16_t last = this->queue.at(this->queue_size); + this->queue.at(position) = last; + this->labels.at(last).position = position; + this->sift_down(position); + this->sift_up(this->labels.at(last).position); } template void TPlanner::sift_up(uint16_t position) { const uint16_t index = this->queue.at(position); + const float cost = this->labels.at(index).cost; while (position > 0) { const uint16_t parent = (position - 1) / 2; - if (this->costs.at(this->queue.at(parent)) <= this->costs.at(index)) { + if (this->labels.at(this->queue.at(parent)).cost <= cost) { break; } this->queue.at(position) = this->queue.at(parent); - this->positions.at(this->queue.at(position)) = position; + this->labels.at(this->queue.at(position)).position = position; position = parent; } this->queue.at(position) = index; - this->positions.at(index) = position; + this->labels.at(index).position = position; } template void TPlanner::sift_down(uint16_t position) { const uint16_t index = this->queue.at(position); + const float cost = this->labels.at(index).cost; while (2 * position + 1 < this->queue_size) { uint16_t child = 2 * position + 1; if (child + 1 < this->queue_size and - this->costs.at(this->queue.at(child + 1)) < this->costs.at(this->queue.at(child))) { + this->labels.at(this->queue.at(child + 1)).cost < this->labels.at(this->queue.at(child)).cost) { child++; } - if (this->costs.at(index) <= this->costs.at(this->queue.at(child))) { + if (cost <= this->labels.at(this->queue.at(child)).cost) { break; } this->queue.at(position) = this->queue.at(child); - this->positions.at(this->queue.at(position)) = position; + this->labels.at(this->queue.at(position)).position = position; position = child; } this->queue.at(position) = index; - this->positions.at(index) = position; + this->labels.at(index).position = position; } } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/lattice.hpp b/micras_nav/include/micras/nav/lattice.hpp index 7628802..6d214e4 100644 --- a/micras_nav/include/micras/nav/lattice.hpp +++ b/micras_nav/include/micras/nav/lattice.hpp @@ -20,6 +20,13 @@ namespace micras::nav { * @details S is a straight along the grid and D a diagonal, followed by the angle turned. SS90S is * the short turn that fits inside one cell, which is also the turn used while searching, and SS90L * the long one that needs a cell of straight on both sides. + * + * The turns from SD45E on enter a cell and leave it through one of its other three walls, two bends + * each. Their last letter is the wall they leave through, in the canonical frame of TurnPrimitive: T + * the wall on the left of the entry, E the wall across from it and B the wall on the right. Those that + * start on a diagonal may end turned to the right of it, since the side of a diagonal turn is fixed by + * the wall it starts on: DS45E, DD90E and DS135B turn right, and DD0E does not turn, + * shifting to the next diagonal. */ enum class TurnId : uint8_t { SS90S = 0, @@ -30,7 +37,15 @@ enum class TurnId : uint8_t { DS45 = 5, DS135 = 6, DD90 = 7, - NUMBER_OF_TURNS = 8, + SD45E = 8, + SD45T = 9, + SD135T = 10, + DS45T = 11, + DD0E = 12, + DS45E = 13, + DD90E = 14, + DS135B = 15, + NUMBER_OF_TURNS = 16, }; /** @@ -38,6 +53,17 @@ enum class TurnId : uint8_t { */ inline constexpr uint8_t number_of_turns{std::to_underlying(TurnId::NUMBER_OF_TURNS)}; +/** + * @brief First of the turns of two bends, whose shapes are designed before the firmware is compiled + * rather than while it is. + */ +inline constexpr uint8_t first_two_bend_turn{std::to_underlying(TurnId::SD45E)}; + +/** + * @brief Number of turns of two bends. + */ +inline constexpr uint8_t number_of_two_bend_turns{number_of_turns - first_two_bend_turn}; + /** * @brief Side a turn is made to. */ @@ -315,6 +341,14 @@ inline constexpr std::array turn_primitives{{ .exit = {.x = 0, .y = 2}, .gates = {{{.x = 1, .y = 1}, {}}}, .number_of_gates = 1}, + {.diagonal_entry = false, .rotation = 1, .exit = {.x = 2, .y = 0}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = false, .rotation = 1, .exit = {.x = 1, .y = 1}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = false, .rotation = 3, .exit = {.x = 1, .y = 1}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = true, .rotation = 1, .exit = {.x = 1, .y = 1}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = true, .rotation = 0, .exit = {.x = 2, .y = 0}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = true, .rotation = 7, .exit = {.x = 2, .y = 0}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = true, .rotation = 6, .exit = {.x = 2, .y = 0}, .gates = {}, .number_of_gates = 0}, + {.diagonal_entry = true, .rotation = 5, .exit = {.x = 1, .y = -1}, .gates = {}, .number_of_gates = 0}, }}; /** diff --git a/micras_nav/include/micras/nav/planner.hpp b/micras_nav/include/micras/nav/planner.hpp index 5fd4443..713af63 100644 --- a/micras_nav/include/micras/nav/planner.hpp +++ b/micras_nav/include/micras/nav/planner.hpp @@ -55,21 +55,27 @@ struct Route { /** * @brief Search for the fastest route from the start to the goal. * - * @details The search is a Dijkstra over the lattice of lattice.hpp. A state is a node together with - * the turn that was just made to reach it, because that turn fixes the speed the next straight - * starts at and how much of it the turn already covered. An edge is a motion the robot actually - * executes, a run of steps ahead followed by a turn, and it exists only if every wall it crosses - * passes the wall assumption. Its cost is the time of the run, from the evaluator the executor plays - * back, plus the time of the turn at the speed the run profile gives it, so the fastest path of the - * graph is the fastest route the robot can drive. Diagonals, the turns of 135 degrees and the two - * sizes of the turn of 90 degrees are part of the search rather than a rewrite of its result. + * @details The search is a Dijkstra over the lattice of lattice.hpp. An edge is a motion the robot + * actually executes, a run of steps ahead followed by a turn, and it exists only if every wall it + * crosses passes the wall assumption. Its cost is the time of the run, from the evaluator the + * executor plays back, plus the time of the turn at the speed the run profile gives it, so the + * fastest path of the graph is the fastest route the robot can drive. Diagonals, the turns of 135 + * degrees and the two sizes of the turn of 90 degrees are part of the search rather than a rewrite + * of its result. + * + * What the search holds are labels: a way of reaching a node, with its cost, the turn that reached + * it and how fast that turn is driven, since those fix the speed the next straight starts at and + * how much of it the turn already covered. A label is dropped only when another label of the same + * node is at least as fast for every continuation of the route, which is what keeps the search + * exact with far fewer labels than there are ways of reaching a node. * * The clock of a run stops when the robot enters the goal, so a route is priced up to that line, * while still being required to come to rest inside the goal. * - * @note The arrays of the search are members, which makes an object of this class large: it belongs - * in static storage, not on the stack. The search can be advanced a bounded number of nodes at a - * time, which is how it runs while the robot is moving. + * @note The labels are members, which makes an object of this class large: it belongs in static + * storage, not on the stack. The search is advanced a bounded number of edges at a time, which is + * how it runs while the robot is moving: an edge costs about the same whatever the turns, while a + * node costs as many edges as there are turns to try from it. * * @tparam width The width of the maze in cells. * @tparam height The height of the maze in cells. @@ -86,6 +92,16 @@ class TPlanner { */ static constexpr uint8_t number_of_candidates{4}; + /** + * @brief Largest number of labels held at once. + * + * @note On ten contest mazes, whole and with a random part of their walls unknown, the most a + * search held was 4519, with every turn and profile, and 4 of 5490 searches held more than 4500. + * A search that needs more drops the costliest labels waiting to be expanded, and reports that it + * is no longer exact. + */ + static constexpr uint16_t max_labels{6144}; + /** * @brief Configuration struct for the planner. * @@ -116,10 +132,13 @@ class TPlanner { /** * @brief Advance the search. * - * @param max_nodes The largest number of nodes to expand in this call. + * @note Before the search itself, every pair of arrivals is compared for the dominance, one run + * of one pair for each edge of the budget. + * + * @param max_edges The largest number of edges to try in this call. * @return True if the search has finished. */ - bool step(uint32_t max_nodes); + bool step(uint32_t max_edges); /** * @brief Check if the search has finished. @@ -128,6 +147,20 @@ class TPlanner { */ bool is_finished() const; + /** + * @brief Check if the search kept every label it needed. + * + * @return False if it ran out of labels, in which case the routes found may not be the fastest. + */ + bool is_exact() const; + + /** + * @brief Get the largest number of labels the search held at once. + * + * @return The number of labels, at most max_labels. + */ + uint16_t get_peak_labels() const; + /** * @brief Get the number of routes found. * @@ -155,27 +188,22 @@ class TPlanner { private: /** - * @brief How a state was reached, which fixes the speed and the offset of the next straight. - */ - enum class Arrival : uint8_t { - REST = 0, - SS90S = 1, - SS90L = 2, - SS180 = 3, - DS45 = 4, - DS135 = 5, - SD45 = 6, - SD135 = 7, - DD90 = 8, - NUMBER_OF_ARRIVALS = 9, - }; - - /** - * @brief Node of the search. + * @brief Way of reaching a node. + * + * @note The arrival is the turn that reached the node plus one, zero being the start, and the + * run and the side are those of the edge that ended with that turn. The position is the place + * of the label in the queue, and is not_queued once it has been expanded. */ - struct State { - LatticePose node; - Arrival arrival; + struct Label { + float cost; + uint16_t node; + uint16_t parent; + uint16_t next; + uint16_t position; + uint8_t arrival; + uint8_t speed_ratio; + uint8_t run; + TurnSide side; }; /** @@ -183,7 +211,7 @@ class TPlanner { */ struct Terminal { float cost; - uint16_t state; + uint16_t label; uint8_t run; bool has_turn; TurnId turn; @@ -203,16 +231,25 @@ class TPlanner { uint8_t speed_ratio; }; + /** + * @brief Arrival of the label of the start, which is at rest. + */ + static constexpr uint8_t rest{0}; + + /** + * @brief Number of ways a node can be reached: at rest, or by one of the turns. + */ + static constexpr uint8_t number_of_arrivals{number_of_turns + 1}; + /** * @brief Speed ratio of a turn driven at the speed the run profile gives it. */ static constexpr uint8_t full_speed{255}; /** - * @brief Number of states of one wall: two headings along the grid reached in six ways, and - * four diagonal headings reached in three. + * @brief Number of headings a wall can be crossed with: two along the grid and four diagonal. */ - static constexpr uint16_t states_per_wall{24}; + static constexpr uint8_t headings_per_wall{6}; /** * @brief Number of walls of the maze, including the ones of the border. @@ -220,43 +257,71 @@ class TPlanner { static constexpr uint16_t number_of_walls{(width + 1) * height + width * (height + 1)}; /** - * @brief Number of states of the search. + * @brief Number of nodes of the lattice. */ - static constexpr uint16_t number_of_states{number_of_walls * states_per_wall}; + static constexpr uint16_t number_of_nodes{number_of_walls * headings_per_wall}; /** - * @brief Largest number of steps of a run. + * @brief Largest number of steps of a run, along a diagonal, where a step is half a cell. */ static constexpr uint8_t max_run{2 * (width > height ? width : height)}; /** - * @brief Number of states whose cost is reset for the price of expanding one node. - * - * @note Resetting every state takes longer than an iteration of the control loop may, so it is - * spread over the calls to step like the search itself. + * @brief Largest number of steps of a run along the grid, where a step is a whole cell. */ - static constexpr uint16_t states_per_node_budget{512}; + static constexpr uint8_t max_straight_run{width > height ? width : height}; /** - * @brief Marker of a state that is not in the queue. + * @brief Marker of the end of a list of labels, and of a label that is not in the queue. */ - static constexpr uint16_t not_queued{0xFFFF}; + static constexpr uint16_t none{0xFFFF}; /** - * @brief Get the arrival that a turn produces. + * @brief Pairs of an arrival and a turn that can follow it, which are the edges worth caching. * - * @param turn The turn. - * @return The arrival of the state at the exit of the turn. + * @details A turn follows an arrival when it starts on the kind of heading, diagonal or along + * the grid, that the arrival ends on. The index of the pair is its row in the cache, or -1. + */ + struct EdgePairs { + /** + * @brief Number the pairs of arrivals and turns. + */ + constexpr EdgePairs() { + for (uint8_t arrival = 0; arrival < number_of_arrivals; arrival++) { + for (uint8_t turn = 0; turn < number_of_turns; turn++) { + const bool follows = + get_primitive(static_cast(turn)).diagonal_entry == ends_diagonal(arrival); + + this->index.at(arrival).at(turn) = follows ? this->count : -1; + this->count += follows ? 1 : 0; + } + } + } + + std::array, number_of_arrivals> index{}; + int16_t count{}; + }; + + /** + * @brief Index of the cached edge of every pair of an arrival and a turn. + */ + static constexpr EdgePairs edge_pairs{}; + + /** + * @brief Check if an arrival ends on a diagonal heading. + * + * @param arrival The arrival. + * @return True if the turn of the arrival ends on a diagonal. */ - static constexpr Arrival to_arrival(TurnId turn); + static constexpr bool ends_diagonal(uint8_t arrival); /** * @brief Get the turn that produced an arrival. * - * @param arrival The arrival, which must not be REST. + * @param arrival The arrival, which must not be the start. * @return The turn. */ - static constexpr TurnId to_turn(Arrival arrival); + static constexpr TurnId to_turn(uint8_t arrival); /** * @brief Check if a node is inside the maze. @@ -267,20 +332,28 @@ class TPlanner { static constexpr bool is_inside(const LatticePose& node); /** - * @brief Get the index of a state. + * @brief Get the index of a node. * - * @param state The state, whose node must be valid and inside the maze. - * @return The index of the state in the arrays of the search. + * @param node The node, which must be valid and inside the maze. + * @return The index of the node, below number_of_nodes. */ - static constexpr uint16_t encode(const State& state); + static constexpr uint16_t encode(const LatticePose& node); /** - * @brief Get the state behind an index. + * @brief Get the node behind an index. * - * @param index The index of the state in the arrays of the search. - * @return The state. + * @param index The index of the node. + * @return The node. + */ + static constexpr LatticePose decode(uint16_t index); + + /** + * @brief Check if a turn can be driven on the run being planned. + * + * @param turn The turn. + * @return True if the turn fits between its nodes with the margin of the run profile. */ - static constexpr State decode(uint16_t index); + bool is_usable(TurnId turn) const; /** * @brief Check if the robot may cross the wall a node is on. @@ -307,57 +380,101 @@ class TPlanner { float get_stop_distance(const LatticePose& node) const; /** - * @brief Get the speed a state leaves its node with. + * @brief Get the speed a label leaves its node with. * - * @param index The index of the state. - * @param arrival The arrival of the state. + * @param arrival The arrival of the label. + * @param speed_ratio How fast the turn of the arrival is driven. * @return The speed in m/s. */ - float get_speed(uint16_t index, Arrival arrival) const; + float get_speed(uint8_t arrival, uint8_t speed_ratio) const; /** - * @brief Get the straight left between the curve that reached a state and its node. + * @brief Get the straight left between the curve that reached a label and its node. * - * @param arrival The arrival of the state. + * @param arrival The arrival of the label. * @return The distance in meters. */ - float get_offset(Arrival arrival) const; + float get_offset(uint8_t arrival) const; /** * @brief Get the time of a run followed by a turn. * * @details When the run is too short to change from the speed of one turn to the speed of the * other, one of the turns has to be driven slower, which is what the velocity planner will do - * to it. If it is the turn ahead, it is priced at the speed the run allows and the state it leads + * to it. If it is the turn ahead, it is priced at the speed the run allows and the label it leads * to remembers that speed. If it is the turn behind, which was already priced, the time it * loses is added to this edge. * - * @param index The index of the state the run starts from. - * @param arrival The arrival of that state. + * @param arrival The arrival of the label the run starts from. + * @param speed_ratio How fast the turn of that arrival is driven. * @param run The number of steps of the run. * @param turn The turn after the run. * @return The time from the end of the previous curve to the end of this one, and the speed of * this one. */ - Edge get_edge(uint16_t index, Arrival arrival, uint8_t run, TurnId turn); + Edge get_edge(uint8_t arrival, uint8_t speed_ratio, uint8_t run, TurnId turn); + + /** + * @brief Check if two arrivals can meet at a node and have to be compared. + * + * @param first The first arrival. + * @param second The second arrival. + * @return True if they are different, end on the same kind of heading and can both be driven. + */ + bool is_comparable(uint8_t first, uint8_t second) const; + + /** + * @brief Compare the next pair of arrivals over the next run, moving on to the next pair after. + */ + void prepare(); + + /** + * @brief Bound how much slower one arrival at full speed can be than another, over one run. + * + * @details Whatever follows a node starts with the same edge from both arrivals. The two routes + * differ by the time of that first edge, not counting its turn, and by how fast that turn is + * driven, after which the arrival at the faster turn can only be faster. So when the first + * arrival never leaves a turn slower than the second, the most its first edge, or its way into + * the goal, takes longer is the most any route from it can take longer. The bound is the largest + * over every run, and is infinite if the first arrival leaves any turn slower. + * + * @param first The arrival that would be kept. + * @param second The arrival that would be dropped. + * @param run The number of steps of the run before the first edge. + */ + void compare_arrivals(uint8_t first, uint8_t second, uint8_t run); + + /** + * @brief Check if a label is at least as fast as another of the same node, for every route. + * + * @note A label is compared with one of the same arrival by the time before its turn and the + * speed of its turn, and with one of another arrival only when the first turn is driven at full + * speed. A second turn driven slower is compared as if it were driven at full speed, less the + * time it loses, since a turn driven slower never leaves a route faster. + * + * @param first The label that would be kept. + * @param second The label that would be dropped. + * @return True if the second label can be dropped. + */ + bool dominates(const Label& first, const Label& second); /** - * @brief Expand a state, relaxing every edge that leaves it. + * @brief Advance the expansion of a label by one edge. * - * @param index The index of the state. + * @note The run is lengthened when every turn at its end has been tried, and the expansion ends + * at a wall that may not be crossed, at the goal or after the longest run. */ - void expand(uint16_t index); + void advance_expansion(); /** * @brief Relax the edges of one turn, to both sides, from the end of a run. * - * @param index The index of the state the run starts from. - * @param state The state the run starts from. + * @param index The index of the label the run starts from. * @param entry The node where the run ends and the turn starts. * @param run The number of steps of the run. * @param turn The turn. */ - void relax_turn(uint16_t index, const State& state, const LatticePose& entry, uint8_t run, TurnId turn); + void relax_turn(uint16_t index, const LatticePose& entry, uint8_t run, TurnId turn); /** * @brief Record a route that ends inside the goal, if it is among the best. @@ -367,22 +484,51 @@ class TPlanner { void add_terminal(const Terminal& terminal); /** - * @brief Lower the cost of a state, moving it up in the queue. + * @brief Add a label to its node, unless another one there is at least as fast. + * + * @note The labels of the node that the new one is at least as fast as are dropped, unless they + * have been expanded, since other labels may lead back to them. + * + * @param label The label, with every field but the links filled in. + */ + void insert(Label label); + + /** + * @brief Take a label from the pool, making room if it is full. * - * @param index The index of the state. - * @param cost The new cost, lower than the current one. - * @param link How the state was reached, for rebuilding the route. - * @param speed_ratio How fast the turn that reached the state is driven. + * @param cost The cost of the label that needs the room. + * @return The index of the label, or none if every label held is cheaper than the cost. */ - void relax(uint16_t index, float cost, uint16_t link, uint8_t speed_ratio); + uint16_t allocate(float cost); /** - * @brief Remove the state with the lowest cost from the queue. + * @brief Remove a label that waits in the queue from its node and give it back to the pool. * - * @return The index of the state. + * @param index The index of the label. + */ + void release(uint16_t index); + + /** + * @brief Add a label to the queue. + * + * @param index The index of the label. + */ + void push(uint16_t index); + + /** + * @brief Remove the label with the lowest cost from the queue. + * + * @return The index of the label. */ uint16_t pop(); + /** + * @brief Remove a label from the queue, wherever it is. + * + * @param position The position of the label in the queue. + */ + void remove(uint16_t position); + /** * @brief Move an entry of the queue up to its place. * @@ -434,47 +580,110 @@ class TPlanner { std::array turn_speeds{}; /** - * @brief Edge of every combination of arrival, run and turn, for an arrival at full speed, - * filled in when first needed. + * @brief Turns that can be driven on the run being planned, from a heading along the grid and + * from a diagonal one. + */ + std::array, 2> usable_turns{}; + + /** + * @brief Number of usable turns from a heading along the grid and from a diagonal one. + */ + std::array number_of_usable_turns{}; + + /** + * @brief Largest number of cells of the goal in a line, which bounds where the robot stops. */ - std::array< - std::array, max_run + 1>, std::to_underlying(Arrival::NUMBER_OF_ARRIVALS)> - edges{}; + uint8_t goal_length{}; /** - * @brief Lowest cost found for each state. + * @brief Edge of every pair of an arrival at full speed and a turn, for every run, filled in when + * first needed. */ - std::array costs{}; + std::array, edge_pairs.count> edge_costs{}; /** - * @brief How each state was reached: the run, the previous arrival and the side of the turn. + * @brief Speed ratio of the turn of every cached edge. */ - std::array links{}; + std::array, edge_pairs.count> edge_speed_ratios{}; /** - * @brief How fast the turn that reached each state is driven, as a fraction of its speed. + * @brief Dominance gap of every pair of arrivals, computed before the search. */ - std::array speed_ratios{}; + std::array, number_of_arrivals> gaps{}; /** - * @brief Binary heap of the states waiting to be expanded, ordered by cost. + * @brief First label of each node. */ - std::array queue{}; + std::array heads{}; /** - * @brief Position of each state in the queue. + * @brief Labels of the search. */ - std::array positions{}; + std::array labels{}; /** - * @brief Number of states in the queue. + * @brief Binary heap of the labels waiting to be expanded, ordered by cost. + */ + std::array queue{}; + + /** + * @brief Number of labels in the queue. */ uint16_t queue_size{}; /** - * @brief Number of states whose cost was reset since the search began. + * @brief Number of labels ever taken from the pool in this search. + */ + uint16_t allocated{}; + + /** + * @brief First label given back to the pool, the others following it. + */ + uint16_t free_list{none}; + + /** + * @brief Number of labels held. + */ + uint16_t held{}; + + /** + * @brief Largest number of labels held at once. + */ + uint16_t peak{}; + + /** + * @brief Whether every label the search needed was kept. + */ + bool exact{true}; + + /** + * @brief Whether the pairs of arrivals are still being compared. + */ + bool preparing{}; + + /** + * @brief Pair of arrivals and run the comparison is at. + */ + ///@{ + uint8_t prepare_first{}; + uint8_t prepare_second{}; + uint8_t prepare_run{}; + ///@} + + /** + * @brief Label being expanded, or none. + */ + uint16_t expanding{none}; + + /** + * @brief Node at the end of the run of the expansion, the number of steps of that run and the + * next turn to try there. */ - uint16_t cleared{number_of_states}; + ///@{ + LatticePose expansion_entry{}; + uint8_t expansion_run{}; + uint8_t expansion_turn{}; + ///@} /** * @brief Best ends of route found, ordered by cost. diff --git a/micras_nav/include/micras/nav/turn_table.hpp b/micras_nav/include/micras/nav/turn_table.hpp index 063541f..f9003b4 100644 --- a/micras_nav/include/micras/nav/turn_table.hpp +++ b/micras_nav/include/micras/nav/turn_table.hpp @@ -18,20 +18,31 @@ namespace micras::nav { /** - * @brief Geometry of a slalom turn. + * @brief How a path bends at one point: its curvature and the rate at which the curvature changes + * with the distance traveled. + * + * @note Both are signed, positive to the left. + */ +struct Bending { + float curvature; + float sharpness; +}; + +/** + * @brief One bend of a turn: a clothoid that ramps the curvature up, an arc at the peak curvature and + * the mirrored clothoid that ramps it down. * - * @details A turn is a clothoid that ramps the curvature up, an arc at the peak curvature and the - * mirrored clothoid that ramps it down, which is the path of a robot that holds its linear speed - * while its angular speed follows a trapezoid. Only the path is stored: the speed it is driven at is - * chosen when a run is planned, and scaling the angular speed with it and the angular acceleration - * with its square keeps the robot on this same path at any speed. + * @details It is the path of a robot that holds its linear speed while its angular speed follows a + * trapezoid. Only the path is stored: the speed it is driven at is chosen when a run is planned, and + * scaling the angular speed with it and the angular acceleration with its square keeps the robot on + * this same path at any speed. * - * @note The straights before and after the curve place it between its entry and exit nodes. All - * lengths are in meters and the angle, in radians, is that of the turn to the left. + * @note The angle is positive to the left. The curvature and the sharpness are magnitudes, and the + * sign of the angle gives them their direction. */ -struct TurnShape { +struct TurnBend { /** - * @brief Point of the curve, in the frame of its start, for the turn to the left. + * @brief Point of a curve, in the frame of its start. * * @tparam T Floating point type. */ @@ -45,11 +56,11 @@ struct TurnShape { }; /** - * @brief Evaluate the curve at a distance from its start. + * @brief Evaluate the bend at a distance from its start. * * @tparam T Floating point type. - * @param distance Distance traveled along the curve, clamped to its length. - * @return The point of the curve, with the curvature and its rate of change there. + * @param distance Distance traveled along the bend, clamped to its length. + * @return The point of the bend, with the curvature and its rate of change there. */ template constexpr Sample sample(T distance) const { @@ -58,6 +69,7 @@ struct TurnShape { const T rate = static_cast(this->sharpness); const T peak = static_cast(this->curvature); const T total = T{2} * ramp + arc; + const T side = this->angle < 0.0F ? T{-1} : T{1}; if (distance < T{0}) { distance = T{0}; @@ -70,10 +82,10 @@ struct TurnShape { return { .x = point.x, - .y = point.y, - .heading = rate * distance * distance / T{2}, - .curvature = rate * distance, - .sharpness = rate, + .y = side * point.y, + .heading = side * rate * distance * distance / T{2}, + .curvature = side * rate * distance, + .sharpness = side * rate, }; } @@ -85,9 +97,9 @@ struct TurnShape { return { .x = ramp_end.x + (core::math::sin(heading) - core::math::sin(ramp_heading)) / peak, - .y = ramp_end.y - (core::math::cos(heading) - core::math::cos(ramp_heading)) / peak, - .heading = heading, - .curvature = peak, + .y = side * (ramp_end.y - (core::math::cos(heading) - core::math::cos(ramp_heading)) / peak), + .heading = side * heading, + .curvature = side * peak, .sharpness = T{0}, }; } @@ -112,20 +124,153 @@ struct TurnShape { return { .x = end_x - (cosine * mirrored.x + sine * mirrored.y), - .y = end_y - (sine * mirrored.x - cosine * mirrored.y), - .heading = angle - rate * remaining * remaining / T{2}, - .curvature = rate * remaining, - .sharpness = -rate, + .y = side * (end_y - (sine * mirrored.x - cosine * mirrored.y)), + .heading = side * (angle - rate * remaining * remaining / T{2}), + .curvature = side * rate * remaining, + .sharpness = -side * rate, }; } /** - * @brief Get the length of the curve. + * @brief Get how the bend bends at a distance from its start, without evaluating its points. + * + * @param distance Distance traveled along the bend, clamped to its length. + * @return The curvature and the sharpness there. + */ + constexpr Bending bending_at(float distance) const { + const float side = this->angle < 0.0F ? -1.0F : 1.0F; + const float total = this->length(); + + distance = std::clamp(distance, 0.0F, total); + + if (distance <= this->ramp_length) { + return {.curvature = side * this->sharpness * distance, .sharpness = side * this->sharpness}; + } + + if (distance <= this->ramp_length + this->arc_length) { + return {.curvature = side * this->curvature, .sharpness = 0.0F}; + } + + return {.curvature = side * this->sharpness * (total - distance), .sharpness = -side * this->sharpness}; + } + + /** + * @brief Get the length of the bend. * * @return The length of the two ramps and the arc in meters. */ constexpr float length() const { return 2.0F * this->ramp_length + this->arc_length; } + float angle{}; + float curvature{}; + float sharpness{}; + float ramp_length{}; + float arc_length{}; +}; + +/** + * @brief Geometry of a slalom turn. + * + * @details A turn is one bend, or two bends joined by a straight, which is how a turn that fits inside + * one cell swings out before it turns in. The planner prices the whole curve at one speed, so the + * curvature and the sharpness that limit that speed are the largest of its bends. The robot then + * drives it at the speed each point allows, which is only faster. + * + * @note The straights before and after the curve place it between its entry and exit nodes. All + * lengths are in meters and the angles, in radians, are those of the turn to the left. + */ +struct TurnShape { + /** + * @brief Largest number of bends of a turn. + */ + static constexpr uint8_t max_bends{2}; + + /** + * @brief Point of the curve, in the frame of its start, for the turn to the left. + * + * @tparam T Floating point type. + */ + template + using Sample = TurnBend::Sample; + + /** + * @brief Evaluate the curve at a distance from its start. + * + * @tparam T Floating point type. + * @param distance Distance traveled along the curve, clamped to its length. + * @return The point of the curve, with the curvature and its rate of change there. + */ + template + constexpr Sample sample(T distance) const { + const TurnBend& first = this->bends.at(0); + const T first_length = static_cast(first.length()); + + if (this->number_of_bends < 2 or distance <= first_length) { + return first.template sample(distance); + } + + const Sample joint = first.template sample(first_length); + const T cosine = core::math::cos(joint.heading); + const T sine = core::math::sin(joint.heading); + const T straight = static_cast(this->straight_length); + const T along = distance - first_length; + + if (along <= straight) { + return { + .x = joint.x + along * cosine, + .y = joint.y + along * sine, + .heading = joint.heading, + .curvature = T{0}, + .sharpness = T{0}, + }; + } + + const Sample local = this->bends.at(1).template sample(along - straight); + const T start_x = joint.x + straight * cosine; + const T start_y = joint.y + straight * sine; + + return { + .x = start_x + cosine * local.x - sine * local.y, + .y = start_y + sine * local.x + cosine * local.y, + .heading = joint.heading + local.heading, + .curvature = local.curvature, + .sharpness = local.sharpness, + }; + } + + /** + * @brief Get how the curve bends at a distance from its start, without evaluating its points. + * + * @param distance Distance traveled along the curve. + * @return The curvature and the sharpness there. + */ + constexpr Bending bending_at(float distance) const { + const float first_length = this->bends.at(0).length(); + + if (this->number_of_bends < 2 or distance <= first_length) { + return this->bends.at(0).bending_at(distance); + } + + if (distance <= first_length + this->straight_length) { + return {.curvature = 0.0F, .sharpness = 0.0F}; + } + + return this->bends.at(1).bending_at(distance - first_length - this->straight_length); + } + + /** + * @brief Get the length of the curve. + * + * @return The length of the bends and of the straight between them in meters. + */ + constexpr float length() const { + if (this->number_of_bends < 2) { + return this->bends.at(0).length(); + } + + return this->bends.at(0).length() + this->straight_length + this->bends.at(1).length(); + } + /** * @brief Get the length of the whole turn, from its entry node to its exit node. * @@ -133,11 +278,17 @@ struct TurnShape { */ constexpr float total_length() const { return this->pre + this->length() + this->post; } + std::array bends{}; + uint8_t number_of_bends{}; + float straight_length{}; + + /** + * @brief Heading at the exit relative to the entry, and the largest curvature and sharpness. + */ float angle{}; float curvature{}; float sharpness{}; - float ramp_length{}; - float arc_length{}; + float pre{}; float post{}; @@ -152,6 +303,22 @@ struct TurnShape { bool valid{}; }; +/** + * @brief Parameters of a turn of two bends, found by a search too long to run in the compiler. + * + * @details The angles are those of the turn to the left, positive to the left, and the curvatures + * are the peaks asked of each bend. The straight between the bends and the one after them follow + * from the place of the exit node, so a design is only the choice of how the turn swings. + * + */ +struct TwoBendDesign { + float first_angle; + float first_curvature; + float second_angle; + float second_curvature; + float pre; +}; + /** * @brief Shape of every turn, computed from the geometry of the maze and of the robot. * @@ -166,17 +333,103 @@ struct TurnShape { class TurnTable { public: /** - * @brief Compute the shape of every turn. + * @brief Designs of the turns of two bends, from first_two_bend_turn on. + */ + using TwoBendDesigns = std::array; + + /** + * @brief Shapes of the turns of two bends, from first_two_bend_turn on. + */ + using TwoBendShapes = std::array; + + /** + * @brief Compute the shape of every turn of one bend, and take those of two. + * + * @note The turns of two bends are placed between their nodes by place(), and checked to clear + * the walls by clears(), each a constant expression of its own, since the compiler gives each one + * a budget and designing the turns of one bend takes most of this one. * * @param model Physical description of the robot and of the maze. * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + * @param two_bend_shapes Shape of each turn of two bends, for this margin. */ - constexpr TurnTable(const RobotModel& model, float margin) { + constexpr TurnTable(const RobotModel& model, float margin, const TwoBendShapes& two_bend_shapes) { const Designer designer{model, margin}; - for (uint8_t i = 0; i < number_of_turns; i++) { + for (uint8_t i = 0; i < first_two_bend_turn; i++) { this->shapes.at(i) = designer.design(static_cast(i)); } + + std::ranges::copy(two_bend_shapes, this->shapes.begin() + first_two_bend_turn); + } + + /** + * @brief Place every turn of two bends between its nodes. + * + * @param model Physical description of the robot and of the maze. + * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + * @param designs Design of each turn of two bends, for this margin. + * @return The shapes, with the valid flag of each cleared if it does not end on the exit node of + * its turn. + */ + static constexpr TwoBendShapes place(const RobotModel& model, float margin, const TwoBendDesigns& designs) { + const Designer designer{model, margin}; + TwoBendShapes shapes{}; + + for (uint8_t i = 0; i < number_of_two_bend_turns; i++) { + shapes.at(i) = designer.build(static_cast(first_two_bend_turn + i), designs.at(i)); + } + + return shapes; + } + + /** + * @brief Build the shape of a turn of two bends and check that it fits. + * + * @note This is what the compiler runs on every design, and what the search for the designs runs + * on every candidate, so a design is accepted by the same test that found it. + * + * @param model Physical description of the robot and of the maze. + * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + * @param turn The turn. + * @param design The parameters of the bends. + * @return The shape, with its valid flag cleared if it does not end on the exit node of the turn + * or does not clear every obstacle. + */ + static constexpr TurnShape check(const RobotModel& model, float margin, TurnId turn, const TwoBendDesign& design) { + return Designer{model, margin}.check(turn, design); + } + + /** + * @brief Check that every turn of two bends clears the walls. + * + * @param model Physical description of the robot and of the maze. + * @param margin Smallest distance allowed between the outline of the robot and an obstacle. + * @param designs Design of each turn of two bends, from first_two_bend_turn on, for this margin. + * @return True if every design ends on its exit node and clears every obstacle. + */ + static constexpr bool clears(const RobotModel& model, float margin, const TwoBendDesigns& designs) { + const Designer designer{model, margin}; + + for (uint8_t i = 0; i < number_of_two_bend_turns; i++) { + if (not designer.check(static_cast(first_two_bend_turn + i), designs.at(i)).valid) { + return false; + } + } + + return true; + } + + /** + * @brief Build one bend the way the turns are built. + * + * @param model Physical description of the robot. + * @param angle The angle of the bend, positive to the left. + * @param curvature The peak curvature asked for, which a bend too short for its ramps lowers. + * @return The bend. + */ + static constexpr TurnBend make_bend(const RobotModel& model, double angle, double curvature) { + return Designer{model, 0.0F}.make_bend(angle, curvature); } /** @@ -262,7 +515,135 @@ class TurnTable { return finish(this->place(layout, feasible), layout); } + /** + * @brief Build the shape of a turn of two bends and check that it fits. + * + * @param turn The turn. + * @param design The parameters of the bends. + * @return The shape, with its valid flag cleared if it does not end on the exit node of the + * turn or does not clear every obstacle. + */ + constexpr TurnShape check(TurnId turn, const TwoBendDesign& design) const { + TurnShape shape = this->build(turn, design); + + if (not shape.valid) { + return shape; + } + + const Layout layout = this->get_layout(get_primitive(turn)); + const double total = shape.total_length(); + const auto samples = static_cast(total / sample_spacing) + 2; + + for (uint16_t i = 0; i < samples; i++) { + if (not this->is_clear(get_placement(shape, layout, total * i / (samples - 1)), layout)) { + shape.valid = false; + return shape; + } + } + + return shape; + } + + /** + * @brief Place a turn of two bends between its nodes. + * + * @details Given the bends and the straight before them, the exit node leaves two unknowns, + * the straight between the bends and the one after them, and two equations for its position. + * The shape is valid when both straights come out positive and the bends turn the angle of + * the turn. Whether it clears the walls is left to check(). + * + * @param turn The turn. + * @param design The parameters of the bends. + * @return The shape, with its valid flag cleared if it does not end on the exit node. + */ + constexpr TurnShape build(TurnId turn, const TwoBendDesign& design) const { + const TurnPrimitive& primitive = get_primitive(turn); + + if (design.first_curvature <= 0.0F or design.second_curvature <= 0.0F or design.pre < 0.0F) { + return {}; + } + + const double first_angle = design.first_angle; + const double second_angle = design.second_angle; + const double turned = (first_angle + second_angle) / (std::numbers::pi / 4.0) - primitive.rotation; + const double revolutions = turned / 8.0; + const double rounded = revolutions < 0.0 ? -static_cast(static_cast(0.5 - revolutions)) : + static_cast(static_cast(revolutions + 0.5)); + + if (core::math::abs(turned - 8.0 * rounded) > 1.0e-4) { + return {}; + } + + const Candidate first = this->make_curve(core::math::abs(first_angle), design.first_curvature); + const Candidate second = this->make_curve(core::math::abs(second_angle), design.second_curvature); + + TurnShape shape{}; + shape.bends.at(0) = to_bend(first, first_angle); + shape.bends.at(1) = to_bend(second, second_angle); + shape.number_of_bends = 2; + shape.angle = static_cast(first_angle + second_angle); + shape.curvature = std::max(shape.bends.at(0).curvature, shape.bends.at(1).curvature); + shape.sharpness = std::max(shape.bends.at(0).sharpness, shape.bends.at(1).sharpness); + shape.pre = design.pre; + + const Layout layout = this->get_layout(primitive); + + const double entry = layout.entry_heading; + const double middle = entry + first_angle; + const double last = middle + second_angle; + const double determinant = core::math::sin(second_angle); + + if (core::math::abs(determinant) < 1.0e-6) { + return {}; + } + + const auto first_end = shape.bends.at(0).template sample(1.0e3); + const auto second_end = shape.bends.at(1).template sample(1.0e3); + const double remaining_x = + layout.exit_x - shape.pre * core::math::cos(entry) - + (core::math::cos(entry) * first_end.x - core::math::sin(entry) * first_end.y) - + (core::math::cos(middle) * second_end.x - core::math::sin(middle) * second_end.y); + const double remaining_y = + layout.exit_y - shape.pre * core::math::sin(entry) - + (core::math::sin(entry) * first_end.x + core::math::cos(entry) * first_end.y) - + (core::math::sin(middle) * second_end.x + core::math::cos(middle) * second_end.y); + + const double straight = + (remaining_x * core::math::sin(last) - remaining_y * core::math::cos(last)) / determinant; + const double post = + (core::math::cos(middle) * remaining_y - core::math::sin(middle) * remaining_x) / determinant; + + if (straight < -1.0e-6 or post < -1.0e-6) { + return {}; + } + + shape.straight_length = static_cast(straight > 0.0 ? straight : 0.0); + shape.post = static_cast(post > 0.0 ? post : 0.0); + shape.valid = true; + + return shape; + } + + /** + * @brief Build one bend. + * + * @param angle The angle of the bend, positive to the left. + * @param curvature The peak curvature asked for. + * @return The bend. + */ + constexpr TurnBend make_bend(double angle, double curvature) const { + return to_bend(this->make_curve(core::math::abs(angle), curvature), angle); + } + private: + /** + * @brief Largest distance between the points of a turn of two bends where the clearance is + * checked, in meters. + * + * @note The points are spread over the whole turn, straights included. + */ + static constexpr double sample_spacing{0.005}; + /** * @brief Number of sizes tried, from the widest down, before refining the first that fits. */ @@ -281,7 +662,7 @@ class TurnTable { /** * @brief Largest number of obstacles considered around a turn. */ - static constexpr uint8_t max_obstacles{48}; + static constexpr uint8_t max_obstacles{96}; /** * @brief Number of points of the curve where the clearance is checked. @@ -344,16 +725,33 @@ class TurnTable { */ static constexpr TurnShape to_shape(const Candidate& candidate, double angle) { TurnShape shape{}; + shape.bends.at(0) = to_bend(candidate, angle); + shape.number_of_bends = 1; shape.angle = static_cast(angle); shape.curvature = static_cast(candidate.curvature); shape.sharpness = static_cast(candidate.sharpness); - shape.ramp_length = static_cast(candidate.ramp); - shape.arc_length = static_cast(candidate.arc); shape.pre = static_cast(candidate.pre); shape.post = static_cast(candidate.post); return shape; } + /** + * @brief Build the bend of a candidate. + * + * @param candidate The candidate. + * @param angle The angle of the bend, positive to the left. + * @return The bend. + */ + static constexpr TurnBend to_bend(const Candidate& candidate, double angle) { + return { + .angle = static_cast(angle), + .curvature = static_cast(candidate.curvature), + .sharpness = static_cast(candidate.sharpness), + .ramp_length = static_cast(candidate.ramp), + .arc_length = static_cast(candidate.arc), + }; + } + /** * @brief Build the curve that turns a given angle with a given peak curvature. * @@ -418,6 +816,45 @@ class TurnTable { }; } + /** + * @brief Get the pose of the robot at a distance from the entry node, along a shape. + * + * @param shape The shape, with its straights. + * @param layout The place of the turn. + * @param distance Distance traveled from the entry node. + * @return The pose in the canonical frame. + */ + static constexpr Placement get_placement(const TurnShape& shape, const Layout& layout, double distance) { + const double cosine = core::math::cos(layout.entry_heading); + const double sine = core::math::sin(layout.entry_heading); + const double pre = shape.pre; + const double length = shape.length(); + + double local_x = distance; + double local_y = 0.0; + double heading = 0.0; + + if (distance > pre) { + const auto sample = shape.template sample(distance > pre + length ? length : distance - pre); + + local_x = pre + sample.x; + local_y = sample.y; + heading = sample.heading; + + if (distance > pre + length) { + const double extra = distance - pre - length; + local_x += extra * core::math::cos(heading); + local_y += extra * core::math::sin(heading); + } + } + + return { + .x = cosine * local_x - sine * local_y, + .y = sine * local_x + cosine * local_y, + .heading = layout.entry_heading + heading, + }; + } + /** * @brief Collect the geometry of a turn in its canonical frame. * @@ -437,7 +874,7 @@ class TurnTable { const double forward = cosine * layout.exit_x + sine * layout.exit_y; const double lateral = -sine * layout.exit_x + cosine * layout.exit_y; - if (primitive.rotation != 4) { + if (primitive.rotation % 4 != 0) { layout.tangent_out = lateral / core::math::sin(layout.angle); layout.tangent_in = forward - layout.tangent_out * core::math::cos(layout.angle); } @@ -553,9 +990,6 @@ class TurnTable { } const double length = 2.0 * candidate.ramp + candidate.arc; - const double longest = this->front_length > this->rear_length ? this->front_length : this->rear_length; - const double reach = - core::math::sqrt(this->half_width * this->half_width + longest * longest) + this->margin; for (uint8_t i = 0; i <= clearance_samples + 1; i++) { double distance = candidate.pre + length * (i - 1) / (clearance_samples - 1); @@ -566,19 +1000,36 @@ class TurnTable { distance = candidate.pre + length + candidate.post; } - const Placement placement = get_placement(candidate, layout, distance); + if (not this->is_clear(get_placement(candidate, layout, distance), layout)) { + return false; + } + } - for (uint8_t j = 0; j < layout.number_of_obstacles; j++) { - const Rectangle& obstacle = layout.obstacles.at(j); + return true; + } - if (core::math::abs(obstacle.center_x - placement.x) > reach + obstacle.half_x or - core::math::abs(obstacle.center_y - placement.y) > reach + obstacle.half_y) { - continue; - } + /** + * @brief Check if the robot keeps clear of every obstacle at one pose. + * + * @param placement The pose of the robot. + * @param layout The place of the turn. + * @return True if the robot stays at least the margin away from every obstacle. + */ + constexpr bool is_clear(const Placement& placement, const Layout& layout) const { + const double longest = this->front_length > this->rear_length ? this->front_length : this->rear_length; + const double reach = + core::math::sqrt(this->half_width * this->half_width + longest * longest) + this->margin; - if (this->touches(placement, obstacle)) { - return false; - } + for (uint8_t j = 0; j < layout.number_of_obstacles; j++) { + const Rectangle& obstacle = layout.obstacles.at(j); + + if (core::math::abs(obstacle.center_x - placement.x) > reach + obstacle.half_x or + core::math::abs(obstacle.center_y - placement.y) > reach + obstacle.half_y) { + continue; + } + + if (this->touches(placement, obstacle)) { + return false; } } diff --git a/micras_nav/src/route_compiler.cpp b/micras_nav/src/route_compiler.cpp index b21f31a..9a1debd 100644 --- a/micras_nav/src/route_compiler.cpp +++ b/micras_nav/src/route_compiler.cpp @@ -34,7 +34,6 @@ void RouteCompiler::compile( .length = length, .start_speed = 0.0F, .end_speed = 0.0F, - .max_speed = 0.0F, .start = start, }); } @@ -66,10 +65,9 @@ void RouteCompiler::compile( segments.push_back({ .kind = SegmentKind::TURN, .turn = step.turn, - .length = step.side == TurnSide::LEFT ? shape.angle : -shape.angle, + .length = step.side == TurnSide::LEFT ? shape.length() : -shape.length(), .start_speed = 0.0F, .end_speed = 0.0F, - .max_speed = 0.0F, .start = retreat(node, -shape.pre), }); From f9250c1572cf5257a4029bffa82d7803da208edf Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 25 Sep 2026 20:27:47 -0300 Subject: [PATCH 505/561] =?UTF-8?q?=E2=9C=A8=20Brake=20and=20accelerate=20?= =?UTF-8?q?through=20the=20turns=20as=20their=20curvature=20allows?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A turn was driven at one speed, the one its tightest point allows, so its two ends had the same speed and two turns too close together were both slowed to what the straight between them could change. A turn is now sampled every 5 mm, each sample gets the speed its curvature and sharpness allow, and passes from the start and from the end lower it to what can be reached and braked with the grip the curve leaves: the lateral part takes its share as on a friction circle, the angular part from the push each tire has left, and a change of speed on a curve also counts for the angular acceleration it causes. CurveSpeed holds the passes, over any curve and over storage the caller owns, and the samples of one turn for the executor to play back with a constant acceleration between two of them. The velocity planner gives a turn different speeds at its two ends. The planner still prices a turn at one speed, and the four best routes are timed with the new rule, so the one chosen is exactly timed. A single bend gains nothing, since its ramps already enter the arc at its speed; the turns of two bends, the straight between two bends and turns next to each other do. On ten saved mazes the fast runs are 1.5 to 1.8 % faster in every profile. The segment loses its max_speed, which nothing read. --- micras_nav/include/micras/nav/curve_speed.hpp | 184 ++++++++++++++++++ .../include/micras/nav/impl/curve_speed.tpp | 53 +++++ .../include/micras/nav/motion_limits.hpp | 67 ++++++- micras_nav/include/micras/nav/segment.hpp | 13 +- .../include/micras/nav/velocity_planner.hpp | 14 +- micras_nav/src/curve_speed.cpp | 154 +++++++++++++++ micras_nav/src/motion_limits.cpp | 51 +++++ micras_nav/src/velocity_planner.cpp | 37 +++- 8 files changed, 548 insertions(+), 25 deletions(-) create mode 100644 micras_nav/include/micras/nav/curve_speed.hpp create mode 100644 micras_nav/include/micras/nav/impl/curve_speed.tpp create mode 100644 micras_nav/src/curve_speed.cpp diff --git a/micras_nav/include/micras/nav/curve_speed.hpp b/micras_nav/include/micras/nav/curve_speed.hpp new file mode 100644 index 0000000..2939272 --- /dev/null +++ b/micras_nav/include/micras/nav/curve_speed.hpp @@ -0,0 +1,184 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_CURVE_SPEED_HPP +#define MICRAS_NAV_CURVE_SPEED_HPP + +#include +#include +#include + +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +/** + * @brief Fastest motion along a curve between two speeds, as a function of time. + * + * @details The curve is sampled at equal steps of distance. Each sample gets the largest speed its + * bending allows, a pass from the start then lowers every sample to what can be reached from the one + * before, with the grip the curve leaves there, and a pass from the end lowers it to what can be + * braked from before the one after. Between two samples the acceleration is constant. A turn is + * therefore braked into as long as its curvature leaves grip to spare and accelerated out of as + * soon as it does, instead of being driven at the speed of its tightest point. + * + * The passes are written over any curve, given as a function of the index of a sample that returns + * the curvature at that sample and the sharpness between it and the next one, and over storage that + * the caller owns, which is how the racing line uses them over thousands of samples. An object of + * this class holds the samples of one turn, which is all the executor needs to play it back. + */ +class CurveSpeed { +public: + /** + * @brief Largest number of steps a turn is sampled with. + */ + static constexpr uint8_t max_intervals{128}; + + /** + * @brief Distance between two samples of a turn, in meters, which the longest turns stretch. + */ + static constexpr float turn_spacing{0.005F}; + + /** + * @brief Construct an empty motion, of zero duration. + */ + CurveSpeed() = default; + + /** + * @brief Construct the motion along a turn. + * + * @note Speeds the turn does not allow are lowered, like the velocity planner would lower them, + * so the motion always covers the turn. + * + * @param shape The shape of the turn. + * @param start_speed The speed at the start of the curve. + * @param end_speed The speed wanted at the end of the curve. + * @param limits The limits of the run. + */ + CurveSpeed(const TurnShape& shape, float start_speed, float end_speed, const CurveLimits& limits); + + /** + * @brief Get the time the motion takes. + * + * @return The duration in seconds. + */ + float duration() const; + + /** + * @brief Evaluate the motion at an instant. + * + * @param time Time since the start of the motion, clamped to its duration. + * @return The distance covered, the speed and the acceleration at that instant. + */ + SpeedProfile::Sample sample(float time) const; + + /** + * @brief Get the largest speed a turn can be entered at. + * + * @param shape The shape of the turn. + * @param end_speed The speed wanted at the end of the curve. + * @param limits The limits of the run. + * @return The speed at the start when braking all the way to the end speed. + */ + static float get_entry_speed(const TurnShape& shape, float end_speed, const CurveLimits& limits); + + /** + * @brief Get the largest speed a turn can be left at. + * + * @param shape The shape of the turn. + * @param start_speed The speed at the start of the curve. + * @param limits The limits of the run. + * @return The speed at the end when accelerating all the way from the start speed. + */ + static float get_exit_speed(const TurnShape& shape, float start_speed, const CurveLimits& limits); + + /** + * @brief Fill in the speed of every sample of a curve. + * + * @tparam F Type of the function giving the bending at a sample, as described for the class. + * @param bending The bending of the curve, called for every sample. + * @param speeds The speed of each sample, one more than there are steps. + * @param spacing The distance between two samples. + * @param start_speed The speed at the first sample. + * @param end_speed The speed wanted at the last sample. + * @param limits The limits of the run. + */ + template + static void plan( + F&& bending, std::span speeds, float spacing, float start_speed, float end_speed, + const CurveLimits& limits + ); + + /** + * @brief Get the time at which every sample of a curve is reached. + * + * @param speeds The speed of each sample. + * @param spacing The distance between two samples. + * @param times The time of each sample, the first being zero. + */ + static void integrate(std::span speeds, float spacing, std::span times); + + /** + * @brief Evaluate the motion along a curve at an instant. + * + * @param speeds The speed of each sample. + * @param times The time of each sample. + * @param spacing The distance between two samples. + * @param time Time since the start of the motion, clamped to its duration. + * @return The distance covered, the speed and the acceleration at that instant. + */ + static SpeedProfile::Sample + sample(std::span speeds, std::span times, float spacing, float time); + +private: + /** + * @brief Smallest sum of the speeds of two samples a step is timed with, in m/s, which only + * matters for a curve that starts or ends at rest. + */ + static constexpr float min_speed_sum{1.0e-3F}; + + /** + * @brief Get the number of steps a turn is sampled with. + * + * @param shape The shape of the turn. + * @return The number of steps. + */ + static uint8_t get_intervals(const TurnShape& shape); + + /** + * @brief Get the largest speed at a sample. + * + * @param before The bending between the sample before and this one. + * @param at The bending at this sample and between it and the next one. + * @param limits The limits of the run. + * @return The speed limit, which takes the sharper side of the sample. + */ + static float get_limit(const Bending& before, const Bending& at, const CurveLimits& limits); + + /** + * @brief Speed at every sample. + */ + std::array speeds{}; + + /** + * @brief Time at which every sample is reached. + */ + std::array times{}; + + /** + * @brief Number of steps between the samples. + */ + uint8_t intervals{}; + + /** + * @brief Distance between two samples. + */ + float spacing{}; +}; +} // namespace micras::nav + +#include "micras/nav/impl/curve_speed.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_CURVE_SPEED_HPP diff --git a/micras_nav/include/micras/nav/impl/curve_speed.tpp b/micras_nav/include/micras/nav/impl/curve_speed.tpp new file mode 100644 index 0000000..200df49 --- /dev/null +++ b/micras_nav/include/micras/nav/impl/curve_speed.tpp @@ -0,0 +1,53 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_CURVE_SPEED_TPP +#define MICRAS_NAV_CURVE_SPEED_TPP + +#include +#include +#include +#include + +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +template +void CurveSpeed::plan( + F&& bending, std::span speeds, float spacing, float start_speed, float end_speed, const CurveLimits& limits +) { + if (speeds.empty()) { + return; + } + + const std::size_t last = speeds.size() - 1; + + Bending before = bending(0); + + for (std::size_t i = 0; i <= last; i++) { + const Bending at = bending(i); + speeds[i] = get_limit(before, at, limits); + before = at; + } + + speeds[0] = std::min(speeds[0], start_speed); + + for (std::size_t i = 0; i < last; i++) { + const float acceleration = limits.get_acceleration(speeds[i], bending(i)); + speeds[i + 1] = std::min(speeds[i + 1], std::sqrt(speeds[i] * speeds[i] + 2.0F * spacing * acceleration)); + } + + speeds[last] = std::min(speeds[last], end_speed); + + for (std::size_t i = last; i > 0; i--) { + const Bending at{.curvature = bending(i).curvature, .sharpness = bending(i - 1).sharpness}; + const float deceleration = limits.get_deceleration(speeds[i], at); + speeds[i - 1] = std::min(speeds[i - 1], std::sqrt(speeds[i] * speeds[i] + 2.0F * spacing * deceleration)); + } +} +} // namespace micras::nav + +#endif // MICRAS_NAV_CURVE_SPEED_TPP diff --git a/micras_nav/include/micras/nav/motion_limits.hpp b/micras_nav/include/micras/nav/motion_limits.hpp index 3f9564d..9bf5969 100644 --- a/micras_nav/include/micras/nav/motion_limits.hpp +++ b/micras_nav/include/micras/nav/motion_limits.hpp @@ -13,11 +13,13 @@ namespace micras::nav { * @brief How a run trades time for safety. * * @note The utilization is the fraction of the available traction the run may ask for, in (0, 1]. - * Whether the run is risky selects the turn table built with the smaller safety margin. The speed - * limit is what makes the search run slow: a fast run leaves it above what the robot can reach. + * Whether the run is risky selects the turn table built with the smaller safety margin. The racing + * line replaces the route by the smoothest line through + * its cells, which only a fast run planned with the robot stopped can drive. The speed limit is what + * makes the search run slow: a fast run leaves it above what the robot can reach. */ struct RunProfile { - bool diagonal; + bool racing_line; bool fan; bool risky; float utilization; @@ -55,6 +57,52 @@ struct MotionLimits { float motor_speed; }; +/** + * @brief Limits of a motion along a curve, where the tires share their grip between turning and + * changing speed. + * + * @details The speed at a point is limited by the lateral acceleration its curvature asks for and + * by the angular acceleration its sharpness asks for, each against what the tires can give, which + * is the rule a turn has always been driven by. What is left of the grip there can change the + * speed: the lateral part takes its share as on a friction circle, and the angular part, which the + * two tires make by pushing in opposite directions, takes it from the push each tire has left. A + * change of speed on a curve also changes the angular speed, and that angular acceleration is + * counted too. + * + * @note On a straight nothing is taken and the limits are those of the linear motion. + */ +struct CurveLimits { + /** + * @brief Get the largest speed at a point. + * + * @param bending How the path bends there. + * @return The speed in m/s. + */ + float get_speed_limit(const Bending& bending) const; + + /** + * @brief Get the largest acceleration at a point, at a speed. + * + * @param speed The speed there. + * @param bending How the path bends there. + * @return The acceleration along the path in m/s^2, which is zero where the grip is all used. + */ + float get_acceleration(float speed, const Bending& bending) const; + + /** + * @brief Get the largest deceleration at a point, at a speed. + * + * @param speed The speed there. + * @param bending How the path bends there. + * @return The deceleration along the path in m/s^2, as a positive number. + */ + float get_deceleration(float speed, const Bending& bending) const; + + MotionLimits linear; + float lateral; + float angular; +}; + /** * @brief Derivation of the limits of a run from the physical model of the robot. */ @@ -101,6 +149,14 @@ class Dynamics { */ MotionLimits get_angular_limits(const RunProfile& profile) const; + /** + * @brief Get the limits of a motion along a curve. + * + * @param profile The profile of the run. + * @return The limits, the linear ones being those of a straight. + */ + CurveLimits get_curve_limits(const RunProfile& profile) const; + /** * @brief Get the shape of a turn for a run. * @@ -111,10 +167,11 @@ class Dynamics { const TurnShape& get_turn(const RunProfile& profile, TurnId turn) const; /** - * @brief Get the speed a turn is driven at. + * @brief Get the speed a turn is priced at. * * @details The largest speed at which neither the lateral acceleration at the peak curvature - * nor the angular acceleration along the ramps exceeds what the tires can give to this run. + * nor the angular acceleration along the ramps exceeds what the tires can give to this run. It + * is the slowest point of the turn, which the robot may drive faster where the turn allows. * * @param profile The profile of the run. * @param turn The turn. diff --git a/micras_nav/include/micras/nav/segment.hpp b/micras_nav/include/micras/nav/segment.hpp index 15bd272..7278d6a 100644 --- a/micras_nav/include/micras/nav/segment.hpp +++ b/micras_nav/include/micras/nav/segment.hpp @@ -17,19 +17,21 @@ namespace micras::nav { enum class SegmentKind : uint8_t { STRAIGHT = 0, // Straight line along the grid. DIAGONAL = 1, // Straight line along a diagonal. - TURN = 2, // Slalom turn from the turn table, at a constant linear speed. + TURN = 2, // Slalom turn from the turn table, braked and accelerated as its curvature allows. SPIN = 3, // Rotation in place. STOP = 4, // Stand still for a given time. ATTACH = 5, // Stand still facing a wall until the pose settles, or for a given time at most. + LINE = 6, // The racing line, from the start to the goal, at the speeds it was planned with. }; /** * @brief One motion of a route, as a plain value. * - * @details The length is in meters for a straight and negative to drive it backwards, in radians for - * a turn and for a rotation in place, positive to the left, and in seconds for the two kinds that - * stand still. The speeds are those at the ends of the segment and are filled in by the velocity - * planner, as is the largest speed allowed along it. + * @details The length is in meters for a straight and negative to drive it backwards, in meters of + * curve for a turn, negative for the mirror of the turn of the table, which is the turn to the right, + * in radians for a rotation in place, positive to the left, in seconds for the two kinds that stand + * still, and in meters for the racing line. The speeds are those at the ends of the segment and are + * filled in by the velocity planner, except for the racing line, which carries its own. * * @note The start is the pose the segment nominally starts from in the maze frame. It comes from the * route on the grid and never from a measurement, so the references generated from it do not move @@ -42,7 +44,6 @@ struct Segment { float length; float start_speed; float end_speed; - float max_speed; Pose start; }; diff --git a/micras_nav/include/micras/nav/velocity_planner.hpp b/micras_nav/include/micras/nav/velocity_planner.hpp index bda9cca..fcb4c0d 100644 --- a/micras_nav/include/micras/nav/velocity_planner.hpp +++ b/micras_nav/include/micras/nav/velocity_planner.hpp @@ -14,12 +14,12 @@ namespace micras::nav { /** * @brief Choice of the speed at every junction of a route. * - * @details Every turn starts at the speed its shape allows and every straight at the speed limit. - * A pass from the end to the start then lowers each junction to what can be braked from before the - * next one, and a pass from the start to the end lowers it to what can be reached from the previous - * one. A turn is driven at a single speed, so lowering one of its ends lowers the other, which is - * how two turns too close together slow each other down. What is left is the fastest set of speeds - * the limits allow. + * @details Every turn starts and ends at the speed its shape allows there and every straight at the + * speed limit. A pass from the end to the start then lowers each junction to what can be braked from + * before the next one, and a pass from the start to the end lowers it to what can be reached from + * the previous one. A turn is braked and accelerated through as its curvature leaves grip to spare, + * which is how two turns too close together slow each other down: the grip they have left has to + * make up for the straight they lack. What is left is the fastest set of speeds the limits allow. */ class VelocityPlanner { public: @@ -46,6 +46,8 @@ class VelocityPlanner { /** * @brief Get the time a segment takes, with the speeds it already has. * + * @note A racing line is timed by the line itself, and gets zero here. + * * @param segment The segment. * @param dynamics The physical limits of the robot. * @param profile The profile of the run. diff --git a/micras_nav/src/curve_speed.cpp b/micras_nav/src/curve_speed.cpp new file mode 100644 index 0000000..04cdfc4 --- /dev/null +++ b/micras_nav/src/curve_speed.cpp @@ -0,0 +1,154 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +namespace { +/** + * @brief Bending of a turn at its samples, as the passes of CurveSpeed read it. + */ +struct TurnBending { + /** + * @brief Get the curvature at a sample and the sharpness between it and the next one. + * + * @param index The index of the sample. + * @return The bending. + */ + Bending operator()(std::size_t index) const { + const auto position = static_cast(index); + const float middle = std::min(position + 0.5F, static_cast(this->intervals) - 0.5F); + + return { + .curvature = this->shape.bending_at(position * this->spacing).curvature, + .sharpness = this->shape.bending_at(middle * this->spacing).sharpness, + }; + } + + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) a view that lives for one call + const TurnShape& shape; + uint8_t intervals; + float spacing; +}; +} // namespace + +CurveSpeed::CurveSpeed(const TurnShape& shape, float start_speed, float end_speed, const CurveLimits& limits) : + intervals{get_intervals(shape)}, spacing{shape.length() / static_cast(get_intervals(shape))} { + const std::span speeds = std::span{this->speeds}.first(this->intervals + 1U); + + plan( + TurnBending{.shape = shape, .intervals = this->intervals, .spacing = this->spacing}, speeds, this->spacing, + start_speed, end_speed, limits + ); + integrate(speeds, this->spacing, std::span{this->times}.first(this->intervals + 1U)); +} + +float CurveSpeed::duration() const { + return this->times.at(this->intervals); +} + +SpeedProfile::Sample CurveSpeed::sample(float time) const { + return sample( + std::span{this->speeds}.first(this->intervals + 1U), std::span{this->times}.first(this->intervals + 1U), + this->spacing, time + ); +} + +float CurveSpeed::get_entry_speed(const TurnShape& shape, float end_speed, const CurveLimits& limits) { + const uint8_t intervals = get_intervals(shape); + const float spacing = shape.length() / static_cast(intervals); + const TurnBending bending{.shape = shape, .intervals = intervals, .spacing = spacing}; + + Bending after = bending(intervals); + float speed = std::min(end_speed, get_limit(bending(intervals - 1U), after, limits)); + + for (uint8_t i = intervals; i > 0; i--) { + const Bending at = bending(i - 1U); + const Bending before = bending(i > 1 ? i - 2U : 0U); + const float deceleration = + limits.get_deceleration(speed, {.curvature = after.curvature, .sharpness = at.sharpness}); + + speed = std::min(std::sqrt(speed * speed + 2.0F * spacing * deceleration), get_limit(before, at, limits)); + after = at; + } + + return speed; +} + +float CurveSpeed::get_exit_speed(const TurnShape& shape, float start_speed, const CurveLimits& limits) { + const uint8_t intervals = get_intervals(shape); + const float spacing = shape.length() / static_cast(intervals); + const TurnBending bending{.shape = shape, .intervals = intervals, .spacing = spacing}; + + Bending at = bending(0); + float speed = std::min(start_speed, get_limit(at, at, limits)); + + for (uint8_t i = 0; i < intervals; i++) { + const Bending next = bending(i + 1U); + const float acceleration = limits.get_acceleration(speed, at); + + speed = std::min(std::sqrt(speed * speed + 2.0F * spacing * acceleration), get_limit(at, next, limits)); + at = next; + } + + return speed; +} + +void CurveSpeed::integrate(std::span speeds, float spacing, std::span times) { + if (times.empty()) { + return; + } + + times[0] = 0.0F; + + for (std::size_t i = 1; i < times.size(); i++) { + times[i] = times[i - 1] + 2.0F * spacing / std::max(speeds[i - 1] + speeds[i], min_speed_sum); + } +} + +SpeedProfile::Sample + CurveSpeed::sample(std::span speeds, std::span times, float spacing, float time) { + if (times.size() < 2) { + return {.distance = 0.0F, .speed = speeds.empty() ? 0.0F : speeds.front(), .acceleration = 0.0F}; + } + + time = std::clamp(time, 0.0F, times.back()); + + const auto next = std::ranges::upper_bound(times, time); + std::size_t index = next == times.begin() ? 0 : static_cast(next - times.begin()) - 1; + index = std::min(index, times.size() - 2); + + const float start = speeds[index]; + const float end = speeds[index + 1]; + const float acceleration = (end * end - start * start) / (2.0F * spacing); + const float elapsed = std::min(time - times[index], times[index + 1] - times[index]); + + return { + .distance = static_cast(index) * spacing + + std::min(start * elapsed + acceleration * elapsed * elapsed / 2.0F, spacing), + .speed = start + acceleration * elapsed, + .acceleration = acceleration, + }; +} + +uint8_t CurveSpeed::get_intervals(const TurnShape& shape) { + const auto wanted = static_cast(std::ceil(shape.length() / turn_spacing)); + return static_cast(std::clamp(wanted, 1, max_intervals)); +} + +float CurveSpeed::get_limit(const Bending& before, const Bending& at, const CurveLimits& limits) { + return limits.get_speed_limit( + {.curvature = at.curvature, .sharpness = std::max(std::abs(before.sharpness), std::abs(at.sharpness))} + ); +} +} // namespace micras::nav diff --git a/micras_nav/src/motion_limits.cpp b/micras_nav/src/motion_limits.cpp index 81fc099..1b0e50f 100644 --- a/micras_nav/src/motion_limits.cpp +++ b/micras_nav/src/motion_limits.cpp @@ -18,6 +18,49 @@ float MotionLimits::crossover_speed() const { return this->motor_speed * (1.0F - this->acceleration / this->motor_acceleration); } +float CurveLimits::get_speed_limit(const Bending& bending) const { + const float curvature = std::abs(bending.curvature); + const float sharpness = std::abs(bending.sharpness); + + float limit = this->linear.max_speed; + + if (curvature > 0.0F) { + limit = std::min(limit, std::sqrt(this->lateral / curvature)); + } + + if (sharpness > 0.0F) { + limit = std::min(limit, std::sqrt(this->angular / sharpness)); + } + + return limit; +} + +float CurveLimits::get_acceleration(float speed, const Bending& bending) const { + const float lateral_use = speed * speed * std::abs(bending.curvature) / this->lateral; + const float angular_use = speed * speed * std::abs(bending.sharpness) / this->angular; + const float spare = std::sqrt(std::max(1.0F - lateral_use * lateral_use, 0.0F)) - angular_use; + + if (spare <= 0.0F) { + return 0.0F; + } + + const float grip = spare / (1.0F / this->linear.acceleration + std::abs(bending.curvature) / this->angular); + + return std::max(std::min(grip, this->linear.acceleration_at(speed)), 0.0F); +} + +float CurveLimits::get_deceleration(float speed, const Bending& bending) const { + const float lateral_use = speed * speed * std::abs(bending.curvature) / this->lateral; + const float angular_use = speed * speed * std::abs(bending.sharpness) / this->angular; + const float spare = std::sqrt(std::max(1.0F - lateral_use * lateral_use, 0.0F)) - angular_use; + + if (spare <= 0.0F) { + return 0.0F; + } + + return spare / (1.0F / this->linear.deceleration + std::abs(bending.curvature) / this->angular); +} + Dynamics::Dynamics(const Config& config) : model{config.model}, turns{config.turns}, @@ -54,6 +97,14 @@ MotionLimits Dynamics::get_angular_limits(const RunProfile& profile) const { }; } +CurveLimits Dynamics::get_curve_limits(const RunProfile& profile) const { + return { + .linear = this->get_linear_limits(profile), + .lateral = profile.utilization * this->model.traction_acceleration(profile.fan), + .angular = profile.utilization * this->model.traction_angular_acceleration(profile.fan), + }; +} + const TurnShape& Dynamics::get_turn(const RunProfile& profile, TurnId turn) const { return profile.risky ? this->risky_turns.get(turn) : this->turns.get(turn); } diff --git a/micras_nav/src/velocity_planner.cpp b/micras_nav/src/velocity_planner.cpp index 27e8947..2be0c70 100644 --- a/micras_nav/src/velocity_planner.cpp +++ b/micras_nav/src/velocity_planner.cpp @@ -7,9 +7,11 @@ #include #include +#include "micras/nav/curve_speed.hpp" #include "micras/nav/motion_limits.hpp" #include "micras/nav/segment.hpp" #include "micras/nav/speed_profile.hpp" +#include "micras/nav/turn_table.hpp" #include "micras/nav/velocity_planner.hpp" namespace micras::nav { @@ -21,23 +23,25 @@ float VelocityPlanner::plan( } const MotionLimits linear_limits = dynamics.get_linear_limits(profile); + const CurveLimits curve_limits = dynamics.get_curve_limits(profile); const auto is_straight = [](const Segment& segment) { return segment.kind == SegmentKind::STRAIGHT or segment.kind == SegmentKind::DIAGONAL; }; for (Segment& segment : route) { - float limit = 0.0F; + segment.start_speed = 0.0F; + segment.end_speed = 0.0F; if (is_straight(segment)) { - limit = linear_limits.max_speed; + segment.start_speed = linear_limits.max_speed; + segment.end_speed = linear_limits.max_speed; } else if (segment.kind == SegmentKind::TURN) { - limit = dynamics.get_turn_speed(profile, segment.turn); - } + const TurnShape& shape = dynamics.get_turn(profile, segment.turn); - segment.max_speed = limit; - segment.start_speed = limit; - segment.end_speed = limit; + segment.start_speed = curve_limits.get_speed_limit(shape.bending_at(0.0F)); + segment.end_speed = curve_limits.get_speed_limit(shape.bending_at(shape.length())); + } } float next_speed = end_speed; @@ -50,6 +54,11 @@ float VelocityPlanner::plan( segment.start_speed, SpeedProfile::get_brakeable_speed(std::abs(segment.length), segment.end_speed, linear_limits) ); + } else if (segment.kind == SegmentKind::TURN) { + segment.start_speed = std::min( + segment.start_speed, + CurveSpeed::get_entry_speed(dynamics.get_turn(profile, segment.turn), segment.end_speed, curve_limits) + ); } else { segment.start_speed = std::min(segment.start_speed, segment.end_speed); } @@ -67,6 +76,11 @@ float VelocityPlanner::plan( segment.end_speed, SpeedProfile::get_reachable_speed(std::abs(segment.length), segment.start_speed, linear_limits) ); + } else if (segment.kind == SegmentKind::TURN) { + segment.end_speed = std::min( + segment.end_speed, + CurveSpeed::get_exit_speed(dynamics.get_turn(profile, segment.turn), segment.start_speed, curve_limits) + ); } else { segment.end_speed = std::min(segment.end_speed, segment.start_speed); } @@ -93,7 +107,14 @@ float VelocityPlanner::get_duration(const Segment& segment, const Dynamics& dyna .duration(); case SegmentKind::TURN: - return dynamics.get_turn(profile, segment.turn).length() / segment.start_speed; + return CurveSpeed{ + dynamics.get_turn(profile, segment.turn), segment.start_speed, segment.end_speed, + dynamics.get_curve_limits(profile) + } + .duration(); + + case SegmentKind::LINE: + return 0.0F; case SegmentKind::SPIN: return SpeedProfile{std::abs(segment.length), 0.0F, 0.0F, dynamics.get_angular_limits(profile)}.duration(); From f4a55cb055cb0be10550275c395565e7462899fb Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 25 Sep 2026 20:27:59 -0300 Subject: [PATCH 506/561] =?UTF-8?q?=E2=9C=A8=20Drive=20a=20racing=20line?= =?UTF-8?q?=20through=20the=20cells=20of=20the=20route,=20on=20the=20secon?= =?UTF-8?q?d=20switch?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Diagonals are always allowed now, so the map is complete for four profiles instead of eight, and the switch that allowed them selects the racing line. Once the route of a fast run is planned, it is sampled every 10 mm and each sample slides sideways, within what keeps the outline of the robot 15 mm from every post and from every wall the route does not cross, to make the sum of the squared curvatures smallest. It is solved in windows of 64 samples by a projected Newton method on the banded normal equations, twelve sweeps with a resampling after each, spread over the iterations of the PLAN state, and every sample is checked at the end. The speeds follow the rule of the turns, with 0.8 of the lateral grip of the run, and the executor plays the line back as one segment. If no line is found, or it is not faster, the robot drives the route. With the risky switch, a second line goes through the route of the risky turns, coming as close as they do, and the robot drives it only if it is the faster of the two; since only one line is kept, it is then optimized again. The time of a line is known before the run, since the run follows the planned reference in time; whether it touches a wall is not. On ten saved mazes the line is 5.5 % faster than the route with the fan and boost, clean on every run; at the full grip or at 10 mm from the walls it hit them. The line takes 57 KB of RAM for up to 25.6 m, and about 18 KB of flash. --- micras_nav/include/micras/nav/executor.hpp | 18 +- .../include/micras/nav/impl/mission.tpp | 128 ++- .../include/micras/nav/impl/racing_line.tpp | 927 ++++++++++++++++++ micras_nav/include/micras/nav/line.hpp | 156 +++ micras_nav/include/micras/nav/mission.hpp | 75 +- micras_nav/include/micras/nav/racing_line.hpp | 492 ++++++++++ micras_nav/src/executor.cpp | 56 +- micras_nav/src/line.cpp | 83 ++ 8 files changed, 1887 insertions(+), 48 deletions(-) create mode 100644 micras_nav/include/micras/nav/impl/racing_line.tpp create mode 100644 micras_nav/include/micras/nav/line.hpp create mode 100644 micras_nav/include/micras/nav/racing_line.hpp create mode 100644 micras_nav/src/line.cpp diff --git a/micras_nav/include/micras/nav/executor.hpp b/micras_nav/include/micras/nav/executor.hpp index bda4571..5ed4836 100644 --- a/micras_nav/include/micras/nav/executor.hpp +++ b/micras_nav/include/micras/nav/executor.hpp @@ -9,6 +9,8 @@ #include #include +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/line.hpp" #include "micras/nav/motion_limits.hpp" #include "micras/nav/segment.hpp" #include "micras/nav/speed_profile.hpp" @@ -45,9 +47,10 @@ class Executor { * @brief Construct a new Executor object. * * @param dynamics The physical limits of the robot, borrowed for the lifetime of the executor. + * @param line The racing line a segment of that kind plays back, borrowed for the same time. * @param config The configuration for the executor. */ - Executor(const Dynamics& dynamics, const Config& config); + Executor(const Dynamics& dynamics, const Line& line, const Config& config); /** * @brief Drop every segment and stand still at a pose. @@ -151,6 +154,12 @@ class Executor { // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot const Dynamics& dynamics; + /** + * @brief Racing line, for the segment that drives it. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const Line& line; + /** * @brief Tolerances to consider the robot settled. */ @@ -192,10 +201,15 @@ class Executor { float duration{}; /** - * @brief Motion along the segment in progress, for the kinds that have one. + * @brief Motion along the segment in progress, for the straights and the rotations in place. */ SpeedProfile speed_profile; + /** + * @brief Motion along the segment in progress, for the turns. + */ + CurveSpeed curve_speed; + /** * @brief Time the robot has been within the settling tolerances. */ diff --git a/micras_nav/include/micras/nav/impl/mission.tpp b/micras_nav/include/micras/nav/impl/mission.tpp index adcdac8..53c683a 100644 --- a/micras_nav/include/micras/nav/impl/mission.tpp +++ b/micras_nav/include/micras/nav/impl/mission.tpp @@ -18,6 +18,7 @@ #include "micras/core/types.hpp" #include "micras/nav/executor.hpp" #include "micras/nav/grid_pose.hpp" +#include "micras/nav/line.hpp" #include "micras/nav/localizer.hpp" #include "micras/nav/measurements.hpp" #include "micras/nav/route_compiler.hpp" @@ -35,7 +36,8 @@ TMission::TMission(const Dynamics& dynamics, const WallModel& wal observer{config.observer}, planner{dynamics, config.planner}, explorer{planner, config.map_profiles}, - executor{dynamics, config.executor}, + racing_line{dynamics, config.racing_line}, + executor{dynamics, racing_line.get_line(), config.executor}, search_speed{std::min( dynamics.get_linear_limits(config.search_profile).max_speed, dynamics.get_turn_speed(config.search_profile, TurnId::SS90S) @@ -43,6 +45,9 @@ TMission::TMission(const Dynamics& dynamics, const WallModel& wal this->solve_segments.reserve(config.executor.capacity); this->candidate_segments.reserve(config.executor.capacity); this->candidate_route.steps.reserve(config.executor.capacity); + this->solve_route.steps.reserve(config.executor.capacity); + this->careful_segments.reserve(config.executor.capacity); + this->careful_route.steps.reserve(config.executor.capacity); this->observer.reset(); } @@ -70,33 +75,103 @@ void TMission::begin_plan(const RunProfile& profile) { this->solve_segments.clear(); this->route_time = std::numeric_limits::infinity(); this->planner.begin(this->maze, WallAssumption::PESSIMISTIC, profile); - this->planning = true; + this->racing_line.reset(); + this->stage = PlanStage::SEARCH; + this->pass = LinePass::REQUESTED; + this->risky_line_time = std::numeric_limits::infinity(); } template -bool TMission::update_plan(uint32_t max_nodes) { - if (not this->planning) { - return true; +bool TMission::update_plan(uint32_t max_edges) { + switch (this->stage) { + case PlanStage::IDLE: + return true; + + case PlanStage::SEARCH: + if (not this->planner.step(max_edges)) { + return false; + } + + if (this->pass == LinePass::REQUESTED) { + this->route_time = this->choose_route(this->solve_profile, this->solve_route, this->solve_segments); + + if (not this->solve_profile.racing_line or this->solve_segments.empty()) { + this->stage = PlanStage::IDLE; + return true; + } + + this->racing_line.begin(this->solve_route, this->solve_segments, this->maze, this->solve_profile); + } else { + RunProfile careful = this->solve_profile; + careful.risky = false; + this->choose_route(careful, this->careful_route, this->careful_segments); + + if (this->careful_segments.empty()) { + this->pass = LinePass::AGAIN; + this->racing_line.begin(this->solve_route, this->solve_segments, this->maze, this->solve_profile); + } else { + this->racing_line.begin(this->careful_route, this->careful_segments, this->maze, careful); + } + } + + this->stage = PlanStage::LINE; + return false; + + case PlanStage::LINE: + break; + } + + if (not this->racing_line.step()) { + return false; + } + + const Line& line = this->racing_line.get_line(); + const float line_time = line.is_ready() ? line.get_finish_time() : std::numeric_limits::infinity(); + + if (this->pass == LinePass::REQUESTED and this->solve_profile.risky) { + RunProfile careful = this->solve_profile; + careful.risky = false; + + this->risky_line_time = line_time; + this->pass = LinePass::CAREFUL; + this->planner.begin(this->maze, WallAssumption::PESSIMISTIC, careful); + this->stage = PlanStage::SEARCH; + return false; } - if (not this->planner.step(max_nodes)) { + if (this->pass == LinePass::CAREFUL and this->risky_line_time < line_time) { + this->pass = LinePass::AGAIN; + this->racing_line.begin(this->solve_route, this->solve_segments, this->maze, this->solve_profile); return false; } - this->planning = false; + this->stage = PlanStage::IDLE; - const MotionLimits limits = this->dynamics.get_linear_limits(this->solve_profile); + if (line.is_ready() and line_time < this->route_time) { + this->route_time = line_time; + this->solve_segments.clear(); + this->solve_segments.push_back(make_segment(SegmentKind::LINE, line.length(), line.get_start())); + } + + return true; +} + +template +float TMission::choose_route(const RunProfile& profile, Route& route, std::vector& segments) { + const MotionLimits limits = this->dynamics.get_linear_limits(profile); + + float best = std::numeric_limits::infinity(); + segments.clear(); for (uint8_t i = 0; i < this->planner.get_number_of_routes(); i++) { this->planner.get_route(i, this->candidate_route); RouteCompiler::compile( - this->candidate_route, this->dynamics, this->solve_profile, this->config.planner.start_distance, + this->candidate_route, this->dynamics, profile, this->config.planner.start_distance, this->candidate_segments ); - const float total_time = - VelocityPlanner::plan(this->candidate_segments, this->dynamics, this->solve_profile, 0.0F, 0.0F); + const float total_time = VelocityPlanner::plan(this->candidate_segments, this->dynamics, profile, 0.0F, 0.0F); const Segment& last = this->candidate_segments.back(); const SpeedProfile braking{last.length, last.start_speed, 0.0F, limits}; @@ -109,18 +184,19 @@ bool TMission::update_plan(uint32_t max_nodes) { after_line += (this->candidate_route.finish_distance - last.length) / last.start_speed; } - if (total_time - after_line < this->route_time) { - this->route_time = total_time - after_line; - std::swap(this->solve_segments, this->candidate_segments); + if (total_time - after_line < best) { + best = total_time - after_line; + std::swap(segments, this->candidate_segments); + std::swap(route, this->candidate_route); } } - return true; + return best; } template bool TMission::has_route() const { - return not this->planning and not this->solve_segments.empty(); + return this->stage == PlanStage::IDLE and not this->solve_segments.empty(); } template @@ -146,8 +222,7 @@ void TMission::start(core::Objective objective) { this->flood(); Move move{}; - move.add( - make_segment(SegmentKind::STRAIGHT, cell_size - this->config.start_offset, this->get_start_pose()) + move.add(make_segment(SegmentKind::STRAIGHT, cell_size - this->config.start_offset, this->get_start_pose()) ); this->execute(move, 0.0F, this->search_speed); break; @@ -182,7 +257,7 @@ TMission::Status TMission::update( bool changed = this->observer.update(measurements, localizer, this->wall_model, this->maze); if (this->objective == core::Objective::RETURN) { - changed = this->explorer.update(this->maze, this->config.nodes_per_iteration) or changed; + changed = this->explorer.update(this->maze, this->config.edges_per_iteration) or changed; } if (changed) { @@ -202,10 +277,8 @@ TMission::Status TMission::update( if (wall == WallState::NO_WALL) { this->watching_front = false; - } else if ( - wall == WallState::WALL or - this->executor.get_reference().distance >= cell_size / 2.0F - braking - this->config.commit_margin - ) { + } else if (wall == WallState::WALL or + this->executor.get_reference().distance >= cell_size / 2.0F - braking - this->config.commit_margin) { this->divert_to_center(); } } @@ -360,7 +433,7 @@ void TMission::decide_at_entry() { const bool to_left = next->orientation == this->cell.turned_left().orientation; Segment turn = make_segment( - SegmentKind::TURN, to_left ? shape.angle : -shape.angle, + SegmentKind::TURN, to_left ? shape.length() : -shape.length(), entry.compose({.position = {.x = shape.pre, .y = 0.0F}, .orientation = 0.0F}) ); turn.turn = TurnId::SS90S; @@ -460,10 +533,8 @@ bool TMission::finish_at_entry() { if (this->objective == core::Objective::EXPLORE and this->maze.is_goal(this->cell.position)) { this->add_stop_at_center(move); - } else if ( - this->objective == core::Objective::RETURN and this->explorer.is_complete() and - this->cell.position == this->maze.get_start().position - ) { + } else if (this->objective == core::Objective::RETURN and this->explorer.is_complete() and + this->cell.position == this->maze.get_start().position) { const GridPose parked{.position = this->cell.position, .orientation = this->maze.get_start().orientation}; this->add_stop_at_center(move); @@ -527,7 +598,6 @@ Segment TMission::make_segment(SegmentKind kind, float length, co .length = length, .start_speed = 0.0F, .end_speed = 0.0F, - .max_speed = 0.0F, .start = start, }; } diff --git a/micras_nav/include/micras/nav/impl/racing_line.tpp b/micras_nav/include/micras/nav/impl/racing_line.tpp new file mode 100644 index 0000000..c7ef2b6 --- /dev/null +++ b/micras_nav/include/micras/nav/impl/racing_line.tpp @@ -0,0 +1,927 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_RACING_LINE_TPP +#define MICRAS_NAV_RACING_LINE_TPP + +#include +#include +#include +#include +#include +#include + +#include "micras/core/math.hpp" +#include "micras/core/vector.hpp" +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/line.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::nav { +template +TRacingLine::TRacingLine(const Dynamics& dynamics, const Config& config) : + dynamics{dynamics}, config{config} { + const float front = dynamics.get_model().chassis.front_length; + const float rear = dynamics.get_model().chassis.rear_length; + const float half = dynamics.get_model().chassis.half_width; + + const std::array corners{{ + {.x = front, .y = half}, + {.x = -rear, .y = half}, + {.x = -rear, .y = -half}, + {.x = front, .y = -half}, + }}; + + for (uint8_t side = 0; side < corners.size(); side++) { + const core::Vector& from = corners.at(side); + const core::Vector& to = corners.at((side + 1) % corners.size()); + const auto steps = static_cast(std::ceil(from.distance(to) / outline_spacing)); + + for (uint8_t step = 0; step < steps and this->outline_size < max_outline_points; step++) { + const float fraction = static_cast(step) / static_cast(steps); + + this->outline.at(this->outline_size++) = { + .x = from.x + fraction * (to.x - from.x), + .y = from.y + fraction * (to.y - from.y), + }; + } + } + + for (uint8_t i = 0; i < this->outline_size; i++) { + this->reach = std::max(this->reach, this->outline.at(i).magnitude()); + } +} + +template +void TRacingLine::reset() { + this->line.ready = false; + this->phase = Phase::IDLE; +} + +template +void TRacingLine::begin( + const Route& route, std::span segments, const TMaze& maze, const RunProfile& profile +) { + this->reset(); + this->maze = &maze; + this->segments = segments; + this->run_profile = profile; + this->least_margin = profile.risky ? this->config.risky_least_margin : this->config.least_margin; + this->openings.fill(0); + this->sweeps = 0; + + TPlanner::for_each_wall(route, [this](const GridPose& wall) { this->open(wall); }); + + for (const GridPoint& cell : maze.get_goal()) { + for (const Side side : all_sides) { + const GridPoint neighbor = cell + side; + + if (TMaze::contains(neighbor) and maze.is_goal(neighbor) and + maze.get_wall({.position = cell, .orientation = side}) == WallState::NO_WALL) { + this->open({.position = cell, .orientation = side}); + } + } + } + + float total = 0.0F; + + for (const Segment& segment : segments) { + total += std::abs(segment.length); + } + + const auto samples = static_cast(std::lround(total / this->config.spacing)) + 1; + + if (segments.empty() or samples > Line::max_samples or samples < 2 * fixed_samples + 2) { + this->finish(false); + return; + } + + this->line.size = static_cast(samples); + this->line.spacing = total / static_cast(samples - 1); + this->cursor = 0; + this->segment_index = 0; + this->segment_start = 0.0F; + this->phase = Phase::SAMPLE; +} + +template +bool TRacingLine::step() { + uint32_t budget = this->config.samples_per_step; + + while (budget > 0) { + switch (this->phase) { + case Phase::IDLE: + return true; + + case Phase::SAMPLE: + this->sample_route(budget); + break; + + case Phase::BOUNDS: + this->find_bounds(budget); + break; + + case Phase::SOLVE: + this->solve_window(); + return false; + + case Phase::MEASURE: + this->measure(budget); + break; + + case Phase::RESAMPLE: + this->resample(budget); + break; + + case Phase::CHECK: + this->check(budget); + break; + + case Phase::CURVATURE: + this->measure_curvature(budget); + break; + + case Phase::SMOOTH: + this->smooth_curvature(budget); + break; + + case Phase::SPEED: + this->plan_speeds(); + return this->phase == Phase::IDLE; + } + } + + return this->phase == Phase::IDLE; +} + +template +const Line& TRacingLine::get_line() const { + return this->line; +} + +template +uint8_t TRacingLine::get_sweeps() const { + return this->sweeps; +} + +template +void TRacingLine::sample_route(uint32_t& budget) { + for (; budget > 0 and this->cursor < this->line.size; budget--, this->cursor++) { + const float distance = static_cast(this->cursor) * this->line.spacing; + + while (this->segment_index + 1U < this->segments.size() and + distance > this->segment_start + std::abs(this->segments[this->segment_index].length)) { + this->segment_start += std::abs(this->segments[this->segment_index].length); + this->segment_index++; + } + + const Segment& segment = this->segments[this->segment_index]; + const float side = std::copysign(1.0F, segment.length); + const float along = std::clamp(distance - this->segment_start, 0.0F, std::abs(segment.length)); + + Pose local{.position = {.x = side * along, .y = 0.0F}, .orientation = 0.0F}; + + if (segment.kind == SegmentKind::TURN) { + const auto point = this->dynamics.get_turn(this->run_profile, segment.turn).template sample(along); + local = {.position = {.x = point.x, .y = side * point.y}, .orientation = side * point.heading}; + } + + const Pose pose = segment.start.compose(local); + + this->line.xs.at(this->cursor) = pose.position.x; + this->line.ys.at(this->cursor) = pose.position.y; + } + + if (this->cursor == this->line.size) { + this->start_sweep(); + } +} + +template +void TRacingLine::find_bounds(uint32_t& budget) { + for (; budget > 0 and this->cursor < this->window_size; budget--, this->cursor++) { + const auto index = static_cast(this->window_start + this->cursor); + const float heading = this->get_heading(index); + const float cosine = std::cos(heading); + const float sine = std::sin(heading); + + const core::Vector position{.x = this->line.xs.at(index), .y = this->line.ys.at(index)}; + + this->headings.at(this->cursor) = heading; + this->lower.at(this->cursor) = 0.0F; + this->upper.at(this->cursor) = 0.0F; + this->old_x.at(this->cursor) = position.x; + this->old_y.at(this->cursor) = position.y; + this->was_clear.at(this->cursor + 1U) = this->is_clear(position, cosine, sine, this->config.margin); + + if (not this->was_clear.at(this->cursor + 1U)) { + continue; + } + + const auto steps = static_cast( + std::min(std::floor(this->config.trust / this->config.scan_step), max_scan_steps) + ); + + for (const float direction : {1.0F, -1.0F}) { + float reached = 0.0F; + + for (uint8_t step = 1; step <= steps; step++) { + const float offset = direction * static_cast(step) * this->config.scan_step; + const core::Vector moved{.x = position.x - offset * sine, .y = position.y + offset * cosine}; + + if (not this->is_clear(moved, cosine, sine, this->config.margin)) { + break; + } + + reached = offset; + } + + if (direction > 0.0F) { + this->upper.at(this->cursor) = reached; + } else { + this->lower.at(this->cursor) = reached; + } + } + } + + if (this->cursor == this->window_size) { + this->phase = Phase::SOLVE; + } +} + +template +void TRacingLine::solve_window() { + const uint16_t first = this->window_start; + const uint8_t size = this->window_size; + + std::array normals_x{}; + std::array normals_y{}; + + for (uint8_t j = 0; j < size; j++) { + normals_x.at(j) = -std::sin(static_cast(this->headings.at(j))); + normals_y.at(j) = std::cos(static_cast(this->headings.at(j))); + } + + this->solve_slides(std::span{normals_x}.first(size), std::span{normals_y}.first(size)); + + const auto neighbor_clear = [this](uint16_t index) { + const float heading = this->get_heading(index); + + return this->is_clear( + {.x = this->line.xs.at(index), .y = this->line.ys.at(index)}, std::cos(heading), std::sin(heading), + this->config.margin + ); + }; + + this->was_clear.at(0) = neighbor_clear(first - 1U); + this->was_clear.at(size + 1U) = neighbor_clear(first + size); + + const auto place = [&]() { + for (uint8_t j = 0; j < size; j++) { + this->line.xs.at(first + j) = this->old_x.at(j) + static_cast(this->slides.at(j) * normals_x.at(j)); + this->line.ys.at(first + j) = this->old_y.at(j) + static_cast(this->slides.at(j) * normals_y.at(j)); + } + }; + + place(); + + for (uint8_t shrink = 0;; shrink++) { + std::array clashes{}; + bool clash = false; + + for (uint8_t j = 0; j < size + 2U; j++) { + clashes.at(j) = this->was_clear.at(j) and not neighbor_clear(first + j - 1U); + clash = clash or clashes.at(j); + } + + if (not clash) { + break; + } + + if (shrink == max_shrinks) { + this->slides.fill(0.0); + place(); + break; + } + + for (uint8_t j = 0; j < size + 2U; j++) { + if (not clashes.at(j)) { + continue; + } + + for (int32_t k = std::max(j - 4, 0); k <= std::min(j + 2, size - 1); k++) { + this->slides.at(k) *= 0.5; + } + } + + place(); + } + + for (uint8_t j = 0; j < size; j++) { + this->sweep_slide = std::max(this->sweep_slide, static_cast(std::abs(this->slides.at(j)))); + } + + this->window_start += stride; + + if (this->window_start + fixed_samples >= this->line.size) { + this->cursor = 0; + this->measured_length = 0.0F; + this->phase = Phase::MEASURE; + return; + } + + this->start_window(); +} + +template +void TRacingLine::measure(uint32_t& budget) { + for (; budget > 0 and this->cursor + 1U < this->line.size; budget--, this->cursor++) { + this->measured_length += std::hypot( + this->line.xs.at(this->cursor + 1U) - this->line.xs.at(this->cursor), + this->line.ys.at(this->cursor + 1U) - this->line.ys.at(this->cursor) + ); + } + + if (this->cursor + 1U < this->line.size) { + return; + } + + const auto samples = static_cast(std::lround(this->measured_length / this->config.spacing)) + 1; + + if (samples > Line::max_samples or samples < 2 * fixed_samples + 2) { + this->finish(false); + return; + } + + this->resampled_size = static_cast(samples); + this->cursor = 0; + this->read_index = 0; + this->read_length = 0.0F; + this->phase = Phase::RESAMPLE; +} + +template +void TRacingLine::resample(uint32_t& budget) { + const float spacing = this->measured_length / static_cast(this->resampled_size - 1); + + for (; budget > 0 and this->cursor < this->resampled_size; budget--, this->cursor++) { + if (this->cursor + 1U == this->resampled_size) { + this->line.curvatures.at(this->cursor) = this->line.xs.at(this->line.size - 1); + this->line.speeds.at(this->cursor) = this->line.ys.at(this->line.size - 1); + continue; + } + + const float distance = static_cast(this->cursor) * spacing; + + float step_length = 0.0F; + + while (true) { + step_length = std::hypot( + this->line.xs.at(this->read_index + 1U) - this->line.xs.at(this->read_index), + this->line.ys.at(this->read_index + 1U) - this->line.ys.at(this->read_index) + ); + + if (this->read_length + step_length >= distance or this->read_index + 2U >= this->line.size) { + break; + } + + this->read_length += step_length; + this->read_index++; + } + + const float fraction = + step_length > 0.0F ? std::clamp((distance - this->read_length) / step_length, 0.0F, 1.0F) : 0.0F; + const uint16_t from = this->read_index; + + this->line.curvatures.at(this->cursor) = + this->line.xs.at(from) + fraction * (this->line.xs.at(from + 1U) - this->line.xs.at(from)); + this->line.speeds.at(this->cursor) = + this->line.ys.at(from) + fraction * (this->line.ys.at(from + 1U) - this->line.ys.at(from)); + } + + if (this->cursor < this->resampled_size) { + return; + } + + std::copy_n(this->line.curvatures.begin(), this->resampled_size, this->line.xs.begin()); + std::copy_n(this->line.speeds.begin(), this->resampled_size, this->line.ys.begin()); + this->line.size = this->resampled_size; + this->line.spacing = spacing; + this->sweeps++; + + if (this->sweep_slide < this->config.convergence or this->sweeps >= this->config.max_sweeps) { + this->cursor = fixed_samples; + this->phase = Phase::CHECK; + return; + } + + this->start_sweep(); +} + +template +void TRacingLine::check(uint32_t& budget) { + const uint16_t end = this->line.size - fixed_samples; + + for (; budget > 0 and this->cursor < end; budget--, this->cursor++) { + const float heading = this->get_heading(this->cursor); + + if (not this->is_clear( + {.x = this->line.xs.at(this->cursor), .y = this->line.ys.at(this->cursor)}, std::cos(heading), + std::sin(heading), this->least_margin + )) { + this->finish(false); + return; + } + } + + if (this->cursor == end) { + this->cursor = 0; + this->phase = Phase::CURVATURE; + } +} + +template +void TRacingLine::measure_curvature(uint32_t& budget) { + const uint16_t last = this->line.size - 1; + + for (; budget > 0 and this->cursor <= last; budget--, this->cursor++) { + if (this->cursor == last) { + this->line.times.at(this->cursor) = 0.0F; + continue; + } + + const float heading = std::atan2( + this->line.ys.at(this->cursor + 1U) - this->line.ys.at(this->cursor), + this->line.xs.at(this->cursor + 1U) - this->line.xs.at(this->cursor) + ); + + this->line.times.at(this->cursor) = + this->cursor == 0 ? 0.0F : core::math::wrap_angle(heading - this->previous_heading) / this->line.spacing; + this->previous_heading = heading; + } + + if (this->cursor > last) { + this->cursor = 0; + this->phase = Phase::SMOOTH; + } +} + +template +void TRacingLine::smooth_curvature(uint32_t& budget) { + const int32_t last = this->line.size - 1; + const int32_t reach = this->config.smoothing; + + for (; budget > 0 and this->cursor < this->line.size; budget--, this->cursor++) { + const int32_t from = std::max(this->cursor - reach, 0); + const int32_t to = std::min(this->cursor + reach, last); + + float sum = 0.0F; + + for (int32_t i = from; i <= to; i++) { + sum += this->line.times.at(i); + } + + this->line.curvatures.at(this->cursor) = sum / static_cast(to - from + 1); + } + + if (this->cursor == this->line.size) { + this->phase = Phase::SPEED; + } +} + +template +void TRacingLine::plan_speeds() { + const uint16_t size = this->line.size; + const float spacing = this->line.spacing; + + const auto bending = [this, size, spacing](std::size_t index) { + const std::size_t from = std::min(index, size - 2U); + + return Bending{ + .curvature = this->line.curvatures.at(index), + .sharpness = (this->line.curvatures.at(from + 1U) - this->line.curvatures.at(from)) / spacing, + }; + }; + + CurveLimits limits = this->dynamics.get_curve_limits(this->run_profile); + limits.lateral *= this->config.lateral_share; + + CurveSpeed::plan(bending, std::span{this->line.speeds}.first(size), spacing, 0.0F, 0.0F, limits); + CurveSpeed::integrate(std::span{this->line.speeds}.first(size), spacing, std::span{this->line.times}.first(size)); + + this->line.finish_time = this->line.times.at(size - 1); + + const float cell_size = this->dynamics.get_model().maze.cell_size; + + for (uint16_t i = 0; i < size; i++) { + const GridPoint cell = GridPoint::from_vector({.x = this->line.xs.at(i), .y = this->line.ys.at(i)}, cell_size); + + if (this->maze->is_goal(cell)) { + this->line.finish_time = this->line.times.at(i); + break; + } + } + + this->finish(true); +} + +template +void TRacingLine::start_sweep() { + this->sweep_slide = 0.0F; + this->window_start = fixed_samples; + this->start_window(); +} + +template +void TRacingLine::start_window() { + this->window_size = + static_cast(std::min(window, this->line.size - fixed_samples - this->window_start)); + this->cursor = 0; + this->phase = Phase::BOUNDS; +} + +template +void TRacingLine::finish(bool found) { + this->line.ready = found; + this->maze = nullptr; + this->segments = {}; + this->phase = Phase::IDLE; +} + +template +float TRacingLine::get_heading(uint16_t index) const { + return this->line.get_heading(index); +} + +template +void TRacingLine::solve_slides(std::span normals_x, std::span normals_y) { + const uint16_t first = this->window_start; + const auto size = static_cast(normals_x.size()); + const double length_weight = static_cast(this->config.length_weight) * + static_cast(this->line.spacing) * static_cast(this->line.spacing); + + this->diagonal.fill(0.0); + this->first_band.fill(0.0); + this->second_band.fill(0.0); + this->linear.fill(0.0); + + const auto add_row = [&](int32_t row, std::span weights, double scale) { + double base_x = 0.0; + double base_y = 0.0; + + for (std::size_t k = 0; k < weights.size(); k++) { + const auto index = static_cast(row + static_cast(k)); + base_x += weights[k] * static_cast(this->line.xs.at(index)); + base_y += weights[k] * static_cast(this->line.ys.at(index)); + } + + for (std::size_t k = 0; k < weights.size(); k++) { + const int32_t j = row + static_cast(k) - first; + + if (j < 0 or j >= size) { + continue; + } + + this->linear.at(j) += scale * weights[k] * (base_x * normals_x[j] + base_y * normals_y[j]); + + for (std::size_t l = k; l < weights.size(); l++) { + const int32_t m = row + static_cast(l) - first; + + if (m >= size) { + continue; + } + + const double value = + scale * weights[k] * weights[l] * (normals_x[j] * normals_x[m] + normals_y[j] * normals_y[m]); + + if (m == j) { + this->diagonal.at(j) += value; + } else if (m == j + 1) { + this->first_band.at(j) += value; + } else { + this->second_band.at(j) += value; + } + } + } + }; + + constexpr std::array second_difference{1.0, -2.0, 1.0}; + constexpr std::array first_difference{-1.0, 1.0}; + + for (int32_t row = first - 2; row <= first + size - 1; row++) { + add_row(row, second_difference, 1.0); + } + + for (int32_t row = first - 1; row <= first + size - 1; row++) { + add_row(row, first_difference, length_weight); + } + + const auto product = [this, size](int32_t j, std::span values) { + double result = this->diagonal.at(j) * values[j]; + + if (j + 1 < size) { + result += this->first_band.at(j) * values[j + 1]; + } + + if (j + 2 < size) { + result += this->second_band.at(j) * values[j + 2]; + } + + if (j >= 1) { + result += this->first_band.at(j - 1) * values[j - 1]; + } + + if (j >= 2) { + result += this->second_band.at(j - 2) * values[j - 2]; + } + + return result; + }; + + const auto objective = [&](std::span values) { + double result = 0.0; + + for (int32_t j = 0; j < size; j++) { + result += values[j] * (product(j, values) + 2.0 * this->linear.at(j)); + } + + return result; + }; + + const auto coupling = [this](int32_t from, int32_t to) { + if (to - from == 1) { + return this->first_band.at(from); + } + + return to - from == 2 ? this->second_band.at(from) : 0.0; + }; + + this->slides.fill(0.0); + + const std::span current{this->slides.data(), static_cast(size)}; + const std::span candidate{this->trial.data(), static_cast(size)}; + + double value = 0.0; + + for (uint8_t iteration = 0; iteration < max_newton_iterations; iteration++) { + uint8_t free = 0; + + for (int32_t j = 0; j < size; j++) { + const double slope = product(j, current) + this->linear.at(j); + const double low = this->lower.at(j); + const double high = this->upper.at(j); + + this->gradient.at(j) = slope; + this->direction.at(j) = 0.0; + + const bool at_lower = this->slides.at(j) <= low + 1.0e-9 and slope > 0.0; + const bool at_upper = this->slides.at(j) >= high - 1.0e-9 and slope < 0.0; + + if (not at_lower and not at_upper) { + this->free_indices.at(free++) = static_cast(j); + } + } + + if (free == 0) { + break; + } + + bool singular = false; + + for (uint8_t c = 0; c < free; c++) { + const uint8_t j = this->free_indices.at(c); + const double first_coupling = c >= 1 ? coupling(this->free_indices.at(c - 1), j) : 0.0; + const double second_coupling = c >= 2 ? coupling(this->free_indices.at(c - 2), j) : 0.0; + + const double second = c >= 2 ? second_coupling / this->factor_diagonal.at(c - 2) : 0.0; + const double first_factor = + c >= 1 ? (first_coupling - + (c >= 2 ? second * this->factor_first.at(c - 1) * this->factor_diagonal.at(c - 2) : 0.0)) / + this->factor_diagonal.at(c - 1) : + 0.0; + + this->factor_second.at(c) = second; + this->factor_first.at(c) = first_factor; + this->factor_diagonal.at(c) = + this->diagonal.at(j) - first_factor * first_factor * (c >= 1 ? this->factor_diagonal.at(c - 1) : 0.0) - + second * second * (c >= 2 ? this->factor_diagonal.at(c - 2) : 0.0); + + if (this->factor_diagonal.at(c) <= 0.0) { + singular = true; + break; + } + } + + if (singular) { + break; + } + + for (uint8_t c = 0; c < free; c++) { + double result = -this->gradient.at(this->free_indices.at(c)); + + if (c >= 1) { + result -= this->factor_first.at(c) * this->trial.at(c - 1); + } + + if (c >= 2) { + result -= this->factor_second.at(c) * this->trial.at(c - 2); + } + + this->trial.at(c) = result; + } + + for (int32_t c = free - 1; c >= 0; c--) { + double result = this->trial.at(c) / this->factor_diagonal.at(c); + + if (c + 1 < free) { + result -= this->factor_first.at(c + 1) * this->direction.at(this->free_indices.at(c + 1)); + } + + if (c + 2 < free) { + result -= this->factor_second.at(c + 2) * this->direction.at(this->free_indices.at(c + 2)); + } + + this->direction.at(this->free_indices.at(c)) = result; + } + + double step = 1.0; + double trial_value = value; + double moved = 0.0; + + for (uint8_t search = 0; search < 20; search++) { + double decrease = 0.0; + moved = 0.0; + + for (int32_t j = 0; j < size; j++) { + this->trial.at(j) = std::clamp( + this->slides.at(j) + step * this->direction.at(j), static_cast(this->lower.at(j)), + static_cast(this->upper.at(j)) + ); + decrease += 2.0 * this->gradient.at(j) * (this->trial.at(j) - this->slides.at(j)); + moved = std::max(moved, std::abs(this->trial.at(j) - this->slides.at(j))); + } + + trial_value = objective(candidate); + + if (trial_value <= value + 1.0e-4 * decrease) { + break; + } + + step /= 2.0; + } + + if (trial_value > value) { + break; + } + + std::copy_n(this->trial.begin(), size, this->slides.begin()); + value = trial_value; + + if (moved < 1.0e-7) { + break; + } + } +} + +template +bool TRacingLine::is_clear(const core::Vector& position, float cosine, float sine, float margin) const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const float half_cell = cell_size / 2.0F; + const float half_wall = this->dynamics.get_model().maze.wall_thickness / 2.0F; + + const auto post_x = static_cast(2 * std::lround(position.x / cell_size)); + const auto post_y = static_cast(2 * std::lround(position.y / cell_size)); + + const core::Vector post{.x = static_cast(post_x) * half_cell, .y = static_cast(post_y) * half_cell}; + + if (this->hits(position, cosine, sine, post, {.x = half_wall, .y = half_wall}, margin)) { + return false; + } + + constexpr std::array, 4> around{{{0, 1}, {0, -1}, {1, 0}, {-1, 0}}}; + + for (const auto& [dx, dy] : around) { + if (this->is_open(post_x + dx, post_y + dy)) { + continue; + } + + const core::Vector center{ + .x = static_cast(post_x + dx) * half_cell, .y = static_cast(post_y + dy) * half_cell + }; + const core::Vector half_size = + dx == 0 ? core::Vector{.x = half_wall, .y = half_cell} : core::Vector{.x = half_cell, .y = half_wall}; + + if (this->hits(position, cosine, sine, center, half_size, margin)) { + return false; + } + } + + return true; +} + +template +bool TRacingLine::hits( + const core::Vector& position, float cosine, float sine, const core::Vector& center, const core::Vector& half_size, + float margin +) const { + const auto outside = [¢er, &half_size, margin](float x, float y) { + const float dx = std::max(std::abs(x - center.x) - half_size.x, 0.0F); + const float dy = std::max(std::abs(y - center.y) - half_size.y, 0.0F); + return dx * dx + dy * dy >= margin * margin; + }; + + const float bound = this->reach + margin; + const float gap_x = std::max(std::abs(position.x - center.x) - half_size.x, 0.0F); + const float gap_y = std::max(std::abs(position.y - center.y) - half_size.y, 0.0F); + + if (gap_x * gap_x + gap_y * gap_y >= bound * bound) { + return false; + } + + for (uint8_t i = 0; i < this->outline_size; i++) { + const core::Vector& point = this->outline.at(i); + + if (not outside( + position.x + cosine * point.x - sine * point.y, position.y + sine * point.x + cosine * point.y + )) { + return true; + } + } + + const float front = this->dynamics.get_model().chassis.front_length; + const float rear = this->dynamics.get_model().chassis.rear_length; + const float half = this->dynamics.get_model().chassis.half_width; + + for (const float sx : {-1.0F, 1.0F}) { + for (const float sy : {-1.0F, 1.0F}) { + const float offset_x = center.x + sx * half_size.x - position.x; + const float offset_y = center.y + sy * half_size.y - position.y; + const float along = cosine * offset_x + sine * offset_y; + const float across = -sine * offset_x + cosine * offset_y; + + float dx = 0.0F; + + if (along > front) { + dx = along - front; + } else if (along < -rear) { + dx = -rear - along; + } + + const float dy = std::max(std::abs(across) - half, 0.0F); + + if (dx * dx + dy * dy < margin * margin) { + return true; + } + } + } + + return false; +} + +template +bool TRacingLine::is_open(int32_t x, int32_t y) const { + if (x < 0 or y < 0 or x >= lattice_width or y >= lattice_height) { + return false; + } + + const auto bit = static_cast(y * lattice_width + x); + return (this->openings.at(bit / 8U) & (1U << (bit % 8U))) != 0; +} + +template +void TRacingLine::open(const GridPose& wall) { + int32_t x = 2 * wall.position.x + 1; + int32_t y = 2 * wall.position.y + 1; + + switch (wall.orientation) { + case Side::RIGHT: + x++; + break; + case Side::UP: + y++; + break; + case Side::LEFT: + x--; + break; + case Side::DOWN: + y--; + break; + } + + if (x < 0 or y < 0 or x >= lattice_width or y >= lattice_height) { + return; + } + + const auto bit = static_cast(y * lattice_width + x); + this->openings.at(bit / 8U) = static_cast(this->openings.at(bit / 8U) | (1U << (bit % 8U))); +} +} // namespace micras::nav + +#endif // MICRAS_NAV_RACING_LINE_TPP diff --git a/micras_nav/include/micras/nav/line.hpp b/micras_nav/include/micras/nav/line.hpp new file mode 100644 index 0000000..39a6be8 --- /dev/null +++ b/micras_nav/include/micras/nav/line.hpp @@ -0,0 +1,156 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_LINE_HPP +#define MICRAS_NAV_LINE_HPP + +#include +#include + +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +template +class TRacingLine; + +/** + * @brief A path given by samples at equal steps of distance, with the speed of the robot at each. + * + * @details This is the racing line as the executor plays it back. Between two samples the position + * follows the cubic that leaves each sample along its heading, the heading and the curvature change + * linearly and the acceleration is constant, so the reference is smooth and its derivatives are the + * ones the path and the speeds imply. The heading of a sample is the direction from the one before + * it to the one after it. + * + * @note The samples live in static arrays, which is most of the memory the racing line takes. + */ +class Line { +public: + /** + * @brief Largest number of samples, which at 10 mm is a path of 25.6 m. + */ + static constexpr uint16_t max_samples{2560}; + + /** + * @brief Point of the path at a distance from its start. + */ + struct Point { + Pose pose; + float curvature; + float sharpness; + }; + + /** + * @brief Check if the line was planned and timed. + * + * @return True if it can be driven. + */ + bool is_ready() const; + + /** + * @brief Get the length of the path. + * + * @return The length in meters. + */ + float length() const; + + /** + * @brief Get the time the motion along the path takes, from rest to rest. + * + * @return The duration in seconds. + */ + float duration() const; + + /** + * @brief Get the time at which the robot crosses into the goal. + * + * @return The time in seconds, which is how a fast run is timed. + */ + float get_finish_time() const; + + /** + * @brief Get the pose at the start of the path. + * + * @return The pose in the maze frame. + */ + Pose get_start() const; + + /** + * @brief Evaluate the motion along the path at an instant. + * + * @param time Time since the start of the motion, clamped to its duration. + * @return The distance covered, the speed and the acceleration at that instant. + */ + SpeedProfile::Sample sample_motion(float time) const; + + /** + * @brief Evaluate the path at a distance from its start. + * + * @param distance The distance along the path, clamped to its length. + * @return The pose in the maze frame, the curvature and the sharpness there. + */ + Point sample_point(float distance) const; + +private: + template + friend class TRacingLine; + + /** + * @brief Get the heading of the path at a sample. + * + * @param index The index of the sample. + * @return The heading in radians. + */ + float get_heading(uint16_t index) const; + + /** + * @brief Position of every sample in the maze frame. + * + * @note While the line is optimized, the curvatures and the speeds hold the positions being + * resampled. + */ + ///@{ + std::array xs{}; + std::array ys{}; + ///@} + + /** + * @brief Curvature at every sample. + */ + std::array curvatures{}; + + /** + * @brief Speed at every sample. + */ + std::array speeds{}; + + /** + * @brief Time at which every sample is reached. + */ + std::array times{}; + + /** + * @brief Number of samples. + */ + uint16_t size{}; + + /** + * @brief Distance between two samples. + */ + float spacing{}; + + /** + * @brief Time at which the robot crosses into the goal. + */ + float finish_time{}; + + /** + * @brief Whether the line was planned and timed. + */ + bool ready{}; +}; +} // namespace micras::nav + +#endif // MICRAS_NAV_LINE_HPP diff --git a/micras_nav/include/micras/nav/mission.hpp b/micras_nav/include/micras/nav/mission.hpp index a0ed953..407b4df 100644 --- a/micras_nav/include/micras/nav/mission.hpp +++ b/micras_nav/include/micras/nav/mission.hpp @@ -18,6 +18,7 @@ #include "micras/nav/measurements.hpp" #include "micras/nav/motion_limits.hpp" #include "micras/nav/planner.hpp" +#include "micras/nav/racing_line.hpp" #include "micras/nav/segment.hpp" #include "micras/nav/wall_model.hpp" #include "micras/nav/wall_observer.hpp" @@ -34,7 +35,7 @@ namespace micras::nav { * - Returning, the explorer names the cells still worth visiting and the robot descends the flood * towards them, until the map proves that the fastest route is known. Then it goes back to the * start cell, squares up against its back wall and parks where a run starts from. - * - Solving, the route planned beforehand is executed as it is. + * - Solving, the route planned beforehand is executed as it is, or the racing line through its cells. * * The move through a cell is decided when the robot crosses into it, as late as it can be, from the * walls that were decided by then. A turn is only made through a wall that was seen absent. A @@ -83,6 +84,7 @@ class TMission { TMaze::Config maze; TPlanner::Config planner; TWallObserver::Config observer; + TRacingLine::Config racing_line; Executor::Config executor; RunProfile search_profile; std::span map_profiles; @@ -92,7 +94,7 @@ class TMission { float look_time; uint16_t max_looks; float commit_margin; - uint32_t nodes_per_iteration; + uint32_t edges_per_iteration; }; /** @@ -136,12 +138,16 @@ class TMission { * @brief Advance the planning of the route of a fast run. * * @note Only to be called with the robot stopped: when the search ends, the best candidates - * are compiled and timed, which is not bounded to fit in an iteration. + * are compiled and timed, which is not bounded to fit in an iteration. When the profile asks + * for the racing line, it is optimized next, through the cells of the route chosen, and it + * replaces the route if it is found and faster. With the risky switch too, a second line goes + * through the route planned without the risky turns, and the risky line is only driven if it + * is the faster of the two; it is then optimized again, since only one line is kept. * - * @param max_nodes The largest number of nodes the planner may expand in this call. + * @param max_edges The largest number of edges the planner may try in this call. * @return True if the planning has finished. */ - bool update_plan(uint32_t max_nodes); + bool update_plan(uint32_t max_edges); /** * @brief Check if a route for a fast run exists. @@ -202,6 +208,25 @@ class TMission { const GridPose& get_cell() const; private: + /** + * @brief What the planning of a fast run is doing. + */ + enum class PlanStage : uint8_t { + IDLE = 0, + SEARCH = 1, + LINE = 2, + }; + + /** + * @brief Which racing line is being optimized: through the route of the profile asked for, + * through the route planned without the risky turns, or the first one again. + */ + enum class LinePass : uint8_t { + REQUESTED = 0, + CAREFUL = 1, + AGAIN = 2, + }; + /** * @brief Largest number of segments a move through one cell takes. */ @@ -236,6 +261,16 @@ class TMission { uint8_t size{}; }; + /** + * @brief Time the candidate routes the planner found and keep the fastest. + * + * @param profile The profile the routes are compiled and timed for. + * @param route Filled with the fastest route. + * @param segments Filled with its segments, and left empty if there is no route. + * @return The time of the fastest route up to the goal line, or infinity. + */ + float choose_route(const RunProfile& profile, Route& route, std::vector& segments); + /** * @brief Get the nominal pose of the robot as it crosses into a cell. * @@ -345,6 +380,11 @@ class TMission { */ TExplorer explorer; + /** + * @brief Smoothest line through the cells of the route of a fast run. + */ + TRacingLine racing_line; + /** * @brief Playback of the moves as a reference. */ @@ -415,15 +455,36 @@ class TMission { */ Route candidate_route; + /** + * @brief Route of the fast run that was planned, which the racing line goes through. + */ + Route solve_route; + + /** + * @brief Route planned without the risky turns, and its segments, for the second racing line. + */ + ///@{ + Route careful_route; + std::vector careful_segments; + ///@} + + /** + * @brief Time of the racing line through the risky route, to compare the second one with. + */ + float risky_line_time{}; + /** * @brief Time the planned fast run is expected to take. */ float route_time{}; /** - * @brief Whether the route of a fast run is being planned. + * @brief What the planning of a fast run is doing, and which racing line it is at. */ - bool planning{}; + ///@{ + PlanStage stage{PlanStage::IDLE}; + LinePass pass{LinePass::REQUESTED}; + ///@} /** * @brief Last reference produced. diff --git a/micras_nav/include/micras/nav/racing_line.hpp b/micras_nav/include/micras/nav/racing_line.hpp new file mode 100644 index 0000000..b00afb0 --- /dev/null +++ b/micras_nav/include/micras/nav/racing_line.hpp @@ -0,0 +1,492 @@ +/** + * @file + */ + +#ifndef MICRAS_NAV_RACING_LINE_HPP +#define MICRAS_NAV_RACING_LINE_HPP + +#include +#include +#include +#include + +#include "micras/core/vector.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/line.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/segment.hpp" + +namespace micras::nav { +/** + * @brief The smoothest line through the cells of a planned route, and the speeds to drive it at. + * + * @details The route is sampled every few millimeters, and each sample may then slide sideways, + * along the normal of the line, as far as the outline of the robot, turned along the line, keeps + * the margin to every post and to every wall the route does not cross. The walls the route does not + * cross are taken as present, whatever the map says, so the line never leaves the cells of the + * route. Among the slides allowed, the ones chosen make the sum of the squared second differences + * of the samples smallest, which is the sum of the squared curvatures, plus a small weight on the + * squared first differences, which prefers the shorter line. + * + * That is a least squares problem with bounds on each unknown, where each unknown only meets its two + * neighbors on each side. It is solved in windows of a few tens of samples, the samples around a + * window held where they are, by a projected Newton method on the banded normal equations. The + * windows overlap and sweep the line from its start to its end, and the line is resampled at equal + * steps after each sweep. A slide is limited per sweep, and the bounds are found again for every + * window, since the robot needs more room as the line turns it. After the last sweep every sample + * is checked, the curvature is measured and lightly smoothed, and the speeds are planned along it + * with the same rule as a turn. + * + * Nothing here is bounded to one iteration of the loop as a whole: every phase advances by a budget + * of samples per call, except for the speeds, which take a few milliseconds in one call, the robot + * being stopped. + * + * @note The first and last samples never move: the line starts where the robot stands and ends + * where the route comes to rest. + * + * @tparam width The width of the maze in cells. + * @tparam height The height of the maze in cells. + */ +template +class TRacingLine { +public: + /** + * @brief Configuration struct for the racing line. + * + * @note The margin is the distance the line keeps between the outline of the robot and any + * obstacle wherever the route leaves room for it, with the risky switch too: at the margin of + * the risky turns the line hit a wall on 4 to 6 of 10 mazes. The least margins are what the + * final check requires everywhere, without and with the risky switch: the margins the turns were + * designed with, less what the resampling and the headings of the final line use up. Where the + * route itself comes closer than the margin, which the risky turns do, the line does not move. The trust is + * how far a sample may slide in one sweep, the scan step the resolution of the bounds, and the + * length weight the weight of the first differences, in units of the spacing. A sweep that + * slides no sample farther than the convergence tolerance ends the optimization. The smoothing + * is the number of samples on each side the curvature is averaged over. The lateral share is the + * part of the lateral grip of the run the curves of the line are planned with: the tires slide + * sideways in proportion to what they are asked for, which the pose estimate does not see, and + * the line holds its curves far longer than a turn does. + */ + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) no defaults, so that a missing field is a warning + struct Config { + float spacing; + float margin; + float least_margin; + float risky_least_margin; + float trust; + float scan_step; + float length_weight; + float convergence; + float lateral_share; + uint8_t max_sweeps; + uint8_t smoothing; + uint16_t samples_per_step; + }; + + /** + * @brief Construct a new TRacingLine object. + * + * @param dynamics The physical limits of the robot, borrowed for the lifetime of the object. + * @param config The configuration for the racing line. + */ + TRacingLine(const Dynamics& dynamics, const Config& config); + + /** + * @brief Forget the line. + */ + void reset(); + + /** + * @brief Start optimizing the line of a route. + * + * @param route The route on the lattice, which tells the walls it crosses. + * @param segments The segments the route compiles to, borrowed until the optimization ends. + * @param maze The map of the walls, borrowed until the optimization ends. + * @param profile The profile of the run. + */ + void begin( + const Route& route, std::span segments, const TMaze& maze, + const RunProfile& profile + ); + + /** + * @brief Advance the optimization. + * + * @return True if it has finished, whether a line was found or not. + */ + bool step(); + + /** + * @brief Get the line. + * + * @return The line, which is ready only if the last optimization found one. + */ + const Line& get_line() const; + + /** + * @brief Get the number of sweeps the last optimization made. + * + * @return The number of sweeps. + */ + uint8_t get_sweeps() const; + +private: + /** + * @brief Largest number of samples solved for at once. + */ + static constexpr uint8_t window{64}; + + /** + * @brief Number of samples a window starts after the start of the one before. + */ + static constexpr uint8_t stride{48}; + + /** + * @brief Number of samples at each end of the line that never move. + */ + static constexpr uint8_t fixed_samples{3}; + + /** + * @brief Largest number of points along the outline of the robot that are checked. + */ + static constexpr uint8_t max_outline_points{32}; + + /** + * @brief Largest distance between two points of the outline that are checked, in meters. + * + * @note Below the thickness of a wall plus twice the smallest margin, so that no wall can pass + * between two points unseen. + */ + static constexpr float outline_spacing{0.02F}; + + /** + * @brief Largest number of iterations of the projected Newton method in a window. + */ + static constexpr uint8_t max_newton_iterations{30}; + + /** + * @brief Largest number of times a window that brings the robot too close is shrunk. + */ + static constexpr uint8_t max_shrinks{8}; + + /** + * @brief Largest number of steps of distance scanned on each side of a sample. + */ + static constexpr uint8_t max_scan_steps{32}; + + /** + * @brief Number of lattice points along each axis, a lattice unit being half a cell. + */ + ///@{ + static constexpr uint16_t lattice_width{2 * width + 1}; + static constexpr uint16_t lattice_height{2 * height + 1}; + ///@} + + /** + * @brief Phase of the optimization. + */ + enum class Phase : uint8_t { + IDLE = 0, + SAMPLE = 1, + BOUNDS = 2, + SOLVE = 3, + MEASURE = 4, + RESAMPLE = 5, + CHECK = 6, + CURVATURE = 7, + SMOOTH = 8, + SPEED = 9, + }; + + /** + * @brief Sample the compiled route at the next samples of the budget. + * + * @param budget The number of samples left in this call, which is decreased. + */ + void sample_route(uint32_t& budget); + + /** + * @brief Find the bounds of the next samples of the window. + * + * @param budget The number of samples left in this call, which is decreased. + */ + void find_bounds(uint32_t& budget); + + /** + * @brief Solve the window, move its samples and go on to the next window. + */ + void solve_window(); + + /** + * @brief Measure the length of the line for the resampling. + * + * @param budget The number of samples left in this call, which is decreased. + */ + void measure(uint32_t& budget); + + /** + * @brief Resample the line at equal steps, then start the next sweep or the checks. + * + * @param budget The number of samples left in this call, which is decreased. + */ + void resample(uint32_t& budget); + + /** + * @brief Check the clearance of the next samples of the line. + * + * @param budget The number of samples left in this call, which is decreased. + */ + void check(uint32_t& budget); + + /** + * @brief Measure the curvature at the next samples of the line. + * + * @param budget The number of samples left in this call, which is decreased. + */ + void measure_curvature(uint32_t& budget); + + /** + * @brief Smooth the curvature at the next samples of the line. + * + * @param budget The number of samples left in this call, which is decreased. + */ + void smooth_curvature(uint32_t& budget); + + /** + * @brief Plan the speeds along the line, time it and mark it ready. + */ + void plan_speeds(); + + /** + * @brief Start a sweep from the first window. + */ + void start_sweep(); + + /** + * @brief Start the window at the current window start. + */ + void start_window(); + + /** + * @brief End the optimization. + * + * @param found Whether a line was found. + */ + void finish(bool found); + + /** + * @brief Get the heading of the line at a sample, from the samples around it. + * + * @param index The index of the sample. + * @return The heading in radians. + */ + float get_heading(uint16_t index) const; + + /** + * @brief Solve for the slides of the window within their bounds. + * + * @param normals_x The x component of the normal of each sample of the window. + * @param normals_y The y component of the normal of each sample of the window. + */ + void solve_slides(std::span normals_x, std::span normals_y); + + /** + * @brief Check if the robot keeps a margin to every obstacle at a pose. + * + * @param position The position of the robot. + * @param cosine The cosine of its heading. + * @param sine The sine of its heading. + * @param margin The distance to keep. + * @return True if nothing is closer than the margin. + */ + bool is_clear(const core::Vector& position, float cosine, float sine, float margin) const; + + /** + * @brief Check if the robot comes within a margin of a rectangle aligned with the grid. + * + * @param position The position of the robot. + * @param cosine The cosine of its heading. + * @param sine The sine of its heading. + * @param center The center of the rectangle. + * @param half_size The half width and half height of the rectangle. + * @param margin The distance to keep. + * @return True if the robot is closer than the margin. + */ + bool hits( + const core::Vector& position, float cosine, float sine, const core::Vector& center, + const core::Vector& half_size, float margin + ) const; + + /** + * @brief Check if a wall may be crossed by the line. + * + * @param x The x coordinate of the midpoint of the wall on the lattice. + * @param y The y coordinate of the midpoint of the wall on the lattice. + * @return True if the route crosses the wall, or it is inside the goal and known to be absent. + */ + bool is_open(int32_t x, int32_t y) const; + + /** + * @brief Mark a wall as one the line may cross. + * + * @param wall The wall, as a side of a cell. + */ + void open(const GridPose& wall); + + /** + * @brief Physical limits of the robot. + */ + // NOLINTNEXTLINE(*-avoid-const-or-ref-data-members) borrowed for the lifetime of the robot + const Dynamics& dynamics; + + /** + * @brief Parameters of the optimization. + */ + Config config; + + /** + * @brief Points of the outline of the robot, in its own frame, and how many there are. + */ + ///@{ + std::array outline{}; + uint8_t outline_size{}; + ///@} + + /** + * @brief Distance from the axle to the farthest point of the outline. + */ + float reach{}; + + /** + * @brief The line being optimized, and then driven. + */ + Line line; + + /** + * @brief Walls the line may cross, one bit per point of the lattice. + */ + std::array openings{}; + + /** + * @brief Map of the walls, while the line is optimized. + */ + const TMaze* maze{nullptr}; + + /** + * @brief Segments of the route, while the line is optimized. + */ + std::span segments; + + /** + * @brief Profile of the run. + */ + RunProfile run_profile{}; + + /** + * @brief Margin the line must keep everywhere, for the run. + */ + float least_margin{}; + + /** + * @brief Phase of the optimization. + */ + Phase phase{Phase::IDLE}; + + /** + * @brief Next sample of the phase in progress. + */ + uint16_t cursor{}; + + /** + * @brief Segment being sampled, and the distance along the route at its start. + */ + ///@{ + uint16_t segment_index{}; + float segment_start{}; + ///@} + + /** + * @brief Length of the line being resampled, and the distance along it at the sample read. + */ + ///@{ + float measured_length{}; + float read_length{}; + ///@} + + /** + * @brief Sample of the line being resampled that is read, and the number of samples written. + */ + ///@{ + uint16_t read_index{}; + uint16_t resampled_size{}; + ///@} + + /** + * @brief Number of sweeps done, and the farthest slide of the sweep in progress. + */ + ///@{ + uint8_t sweeps{}; + float sweep_slide{}; + ///@} + + /** + * @brief Heading of the line at the sample before, for the curvature. + */ + float previous_heading{}; + + /** + * @brief First sample of the window and its number of samples. + */ + ///@{ + uint16_t window_start{}; + uint8_t window_size{}; + ///@} + + /** + * @brief Heading, bounds and position before the move of each sample of the window. + */ + ///@{ + std::array headings{}; + std::array lower{}; + std::array upper{}; + std::array old_x{}; + std::array old_y{}; + ///@} + + /** + * @brief Whether each sample of the window and its two neighbors kept the margin before the move. + */ + std::array was_clear{}; + + /** + * @brief Banded normal equations of the window: the diagonal, the two bands above it and the + * linear term. + */ + ///@{ + std::array diagonal{}; + std::array first_band{}; + std::array second_band{}; + std::array linear{}; + ///@} + + /** + * @brief Slides of the window, and the work space of the projected Newton method. + */ + ///@{ + std::array slides{}; + std::array gradient{}; + std::array direction{}; + std::array trial{}; + std::array factor_diagonal{}; + std::array factor_first{}; + std::array factor_second{}; + std::array free_indices{}; + ///@} +}; +} // namespace micras::nav + +#include "micras/nav/impl/racing_line.tpp" // IWYU pragma: export + +#endif // MICRAS_NAV_RACING_LINE_HPP diff --git a/micras_nav/src/executor.cpp b/micras_nav/src/executor.cpp index 9a812fc..6bbe473 100644 --- a/micras_nav/src/executor.cpp +++ b/micras_nav/src/executor.cpp @@ -6,7 +6,9 @@ #include #include +#include "micras/nav/curve_speed.hpp" #include "micras/nav/executor.hpp" +#include "micras/nav/line.hpp" #include "micras/nav/motion_limits.hpp" #include "micras/nav/segment.hpp" #include "micras/nav/speed_profile.hpp" @@ -15,7 +17,8 @@ #include "micras/nav/velocity_planner.hpp" namespace micras::nav { -Executor::Executor(const Dynamics& dynamics, const Config& config) : dynamics{dynamics}, config{config} { +Executor::Executor(const Dynamics& dynamics, const Line& line, const Config& config) : + dynamics{dynamics}, line{line}, config{config} { this->segments.reserve(config.capacity); this->durations.reserve(config.capacity); } @@ -44,7 +47,11 @@ void Executor::push(std::span segments) { for (const Segment& segment : segments) { this->segments.push_back(segment); - this->durations.push_back(VelocityPlanner::get_duration(segment, this->dynamics, this->run_profile)); + this->durations.push_back( + segment.kind == SegmentKind::LINE ? + this->line.duration() : + VelocityPlanner::get_duration(segment, this->dynamics, this->run_profile) + ); this->queued_time += this->durations.back(); } @@ -147,6 +154,14 @@ void Executor::start_next() { this->duration = this->speed_profile.duration(); break; + case SegmentKind::TURN: + this->curve_speed = CurveSpeed{ + this->dynamics.get_turn(this->run_profile, segment.turn), segment.start_speed, segment.end_speed, + this->dynamics.get_curve_limits(this->run_profile) + }; + this->duration = this->curve_speed.duration(); + break; + default: this->duration = this->durations.at(this->index); break; @@ -172,18 +187,39 @@ Reference Executor::evaluate() const { } case SegmentKind::TURN: { - const TurnShape& shape = this->dynamics.get_turn(this->run_profile, segment.turn); - const float speed = segment.start_speed; - const float distance = std::min(speed * this->clock, shape.length()); - const auto sample = shape.sample(distance); + const TurnShape& shape = this->dynamics.get_turn(this->run_profile, segment.turn); + const SpeedProfile::Sample motion = this->curve_speed.sample(this->clock); + const auto point = shape.sample(motion.distance); return { .pose = segment.start.compose( - {.position = {.x = sample.x, .y = side * sample.y}, .orientation = side * sample.heading} + {.position = {.x = point.x, .y = side * point.y}, .orientation = side * point.heading} ), - .twist = {.linear = speed, .angular = side * sample.curvature * speed}, - .acceleration = {.linear = 0.0F, .angular = side * sample.sharpness * speed * speed}, - .distance = distance, + .twist = {.linear = motion.speed, .angular = side * point.curvature * motion.speed}, + .acceleration = + { + .linear = motion.acceleration, + .angular = side * (point.sharpness * motion.speed * motion.speed + + point.curvature * motion.acceleration), + }, + .distance = motion.distance, + }; + } + + case SegmentKind::LINE: { + const SpeedProfile::Sample motion = this->line.sample_motion(this->clock); + const Line::Point point = this->line.sample_point(motion.distance); + + return { + .pose = point.pose, + .twist = {.linear = motion.speed, .angular = point.curvature * motion.speed}, + .acceleration = + { + .linear = motion.acceleration, + .angular = + point.sharpness * motion.speed * motion.speed + point.curvature * motion.acceleration, + }, + .distance = motion.distance, }; } diff --git a/micras_nav/src/line.cpp b/micras_nav/src/line.cpp new file mode 100644 index 0000000..8e2ef50 --- /dev/null +++ b/micras_nav/src/line.cpp @@ -0,0 +1,83 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/core/math.hpp" +#include "micras/nav/curve_speed.hpp" +#include "micras/nav/line.hpp" +#include "micras/nav/speed_profile.hpp" +#include "micras/nav/state.hpp" + +namespace micras::nav { +bool Line::is_ready() const { + return this->ready; +} + +float Line::length() const { + return this->size < 2 ? 0.0F : static_cast(this->size - 1) * this->spacing; +} + +float Line::duration() const { + return this->size == 0 ? 0.0F : this->times.at(this->size - 1); +} + +float Line::get_finish_time() const { + return this->finish_time; +} + +Pose Line::get_start() const { + return {.position = {.x = this->xs.at(0), .y = this->ys.at(0)}, .orientation = this->get_heading(0)}; +} + +SpeedProfile::Sample Line::sample_motion(float time) const { + return CurveSpeed::sample( + std::span{this->speeds}.first(this->size), std::span{this->times}.first(this->size), this->spacing, time + ); +} + +Line::Point Line::sample_point(float distance) const { + const float position = std::clamp(distance / this->spacing, 0.0F, static_cast(this->size - 1)); + const auto index = static_cast(std::min(std::floor(position), static_cast(this->size - 2))); + const auto next = static_cast(index + 1); + const float fraction = position - static_cast(index); + + const float start_heading = this->get_heading(index); + const float turn = core::math::wrap_angle(this->get_heading(next) - start_heading); + + const float h00 = (1.0F + 2.0F * fraction) * (1.0F - fraction) * (1.0F - fraction); + const float h10 = fraction * (1.0F - fraction) * (1.0F - fraction) * this->spacing; + const float h01 = fraction * fraction * (3.0F - 2.0F * fraction); + const float h11 = fraction * fraction * (fraction - 1.0F) * this->spacing; + + const float start_curvature = this->curvatures.at(index); + const float end_curvature = this->curvatures.at(next); + + return { + .pose = + { + .position = + { + .x = h00 * this->xs.at(index) + h10 * std::cos(start_heading) + h01 * this->xs.at(next) + + h11 * std::cos(start_heading + turn), + .y = h00 * this->ys.at(index) + h10 * std::sin(start_heading) + h01 * this->ys.at(next) + + h11 * std::sin(start_heading + turn), + }, + .orientation = core::math::wrap_angle(start_heading + fraction * turn), + }, + .curvature = start_curvature + fraction * (end_curvature - start_curvature), + .sharpness = (end_curvature - start_curvature) / this->spacing, + }; +} + +float Line::get_heading(uint16_t index) const { + const uint16_t before = index == 0 ? 0 : index - 1; + const uint16_t after = std::min(index + 1, this->size - 1); + + return std::atan2(this->ys.at(after) - this->ys.at(before), this->xs.at(after) - this->xs.at(before)); +} +} // namespace micras::nav From 26cf332359580b19a831a1ad3116410bb83df646 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Fri, 25 Sep 2026 20:28:59 -0300 Subject: [PATCH 507/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Split=20the=20simu?= =?UTF-8?q?lator=20into=20an=20engine,=20a=20view,=20a=20bridge=20and=20an?= =?UTF-8?q?=20app=20over=20a=20host=20HAL?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The simulator knows no robot any more. engine/ holds the world, the clock, the run loop, the firmware thread, the devices, the arenas, the robot description and its model, scenarios and recording; view/ the window, the declarative panel and overlay and the video; bridge/ the monitor bridge; app/ the command line and the Target interface a robot implements. hal_host/ implements micras_hal on a PC and knows no physics. Everything about the Micras lives in targets/micras: its bindings, the proxies of its two SPI chips, the hand written Cube layer, the robot description in robot.toml, its tests, tools and baselines, and the firmware submodule, which now follows high-level-review. The rule everything serves is that a run is reproducible: the same binary with the same arguments writes byte identical data.csv and meta.json, headless or watched, recorded or not, with a bridge nobody connects to or without one. Noise is seeded. Baselines are summaries with tolerances instead of recordings. The ten contest mazes join the two there were, and the old models, the Python maze generator, the firmware build and the recorded runs are gone. SPEC_1_WORK.md and SPEC_2_DECISIONS.md record the work and the decisions. --- .../host/include/micras/hal/host/board.hpp | 143 +++++++++ .../host/include/micras/hal/host/clock.hpp | 146 +++++++++ .../host/include/micras/hal/host/ports.hpp | 290 ++++++++++++++++++ micras_hal/host/include/stm32_host.h | 284 +++++++++++++++++ micras_hal/host/src/adc_dma.cpp | 114 +++++++ micras_hal/host/src/board.cpp | 173 +++++++++++ micras_hal/host/src/clock.cpp | 57 ++++ micras_hal/host/src/crc.cpp | 79 +++++ micras_hal/host/src/encoder.cpp | 29 ++ micras_hal/host/src/flash.cpp | 133 ++++++++ micras_hal/host/src/gpio.cpp | 26 ++ micras_hal/host/src/mcu.cpp | 78 +++++ micras_hal/host/src/ports.cpp | 33 ++ micras_hal/host/src/pwm.cpp | 88 ++++++ micras_hal/host/src/pwm_dma.cpp | 86 ++++++ micras_hal/host/src/spi.cpp | 74 +++++ micras_hal/host/src/timer.cpp | 34 ++ micras_hal/host/src/uart_dma.cpp | 82 +++++ 18 files changed, 1949 insertions(+) create mode 100644 micras_hal/host/include/micras/hal/host/board.hpp create mode 100644 micras_hal/host/include/micras/hal/host/clock.hpp create mode 100644 micras_hal/host/include/micras/hal/host/ports.hpp create mode 100644 micras_hal/host/include/stm32_host.h create mode 100644 micras_hal/host/src/adc_dma.cpp create mode 100644 micras_hal/host/src/board.cpp create mode 100644 micras_hal/host/src/clock.cpp create mode 100644 micras_hal/host/src/crc.cpp create mode 100644 micras_hal/host/src/encoder.cpp create mode 100644 micras_hal/host/src/flash.cpp create mode 100644 micras_hal/host/src/gpio.cpp create mode 100644 micras_hal/host/src/mcu.cpp create mode 100644 micras_hal/host/src/ports.cpp create mode 100644 micras_hal/host/src/pwm.cpp create mode 100644 micras_hal/host/src/pwm_dma.cpp create mode 100644 micras_hal/host/src/spi.cpp create mode 100644 micras_hal/host/src/timer.cpp create mode 100644 micras_hal/host/src/uart_dma.cpp diff --git a/micras_hal/host/include/micras/hal/host/board.hpp b/micras_hal/host/include/micras/hal/host/board.hpp new file mode 100644 index 0000000..1d62ef8 --- /dev/null +++ b/micras_hal/host/include/micras/hal/host/board.hpp @@ -0,0 +1,143 @@ +/** + * @file + * + * @brief Every host peripheral port, found by the handle or pin that names it. + */ + +#ifndef MICRAS_HAL_HOST_BOARD_HPP +#define MICRAS_HAL_HOST_BOARD_HPP + +#include +#include +#include +#include + +#include "micras/hal/host/ports.hpp" + +namespace micras::hal::host { +/** + * @brief Registry of the ports the host backend and the outside world share. + * + * @note A port is created the first time either side asks for it, keyed by the + * address of the Cube handle or GPIO port that names it, so the firmware's + * own configuration is the key: `.port = LED_Red_GPIO_Port, .pin = + * LED_Red_Pin` in target.hpp and the same two names in a robot's bindings + * reach the same port. Nothing here knows a physics engine; bindings + * connect ports to whatever simulates the board. + * + * @note Not thread safe, and it does not need to be: the firmware and whatever + * drives the ports take strict turns. + */ +class Board { +public: + Board() = delete; + + /** + * @brief Get the port of a GPIO pin. + * + * @param port GPIO port of the pin. + * @param pin Pin mask. + * @return The port. + */ + static GpioPort& gpio(const void* port, uint16_t pin); + + /** + * @brief Get the port of a PWM channel. + * + * @param timer Timer handle. + * @param channel Timer channel. + * @return The port. + */ + static PwmPort& pwm(const void* timer, uint32_t channel); + + /** + * @brief Get the port of a DMA-fed timer channel. + * + * @param timer Timer handle. + * @param channel Timer channel. + * @return The port. + */ + static PwmDmaPort& pwm_dma(const void* timer, uint32_t channel); + + /** + * @brief Get the port of an ADC. + * + * @param adc ADC handle. + * @return The port. + */ + static AdcPort& adc(const void* adc); + + /** + * @brief Get the port of a UART. + * + * @param uart UART handle. + * @return The port. + */ + static UartPort& uart(const void* uart); + + /** + * @brief Get the port of a timer in encoder mode. + * + * @param timer Timer handle. + * @return The port. + */ + static EncoderPort& encoder(const void* timer); + + /** + * @brief Get a named sample port, for a swapped chip driver. + * + * @param name Name both sides agree on. + * @return The port. + */ + static SamplePort& samples(std::string_view name); + + /** + * @brief Get the flash. + * + * @return The port. + */ + static FlashPort& flash(); + + /** + * @brief Get the microcontroller core. + * + * @return The port. + */ + static McuPort& mcu(); + + /** + * @brief Name a GPIO pin, as the Cube layer labels it. + * + * @param port GPIO port of the pin. + * @param pin Pin mask. + * @param name Label. + */ + static void name_gpio(const void* port, uint16_t pin, std::string_view name); + + /** + * @brief Name a peripheral handle, as the Cube layer calls it. + * + * @note Ports created for the handle later are named after it. + * + * @param handle Peripheral handle. + * @param name Name. + */ + static void name_handle(const void* handle, std::string_view name); + + /** + * @brief List the ports the firmware used that nothing is bound to. + * + * @return Their names, one per port. + */ + static std::vector unbound(); + + /** + * @brief Forget every port and name. + * + * @note For tests. References to ports obtained before are dangling after. + */ + static void reset(); +}; +} // namespace micras::hal::host + +#endif // MICRAS_HAL_HOST_BOARD_HPP diff --git a/micras_hal/host/include/micras/hal/host/clock.hpp b/micras_hal/host/include/micras/hal/host/clock.hpp new file mode 100644 index 0000000..108d738 --- /dev/null +++ b/micras_hal/host/include/micras/hal/host/clock.hpp @@ -0,0 +1,146 @@ +/** + * @file + * + * @brief The only place time exists for a firmware running on a host. + */ + +#ifndef MICRAS_HAL_HOST_CLOCK_HPP +#define MICRAS_HAL_HOST_CLOCK_HPP + +#include +#include + +namespace micras::hal::host { +/** + * @brief Counts the core cycles the firmware sees, and hands time over to the world. + * + * @note Every read of the timer costs a fixed quantum of simulated time, one + * microsecond unless configured otherwise, since a read on the robot also + * takes time and a loop that polls the timer must see it advance. When a + * read crosses the end of a step, the handover callback runs: whatever + * drives the world advances it by one step and returns, and the read + * returns after it. So a busy wait runs the world exactly as long as it + * waits, and time inside an iteration is the number of timer reads times + * the quantum: deterministic, but not a measurement of anything. + * + * @note The counter wraps at 32 bits exactly as the cycle counter does. + */ +class Clock { +public: + /** + * @brief Called at every step boundary a read crosses. + */ + using Handover = std::function; + + /** + * @brief Get the process-wide clock. + * + * @return The clock. + */ + static Clock& instance(); + + /** + * @brief Set the core frequency and the cost of one read. + * + * @param cycles_per_microsecond Core clock, in cycles per microsecond. + * @param quantum_us Simulated time one read costs, in microseconds. + */ + void configure(uint32_t cycles_per_microsecond, uint32_t quantum_us = 1); + + /** + * @brief Hand time over to the world at every step. + * + * @param step_us Length of a step, in microseconds. + * @param handover Called when a read crosses a step boundary. + */ + void set_handover(uint32_t step_us, Handover handover); + + /** + * @brief Stop handing time over; reads keep counting. + */ + void clear_handover(); + + /** + * @brief Read the cycle counter, as the firmware does. + * + * @return Cycles since the start, modulo 2^32. + */ + uint32_t read_cycles(); + + /** + * @brief Read the millisecond counter, as the firmware does. + * + * @return Milliseconds since the start, modulo 2^32. + */ + uint32_t read_ms(); + + /** + * @brief Get the time without charging for it. + * + * @return Cycles since the start, not wrapped. + */ + uint64_t now() const { return this->cycles; } + + /** + * @brief Get the core frequency. + * + * @return Cycles per microsecond. + */ + uint32_t cycles_per_microsecond() const { return this->cycles_per_us; } + + /** + * @brief Get how many step boundaries were handed over. + * + * @return Number of handovers. + */ + uint64_t steps() const { return this->handed_over; } + + /** + * @brief Rewind to the start, with no handover. + */ + void reset(); + +private: + /** + * @brief Charge one read and hand over at every boundary it crosses. + */ + void charge(); + + /** + * @brief Core cycles per microsecond. + */ + uint32_t cycles_per_us{1}; + + /** + * @brief Cycles one read costs. + */ + uint64_t quantum{1}; + + /** + * @brief Cycles since the start. + */ + uint64_t cycles{0}; + + /** + * @brief Cycles per step, zero when nothing is handed over. + */ + uint64_t step{0}; + + /** + * @brief Cycle count of the next step boundary. + */ + uint64_t next_boundary{0}; + + /** + * @brief Number of boundaries handed over. + */ + uint64_t handed_over{0}; + + /** + * @brief What runs at a boundary. + */ + Handover handover; +}; +} // namespace micras::hal::host + +#endif // MICRAS_HAL_HOST_CLOCK_HPP diff --git a/micras_hal/host/include/micras/hal/host/ports.hpp b/micras_hal/host/include/micras/hal/host/ports.hpp new file mode 100644 index 0000000..f192967 --- /dev/null +++ b/micras_hal/host/include/micras/hal/host/ports.hpp @@ -0,0 +1,290 @@ +/** + * @file + * + * @brief What each host peripheral holds: the state the firmware and the outside world share. + */ + +#ifndef MICRAS_HAL_HOST_PORTS_HPP +#define MICRAS_HAL_HOST_PORTS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace micras::hal::host { +/** + * @brief What every port has: a name and who has used it. + * + * @note touched is set by the firmware side, bound by whatever connects the port + * to the outside world. A port the firmware touched that nothing is bound + * to is a wiring mistake, and is reported. + */ +struct Port { + /** + * @brief Label from the Cube layer, or a description of the handle. + */ + std::string name; + + /** + * @brief Whether the firmware used the port. + */ + bool touched{false}; + + /** + * @brief Whether something outside the firmware is connected to it. + */ + bool bound{false}; +}; + +/** + * @brief One GPIO pin. + */ +struct GpioPort : Port { + /** + * @brief Level the firmware drives, as its output data register holds it. + */ + bool output{false}; + + /** + * @brief Level driven from outside, which is what a read returns when set. + */ + std::optional input; + + /** + * @brief Get the level the pin reads. + * + * @return The external level when one is driven, the output level otherwise. + */ + bool read() const { return this->input.value_or(this->output); } +}; + +/** + * @brief One PWM output channel. + */ +struct PwmPort : Port { + /** + * @brief Fraction of the period the output is active, in percent, polarity applied. + */ + float duty_cycle{0.0F}; + + /** + * @brief Frequency the channel runs at, in hertz. + */ + float frequency{0.0F}; +}; + +/** + * @brief One timer channel whose compare register a DMA stream feeds. + */ +struct PwmDmaPort : Port { + /** + * @brief Compare values of the last transfer the firmware started. + */ + std::vector compares; + + /** + * @brief Counts in one period, the autoreload plus one. + */ + uint32_t period{0}; + + /** + * @brief Whether a transfer is still going out. + */ + bool busy{false}; + + /** + * @brief Host clock cycle the current transfer ends at, one timer period per value. + */ + uint64_t transfer_end{0}; + + /** + * @brief Number of transfers the firmware started. + */ + uint32_t transfers{0}; +}; + +/** + * @brief One ADC with its DMA stream. + * + * @note The firmware hands over the buffer the DMA writes; a device fills it + * with a whole sequence and then calls finish_sequence(), which is what + * the transfer complete interrupt does on the robot. + */ +struct AdcPort : Port { + /** + * @brief Buffer the firmware handed to the DMA, in one of its two widths. + */ + ///@{ + std::span buffer16; + std::span buffer32; + ///@} + + /** + * @brief Number of conversions in one sequence, from the ADC configuration. + */ + uint32_t conversions{0}; + + /** + * @brief Called at the end of a sequence, as the conversion complete interrupt would. + */ + std::function complete; + + /** + * @brief Number of sequences finished. + */ + uint32_t sequences{0}; + + /** + * @brief Get how many conversions the buffer holds. + * + * @return Size of the buffer, zero before the DMA was started. + */ + std::size_t size() const { return this->buffer16.empty() ? this->buffer32.size() : this->buffer16.size(); } + + /** + * @brief Write one conversion result. + * + * @param index Index in the buffer; ignored when out of range. + * @param counts Result, in ADC counts. + * + * @note Const because the buffer belongs to the firmware and is only viewed through a span: the port itself + * does not change. + */ + void write(std::size_t index, uint32_t counts) const; + + /** + * @brief End the sequence, as the DMA interrupt does. + */ + void finish_sequence(); +}; + +/** + * @brief One UART with its DMA streams. + */ +struct UartPort : Port { + /** + * @brief Circular buffer the firmware handed to the receive DMA. + */ + std::span rx_buffer; + + /** + * @brief Index the next received byte goes to, as the DMA counter implies it. + */ + std::size_t rx_head{0}; + + /** + * @brief Bytes the firmware handed to the transmit DMA and not yet sent. + */ + std::deque tx; + + /** + * @brief Bytes received while the receiver was not started. + */ + uint32_t lost_bytes{0}; + + /** + * @brief Baud rate from the UART configuration. + */ + uint32_t baud_rate{0}; + + /** + * @brief Put one received byte where the DMA would. + * + * @param byte Byte on the wire. + */ + void receive(uint8_t byte); + + /** + * @brief Check whether a transmission is in progress. + * + * @return True while bytes are waiting to leave. + */ + bool transmitting() const { return not this->tx.empty(); } +}; + +/** + * @brief The part of the flash the firmware may use, as a byte array. + */ +struct FlashPort : Port { + /** + * @brief Contents, erased to 0xFF. + */ + std::vector bytes; + + /** + * @brief Number of program and erase operations performed. + */ + ///@{ + uint32_t writes{0}; + uint32_t erases{0}; + ///@} +}; + +/** + * @brief What the microcontroller core reports. + */ +struct McuPort : Port { + /** + * @brief Independent watchdog timeout the firmware set, zero before it set one. + */ + uint32_t watchdog_timeout_ms{0}; + + /** + * @brief Host time of the last refresh, in timer cycles. + */ + uint64_t last_refresh{0}; + + /** + * @brief Number of refreshes. + */ + uint64_t refreshes{0}; + + /** + * @brief Number of times the watchdog would have reset the chip. + */ + uint32_t watchdog_expiries{0}; + + /** + * @brief Number of emergency stops the firmware performed. + */ + uint32_t emergency_stops{0}; +}; + +/** + * @brief A quadrature count, as a timer in encoder mode holds it. + */ +struct EncoderPort : Port { + /** + * @brief Count, positive forward. + */ + int32_t count{0}; +}; + +/** + * @brief Samples a swapped chip driver reads, such as an inertial measurement unit's. + */ +struct SamplePort : Port { + /** + * @brief Largest number of values one sample holds. + */ + static constexpr std::size_t max_values{16}; + + /** + * @brief Latest sample. + */ + std::array values{}; + + /** + * @brief Incremented with every new sample. + */ + uint32_t sequence{0}; +}; +} // namespace micras::hal::host + +#endif // MICRAS_HAL_HOST_PORTS_HPP diff --git a/micras_hal/host/include/stm32_host.h b/micras_hal/host/include/stm32_host.h new file mode 100644 index 0000000..cecec7f --- /dev/null +++ b/micras_hal/host/include/stm32_host.h @@ -0,0 +1,284 @@ +/** + * @file + * + * @brief The STM32 handle types and HAL constants a fake Cube layer builds on. + * + * @note The micras_hal headers name STM32 handle types in their configurations, + * and a robot's target.hpp names handles, channels and SPI modes. On a PC + * none of the vendor HAL exists, so a robot's fake Cube layer includes this + * file from its main.h and adds only what is its own: its handles, pins, + * init functions and clock tree. The types carry just the fields the host + * backend reads or writes; the constants keep the vendor's values where the + * firmware could compare them. + */ + +#ifndef STM32_HOST_H +#define STM32_HOST_H + +#include + +/***************************************** + * Status + *****************************************/ + +/** + * @brief Result of a vendor HAL call. + */ +enum HAL_StatusTypeDef : uint8_t { + HAL_OK = 0, + HAL_ERROR = 1, + HAL_BUSY = 2, + HAL_TIMEOUT = 3, +}; + +/***************************************** + * GPIO + *****************************************/ + +/** + * @brief A GPIO port, identified by its address alone. + */ +struct GPIO_TypeDef { + /** + * @brief Letter of the port, for messages. + */ + char name; +}; + +///@{ +inline constexpr uint16_t GPIO_PIN_0{0x0001U}; +inline constexpr uint16_t GPIO_PIN_1{0x0002U}; +inline constexpr uint16_t GPIO_PIN_2{0x0004U}; +inline constexpr uint16_t GPIO_PIN_3{0x0008U}; +inline constexpr uint16_t GPIO_PIN_4{0x0010U}; +inline constexpr uint16_t GPIO_PIN_5{0x0020U}; +inline constexpr uint16_t GPIO_PIN_6{0x0040U}; +inline constexpr uint16_t GPIO_PIN_7{0x0080U}; +inline constexpr uint16_t GPIO_PIN_8{0x0100U}; +inline constexpr uint16_t GPIO_PIN_9{0x0200U}; +inline constexpr uint16_t GPIO_PIN_10{0x0400U}; +inline constexpr uint16_t GPIO_PIN_11{0x0800U}; +inline constexpr uint16_t GPIO_PIN_12{0x1000U}; +inline constexpr uint16_t GPIO_PIN_13{0x2000U}; +inline constexpr uint16_t GPIO_PIN_14{0x4000U}; +inline constexpr uint16_t GPIO_PIN_15{0x8000U}; + +///@} + +/***************************************** + * Timers + *****************************************/ + +/** + * @brief The registers of a timer the host backend models. + * + * @note kernel_clock is not a register: it is the frequency the timer counts + * at, which on the chip comes from the clock tree the init functions + * configure. The fake init function writes it. + */ +struct TIM_TypeDef { + uint32_t CR1; + uint32_t CCER; + uint32_t PSC; + uint32_t ARR; + uint32_t CNT; + uint32_t kernel_clock; +}; + +/** + * @brief Base configuration of a timer, as the init function fills it. + */ +struct TIM_Base_InitTypeDef { + uint32_t Prescaler; + uint32_t CounterMode; + uint32_t Period; +}; + +/** + * @brief State of a timer handle. + */ +enum HAL_TIM_StateTypeDef : uint8_t { + HAL_TIM_STATE_RESET = 0, + HAL_TIM_STATE_READY = 1, + HAL_TIM_STATE_BUSY = 2, +}; + +/** + * @brief A timer handle. + */ +struct TIM_HandleTypeDef { + TIM_TypeDef* Instance; + TIM_Base_InitTypeDef Init; + HAL_TIM_StateTypeDef State; +}; + +///@{ +inline constexpr uint32_t TIM_CHANNEL_1{0x00000000U}; +inline constexpr uint32_t TIM_CHANNEL_2{0x00000004U}; +inline constexpr uint32_t TIM_CHANNEL_3{0x00000008U}; +inline constexpr uint32_t TIM_CHANNEL_4{0x0000000CU}; +inline constexpr uint32_t TIM_CHANNEL_ALL{0x0000003CU}; +///@} + +///@{ +inline constexpr uint32_t TIM_CR1_CMS{0x3U << 5U}; +inline constexpr uint32_t TIM_CCER_CC1P{0x1U << 1U}; +inline constexpr uint32_t TIM_COUNTERMODE_UP{0x00000000U}; +inline constexpr uint32_t TIM_COUNTERMODE_CENTERALIGNED1{0x1U << 5U}; + +///@} + +/***************************************** + * ADC + *****************************************/ + +/** + * @brief Configuration of an ADC, as the init function fills it. + */ +struct ADC_InitTypeDef { + uint32_t Resolution; + uint32_t NbrOfConversion; +}; + +///@{ +inline constexpr uint32_t HAL_ADC_STATE_RESET{0x00000000U}; +inline constexpr uint32_t HAL_ADC_STATE_READY{0x00000001U}; + +///@} + +/** + * @brief An ADC handle. + */ +struct ADC_HandleTypeDef { + ADC_InitTypeDef Init; + uint32_t State; +}; + +/***************************************** + * SPI + *****************************************/ + +///@{ +inline constexpr uint32_t SPI_POLARITY_LOW{0x00000000U}; +inline constexpr uint32_t SPI_POLARITY_HIGH{0x1U << 25U}; +inline constexpr uint32_t SPI_PHASE_1EDGE{0x00000000U}; +inline constexpr uint32_t SPI_PHASE_2EDGE{0x1U << 24U}; + +///@} + +/** + * @brief Configuration of an SPI bus, as the init function fills it. + */ +struct SPI_InitTypeDef { + uint32_t CLKPolarity; + uint32_t CLKPhase; +}; + +/** + * @brief State of an SPI handle. + */ +enum HAL_SPI_StateTypeDef : uint8_t { + HAL_SPI_STATE_RESET = 0, + HAL_SPI_STATE_READY = 1, +}; + +/** + * @brief An SPI handle. + */ +struct SPI_HandleTypeDef { + SPI_InitTypeDef Init; + HAL_SPI_StateTypeDef State; +}; + +/***************************************** + * UART + *****************************************/ + +/** + * @brief State of a UART handle. + */ +enum HAL_UART_StateTypeDef : uint8_t { + HAL_UART_STATE_RESET = 0x00, + HAL_UART_STATE_READY = 0x20, + HAL_UART_STATE_BUSY_TX = 0x21, + HAL_UART_STATE_BUSY_RX = 0x22, +}; + +/** + * @brief Configuration of a UART, as the init function fills it. + */ +struct UART_InitTypeDef { + uint32_t BaudRate; +}; + +/** + * @brief A UART handle. + */ +struct UART_HandleTypeDef { + UART_InitTypeDef Init; + HAL_UART_StateTypeDef gState; + HAL_UART_StateTypeDef RxState; +}; + +/***************************************** + * CRC + *****************************************/ + +///@{ +inline constexpr uint8_t DEFAULT_POLYNOMIAL_ENABLE{0x00U}; +inline constexpr uint8_t DEFAULT_POLYNOMIAL_DISABLE{0x01U}; +inline constexpr uint8_t DEFAULT_INIT_VALUE_ENABLE{0x00U}; +inline constexpr uint8_t DEFAULT_INIT_VALUE_DISABLE{0x01U}; +inline constexpr uint32_t CRC_POLYLENGTH_32B{0x00000000U}; +inline constexpr uint32_t CRC_POLYLENGTH_16B{0x1U << 3U}; +inline constexpr uint32_t CRC_POLYLENGTH_8B{0x2U << 3U}; +inline constexpr uint32_t CRC_POLYLENGTH_7B{0x3U << 3U}; +inline constexpr uint32_t CRC_INPUTDATA_INVERSION_NONE{0x00000000U}; +inline constexpr uint32_t CRC_INPUTDATA_INVERSION_BYTE{0x1U << 5U}; +inline constexpr uint32_t CRC_INPUTDATA_INVERSION_HALFWORD{0x2U << 5U}; +inline constexpr uint32_t CRC_INPUTDATA_INVERSION_WORD{0x3U << 5U}; +inline constexpr uint32_t CRC_OUTPUTDATA_INVERSION_DISABLE{0x00000000U}; +inline constexpr uint32_t CRC_OUTPUTDATA_INVERSION_ENABLE{0x1U << 7U}; +inline constexpr uint32_t CRC_INPUTDATA_FORMAT_BYTES{0x00000001U}; +inline constexpr uint32_t CRC_INPUTDATA_FORMAT_HALFWORDS{0x00000002U}; +inline constexpr uint32_t CRC_INPUTDATA_FORMAT_WORDS{0x00000003U}; +inline constexpr uint32_t DEFAULT_CRC32_POLY{0x04C11DB7U}; +inline constexpr uint32_t DEFAULT_CRC_INITVALUE{0xFFFFFFFFU}; + +///@} + +/** + * @brief Configuration of the CRC unit, as the init function fills it. + */ +struct CRC_InitTypeDef { + uint8_t DefaultPolynomialUse; + uint8_t DefaultInitValueUse; + uint32_t GeneratingPolynomial; + uint32_t CRCLength; + uint32_t InitValue; + uint32_t InputDataInversionMode; + uint32_t OutputDataInversionMode; +}; + +/** + * @brief The CRC unit's handle. + */ +struct CRC_HandleTypeDef { + CRC_InitTypeDef Init; + uint32_t InputDataFormat; +}; + +/***************************************** + * FMAC + *****************************************/ + +/** + * @brief The filter accelerator's handle; nothing on the host uses it. + */ +struct FMAC_HandleTypeDef { + uint32_t State; +}; + +inline constexpr uint32_t HAL_FMAC_STATE_RESET{0x00000000U}; + +#endif // STM32_HOST_H diff --git a/micras_hal/host/src/adc_dma.cpp b/micras_hal/host/src/adc_dma.cpp new file mode 100644 index 0000000..6114873 --- /dev/null +++ b/micras_hal/host/src/adc_dma.cpp @@ -0,0 +1,114 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/host/board.hpp" + +namespace micras::hal { +std::array AdcDma::instances{}; + +AdcDma::AdcDma(const Config& config) : + max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { + if (this->handle->State == HAL_ADC_STATE_RESET) { + config.init_function(); + } + + this->initialized = this->handle->State == HAL_ADC_STATE_READY; +} + +AdcDma::~AdcDma() { + std::ranges::replace(instances, this, static_cast(nullptr)); +} + +bool AdcDma::start_dma(std::span buffer) { + host::AdcPort& port = host::Board::adc(this->handle); + port.touched = true; + port.buffer32 = buffer; + port.buffer16 = {}; + port.conversions = this->handle->Init.NbrOfConversion; + port.complete = [adc = this->handle] { on_sequence_complete(adc); }; + return true; +} + +bool AdcDma::start_dma(std::span buffer) { + host::AdcPort& port = host::Board::adc(this->handle); + port.touched = true; + port.buffer16 = buffer; + port.buffer32 = {}; + port.conversions = this->handle->Init.NbrOfConversion; + port.complete = [adc = this->handle] { on_sequence_complete(adc); }; + return true; +} + +bool AdcDma::start_dma(std::span buffer, std::span snapshot) { + if (snapshot.size() != buffer.size()) { + this->initialized = false; + return false; + } + + this->buffer = buffer; + this->snapshot = snapshot; + + auto* const slot = std::ranges::find(instances, nullptr); + + if (slot == instances.end()) { + this->initialized = false; + return false; + } + + *slot = this; + + return this->start_dma(buffer); +} + +uint32_t AdcDma::read_snapshot(std::span destination) const { + uint32_t before = this->sequence; + + while (true) { + std::ranges::copy(this->snapshot, destination.begin()); + + const uint32_t after = this->sequence; + + if (after == before) { + return after; + } + + before = after; + } +} + +void AdcDma::on_sequence_complete(const ADC_HandleTypeDef* handle) { + for (AdcDma* instance : instances) { + if (instance != nullptr and instance->handle == handle) { + std::ranges::copy(instance->buffer, instance->snapshot.begin()); + instance->sequence = instance->sequence + 1; + return; + } + } +} + +void AdcDma::stop_dma() { + host::AdcPort& port = host::Board::adc(this->handle); + port.buffer16 = {}; + port.buffer32 = {}; + port.complete = nullptr; +} + +uint16_t AdcDma::get_max_reading() const { + return this->max_reading; +} + +float AdcDma::get_reference_voltage() const { + return this->reference_voltage; +} + +bool AdcDma::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/board.cpp b/micras_hal/host/src/board.cpp new file mode 100644 index 0000000..e5709f3 --- /dev/null +++ b/micras_hal/host/src/board.cpp @@ -0,0 +1,173 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/hal/host/board.hpp" + +namespace micras::hal::host { +namespace { +/** + * @brief Key of a port: the handle or GPIO port, and the channel or pin. + */ +using Key = std::pair; + +/** + * @brief Every port and name, created on first use. + */ +struct Registry { + std::map gpios; + std::map pwms; + std::map pwm_dmas; + std::map adcs; + std::map uarts; + std::map encoders; + std::map> samples; + std::map gpio_names; + std::map handle_names; + FlashPort flash{{.name = "flash"}}; + McuPort mcu{{.name = "mcu"}}; +}; + +/** + * @brief Get the registry. + * + * @return The process-wide registry. + */ +Registry& registry() { + static Registry instance; + return instance; +} + +/** + * @brief Name a handle, falling back to its address. + * + * @param handle Peripheral handle. + * @return Its name. + */ +std::string handle_name(const void* handle) { + const auto found = registry().handle_names.find(handle); + return found != registry().handle_names.end() ? found->second : + std::format("handle@{:#x}", std::bit_cast(handle)); +} + +/** + * @brief Find a port, creating it with a name on first use. + * + * @param ports Ports of one kind. + * @param key Key of the port. + * @param name Makes the name of a new port. + * @return The port. + */ +template +typename Map::mapped_type& find_or_create(Map& ports, const typename Map::key_type& key, const Namer& name) { + auto found = ports.find(key); + + if (found == ports.end()) { + found = ports.emplace(key, typename Map::mapped_type{}).first; + found->second.name = name(); + } + + return found->second; +} +} // namespace + +GpioPort& Board::gpio(const void* port, uint16_t pin) { + const Key key{port, pin}; + + return find_or_create(registry().gpios, key, [&key] { + const auto found = registry().gpio_names.find(key); + return found != registry().gpio_names.end() ? found->second : + std::format("{} pin {:#06x}", handle_name(key.first), key.second); + }); +} + +PwmPort& Board::pwm(const void* timer, uint32_t channel) { + return find_or_create(registry().pwms, Key{timer, channel}, [timer, channel] { + return std::format("{} channel {}", handle_name(timer), channel / 4 + 1); + }); +} + +PwmDmaPort& Board::pwm_dma(const void* timer, uint32_t channel) { + return find_or_create(registry().pwm_dmas, Key{timer, channel}, [timer, channel] { + return std::format("{} channel {} DMA", handle_name(timer), channel / 4 + 1); + }); +} + +AdcPort& Board::adc(const void* adc) { + return find_or_create(registry().adcs, adc, [adc] { return handle_name(adc); }); +} + +UartPort& Board::uart(const void* uart) { + return find_or_create(registry().uarts, uart, [uart] { return handle_name(uart); }); +} + +EncoderPort& Board::encoder(const void* timer) { + return find_or_create(registry().encoders, timer, [timer] { return handle_name(timer) + " encoder"; }); +} + +SamplePort& Board::samples(std::string_view name) { + auto found = registry().samples.find(name); + + if (found == registry().samples.end()) { + found = registry().samples.emplace(std::string{name}, SamplePort{}).first; + found->second.name = std::string{name}; + } + + return found->second; +} + +FlashPort& Board::flash() { + return registry().flash; +} + +McuPort& Board::mcu() { + return registry().mcu; +} + +void Board::name_gpio(const void* port, uint16_t pin, std::string_view name) { + const Key key{port, pin}; + registry().gpio_names[key] = std::string{name}; + + const auto found = registry().gpios.find(key); + + if (found != registry().gpios.end()) { + found->second.name = std::string{name}; + } +} + +void Board::name_handle(const void* handle, std::string_view name) { + registry().handle_names[handle] = std::string{name}; +} + +std::vector Board::unbound() { + std::vector names; + + const auto collect = [&names](const auto& ports) { + for (const auto& [key, port] : ports) { + if (port.touched and not port.bound) { + names.push_back(port.name); + } + } + }; + + collect(registry().gpios); + collect(registry().pwms); + collect(registry().pwm_dmas); + collect(registry().adcs); + collect(registry().uarts); + collect(registry().encoders); + collect(registry().samples); + + return names; +} + +void Board::reset() { + registry() = Registry{}; +} +} // namespace micras::hal::host diff --git a/micras_hal/host/src/clock.cpp b/micras_hal/host/src/clock.cpp new file mode 100644 index 0000000..051c506 --- /dev/null +++ b/micras_hal/host/src/clock.cpp @@ -0,0 +1,57 @@ +/** + * @file + */ + +#include + +#include "micras/hal/host/clock.hpp" + +namespace micras::hal::host { +Clock& Clock::instance() { + static Clock clock; + return clock; +} + +void Clock::configure(uint32_t cycles_per_microsecond, uint32_t quantum_us) { + this->cycles_per_us = cycles_per_microsecond; + this->quantum = static_cast(quantum_us) * cycles_per_microsecond; +} + +void Clock::set_handover(uint32_t step_us, Handover handover) { + this->step = static_cast(step_us) * this->cycles_per_us; + this->next_boundary = (this->cycles / this->step + 1) * this->step; + this->handover = std::move(handover); +} + +void Clock::clear_handover() { + this->step = 0; + this->handover = nullptr; +} + +void Clock::charge() { + this->cycles += this->quantum; + + while (this->step != 0 and this->cycles >= this->next_boundary) { + this->next_boundary += this->step; + this->handed_over++; + + if (this->handover) { + this->handover(); + } + } +} + +uint32_t Clock::read_cycles() { + this->charge(); + return static_cast(this->cycles); +} + +uint32_t Clock::read_ms() { + this->charge(); + return static_cast(this->cycles / (1000ULL * this->cycles_per_us)); +} + +void Clock::reset() { + *this = Clock{}; +} +} // namespace micras::hal::host diff --git a/micras_hal/host/src/crc.cpp b/micras_hal/host/src/crc.cpp new file mode 100644 index 0000000..4dc6923 --- /dev/null +++ b/micras_hal/host/src/crc.cpp @@ -0,0 +1,79 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/crc.hpp" + +namespace micras::hal { +namespace { +/** + * @brief Reverse the order of the lowest bits of a value. + * + * @param value Value to reflect. + * @param width Number of bits to reflect. + * @return The reflected bits. + */ +uint32_t reflect(uint32_t value, uint32_t width) { + uint32_t reflected = 0; + + for (uint32_t bit = 0; bit < width; bit++) { + reflected |= ((value >> bit) & 1U) << (width - 1 - bit); + } + + return reflected; +} + +/** + * @brief Get the width of the CRC a configuration computes. + * + * @param length CRCLength field. + * @return Width in bits. + */ +uint32_t width_of(uint32_t length) { + if (length == CRC_POLYLENGTH_7B) { + return 7; + } + + if (length == CRC_POLYLENGTH_8B) { + return 8; + } + + if (length == CRC_POLYLENGTH_16B) { + return 16; + } + + return 32; +} +} // namespace + +Crc::Crc(const Config& config) : handle{config.handle} { } + +uint32_t Crc::calculate(std::span data) { + const CRC_InitTypeDef& init = this->handle->Init; + const uint32_t width = width_of(init.CRCLength); + const uint32_t mask = width == 32 ? 0xFFFFFFFFU : (1U << width) - 1; + const uint32_t polynomial = + (init.DefaultPolynomialUse == DEFAULT_POLYNOMIAL_ENABLE ? DEFAULT_CRC32_POLY : init.GeneratingPolynomial) & + mask; + uint32_t crc = + (init.DefaultInitValueUse == DEFAULT_INIT_VALUE_ENABLE ? DEFAULT_CRC_INITVALUE : init.InitValue) & mask; + + for (const uint8_t byte : data) { + const uint32_t input = init.InputDataInversionMode == CRC_INPUTDATA_INVERSION_NONE ? byte : reflect(byte, 8); + + for (int bit = 7; bit >= 0; bit--) { + const uint32_t top = (crc >> (width - 1)) & 1U; + crc = (crc << 1U) & mask; + + if ((top ^ ((input >> static_cast(bit)) & 1U)) != 0) { + crc ^= polynomial; + } + } + } + + return init.OutputDataInversionMode == CRC_OUTPUTDATA_INVERSION_ENABLE ? reflect(crc, width) : crc; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/encoder.cpp b/micras_hal/host/src/encoder.cpp new file mode 100644 index 0000000..562b1e4 --- /dev/null +++ b/micras_hal/host/src/encoder.cpp @@ -0,0 +1,29 @@ +/** + * @file + */ + +#include + +#include "micras/hal/encoder.hpp" +#include "micras/hal/host/board.hpp" + +namespace micras::hal { +Encoder::Encoder(const Config& config) : handle{config.handle}, start_count{0} { + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + host::EncoderPort& port = host::Board::encoder(this->handle); + port.touched = true; + this->start_count = static_cast(port.count); + this->initialized = this->handle->State == HAL_TIM_STATE_READY; +} + +int32_t Encoder::get_counter() const { + return static_cast(static_cast(host::Board::encoder(this->handle).count) - this->start_count); +} + +bool Encoder::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/flash.cpp b/micras_hal/host/src/flash.cpp new file mode 100644 index 0000000..9404eed --- /dev/null +++ b/micras_hal/host/src/flash.cpp @@ -0,0 +1,133 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/hal/flash.hpp" +#include "micras/hal/host/board.hpp" + +namespace micras::hal { +namespace { +/** + * @brief Round a size up to whole flash words. + * + * @param size Size in bytes. + * @return The rounded size. + */ +constexpr uint32_t align_size(uint32_t size) { + return (size + FlashWord::size - 1) / FlashWord::size * FlashWord::size; +} + +/** + * @brief Get the flash contents, erased on first use. + * + * @return The port. + */ +host::FlashPort& storage() { + host::FlashPort& port = host::Board::flash(); + + if (port.bytes.size() != Flash::total_size) { + port.bytes.assign(Flash::total_size, FlashWord::erased_value); + } + + port.touched = true; + return port; +} +} // namespace + +FlashWord::FlashWord(std::span data) { + const auto data_address = std::bit_cast(data.data()); + + if (data.size() >= size and data_address % alignof(uint32_t) == 0) { + this->source = std::bit_cast(data.data()); + return; + } + + const std::span bytes{std::bit_cast(this->buffer.data()), size}; + + std::ranges::fill(bytes, erased_value); + std::ranges::copy(data.first(std::min(data.size(), size)), bytes.begin()); + + this->source = this->buffer.data(); +} + +const uint32_t* FlashWord::data() const { + return this->source; +} + +bool FlashWord::is_padded() const { + return this->source == this->buffer.data(); +} + +std::span Flash::read(uint32_t address, uint32_t size) { + if (address > total_size or size > total_size - address) { + return {}; + } + + return std::span{storage().bytes}.subspan(address, size); +} + +std::span Flash::read(uint16_t sector, uint32_t sector_address, uint32_t size) { + if (sector >= total_sectors or sector_address > sector_size) { + return {}; + } + + return read(sector * sector_size + sector_address, size); +} + +Flash::Status Flash::write(uint32_t address, std::span data) { + if (address % FlashWord::size != 0) { + return Status::MISALIGNED; + } + + if (address > total_size or align_size(data.size()) > total_size - address) { + return Status::OUT_OF_BOUNDS; + } + + host::FlashPort& port = storage(); + const std::span memory{port.bytes}; + + for (uint32_t offset = 0; offset < data.size(); offset += FlashWord::size) { + const FlashWord word{data.subspan(offset)}; + const std::span target = memory.subspan(address + offset, FlashWord::size); + + if (not std::ranges::all_of(target, [](uint8_t byte) { return byte == FlashWord::erased_value; })) { + return Status::ERROR; + } + + const std::span bytes{std::bit_cast(word.data()), FlashWord::size}; + std::ranges::copy(bytes, target.begin()); + port.writes++; + } + + return Status::OK; +} + +Flash::Status Flash::write(uint16_t sector, uint32_t sector_address, std::span data) { + if (sector >= total_sectors or sector_address > sector_size) { + return Status::OUT_OF_BOUNDS; + } + + return write(sector * sector_size + sector_address, data); +} + +Flash::Status Flash::erase_sectors(uint16_t start_sector, uint16_t number_of_sectors) { + if (start_sector >= total_sectors or number_of_sectors > total_sectors - start_sector) { + return Status::OUT_OF_BOUNDS; + } + + host::FlashPort& port = storage(); + const auto first = static_cast(start_sector) * sector_size; + const auto count = static_cast(number_of_sectors) * sector_size; + + std::fill(port.bytes.begin() + first, port.bytes.begin() + first + count, FlashWord::erased_value); + port.erases++; + + return Status::OK; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/gpio.cpp b/micras_hal/host/src/gpio.cpp new file mode 100644 index 0000000..a340c94 --- /dev/null +++ b/micras_hal/host/src/gpio.cpp @@ -0,0 +1,26 @@ +/** + * @file + */ + +#include "micras/hal/gpio.hpp" +#include "micras/hal/host/board.hpp" + +namespace micras::hal { +Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { } + +bool Gpio::read() const { + host::GpioPort& gpio = host::Board::gpio(this->port, this->pin); + gpio.touched = true; + return gpio.read(); +} + +void Gpio::write(bool state) { + host::GpioPort& gpio = host::Board::gpio(this->port, this->pin); + gpio.touched = true; + gpio.output = state; +} + +void Gpio::toggle() { + this->write(not host::Board::gpio(this->port, this->pin).output); +} +} // namespace micras::hal diff --git a/micras_hal/host/src/mcu.cpp b/micras_hal/host/src/mcu.cpp new file mode 100644 index 0000000..cbbb810 --- /dev/null +++ b/micras_hal/host/src/mcu.cpp @@ -0,0 +1,78 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/mcu.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::hal { +void Mcu::init(const Config& config) { + config.clock_init(); + + if (config.peripheral_clock_init != nullptr) { + config.peripheral_clock_init(); + } + + Timer::init(); + + for (const InitFunction init_function : config.peripheral_inits) { + init_function(); + } + + host::Board::mcu().touched = true; +} + +void Mcu::emergency_stop(std::span pwm_outputs, std::span enable_gpios) { + for (const auto& pwm_output : pwm_outputs) { + host::Board::pwm(pwm_output.handle, pwm_output.timer_channel).duty_cycle = 0.0F; + } + + for (const auto& enable_gpio : enable_gpios) { + host::Board::gpio(enable_gpio.port, enable_gpio.pin).output = false; + } + + host::McuPort& mcu = host::Board::mcu(); + mcu.emergency_stops++; + + const host::Clock& clock = host::Clock::instance(); + std::array message{}; + const auto result = std::format_to_n( + message.data(), static_cast(message.size()), "firmware emergency stop at step {} ({:.6f} s)\n", + clock.steps(), static_cast(clock.now()) / (1e6 * clock.cycles_per_microsecond()) + ); + const auto length = std::min(static_cast(result.size), message.size()); + const ssize_t written = ::write(STDERR_FILENO, message.data(), length); + static_cast(written); +} + +void Mcu::set_watchdog_timeout(uint32_t timeout_ms) { + host::McuPort& mcu = host::Board::mcu(); + mcu.touched = true; + mcu.watchdog_timeout_ms = timeout_ms; + mcu.last_refresh = host::Clock::instance().now(); +} + +void Mcu::refresh_watchdog() { + host::McuPort& mcu = host::Board::mcu(); + const host::Clock& clock = host::Clock::instance(); + const uint64_t timeout = static_cast(mcu.watchdog_timeout_ms) * 1000 * clock.cycles_per_microsecond(); + + if (mcu.watchdog_timeout_ms != 0 and clock.now() - mcu.last_refresh > timeout) { + mcu.watchdog_expiries++; + } + + mcu.last_refresh = clock.now(); + mcu.refreshes++; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/ports.cpp b/micras_hal/host/src/ports.cpp new file mode 100644 index 0000000..0ec8aea --- /dev/null +++ b/micras_hal/host/src/ports.cpp @@ -0,0 +1,33 @@ +/** + * @file + */ + +#include "micras/hal/host/ports.hpp" + +namespace micras::hal::host { +void AdcPort::write(std::size_t index, uint32_t counts) const { + if (index < this->buffer16.size()) { + this->buffer16[index] = static_cast(counts); + } else if (index < this->buffer32.size()) { + this->buffer32[index] = counts; + } +} + +void AdcPort::finish_sequence() { + this->sequences++; + + if (this->complete) { + this->complete(); + } +} + +void UartPort::receive(uint8_t byte) { + if (this->rx_buffer.empty()) { + this->lost_bytes++; + return; + } + + this->rx_buffer[this->rx_head] = byte; + this->rx_head = (this->rx_head + 1) % this->rx_buffer.size(); +} +} // namespace micras::hal::host diff --git a/micras_hal/host/src/pwm.cpp b/micras_hal/host/src/pwm.cpp new file mode 100644 index 0000000..750e885 --- /dev/null +++ b/micras_hal/host/src/pwm.cpp @@ -0,0 +1,88 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/host/board.hpp" +#include "micras/hal/pwm.hpp" + +namespace micras::hal { +namespace { +/** + * @brief Mask of the channel offset, which the vendor HAL uses as a bit shift. + */ +constexpr uint32_t channel_shift_mask{0x1F}; + +/** + * @brief Check whether a timer counts up and down. + * + * @param handle Timer handle. + * @return True in a center-aligned mode. + */ +bool is_center_aligned(const TIM_HandleTypeDef* handle) { + return (handle->Instance->CR1 & TIM_CR1_CMS) != 0; +} +} // namespace + +Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel}, inverted{config.inverted} { + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + if (this->inverted) { + this->handle->Instance->CCER |= TIM_CCER_CC1P << (this->channel & channel_shift_mask); + } + + this->set_duty_cycle(0.0F); + this->initialized = this->handle->State == HAL_TIM_STATE_READY; + + host::PwmPort& port = host::Board::pwm(this->handle, this->channel); + port.frequency = this->get_frequency(); +} + +void Pwm::set_duty_cycle(float duty_cycle) { + duty_cycle = std::clamp(duty_cycle, 0.0F, 100.0F); + + if (this->inverted) { + duty_cycle = 100.0F - duty_cycle; + } + + const uint32_t autoreload = this->handle->Instance->ARR; + const float scaled = duty_cycle * static_cast(autoreload + 1) * 0.01F; + + // NOLINTNEXTLINE(bugprone-incorrect-roundings): the rounding the firmware's own driver does. + const auto compare = static_cast(scaled + 0.5F); + const float counts = + is_center_aligned(this->handle) ? static_cast(autoreload) : static_cast(autoreload + 1); + const float active = std::min(100.0F, 100.0F * static_cast(compare) / counts); + + host::PwmPort& port = host::Board::pwm(this->handle, this->channel); + port.touched = true; + port.duty_cycle = this->inverted ? 100.0F - active : active; +} + +void Pwm::set_frequency(uint32_t frequency) { + const uint32_t prescaler = this->handle->Instance->PSC; + + this->handle->Instance->ARR = this->handle->Instance->kernel_clock / ((prescaler + 1) * frequency) - 1; + this->handle->Instance->CNT = 0; + + host::PwmPort& port = host::Board::pwm(this->handle, this->channel); + port.touched = true; + port.frequency = this->get_frequency(); +} + +float Pwm::get_frequency() const { + const uint32_t autoreload = this->handle->Instance->ARR; + const uint32_t period = is_center_aligned(this->handle) ? 2 * autoreload : autoreload + 1; + const auto ticks = static_cast(this->handle->Instance->PSC + 1) * static_cast(period); + + return static_cast(this->handle->Instance->kernel_clock) / ticks; +} + +bool Pwm::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/pwm_dma.cpp b/micras_hal/host/src/pwm_dma.cpp new file mode 100644 index 0000000..c69462c --- /dev/null +++ b/micras_hal/host/src/pwm_dma.cpp @@ -0,0 +1,86 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/pwm_dma.hpp" + +namespace micras::hal { +namespace { +/** + * @brief Start a transfer of compare values. + * + * @param handle Timer handle. + * @param channel Timer channel. + * @param compares Values the DMA feeds the compare register with. + */ +template +void start(TIM_HandleTypeDef* handle, uint32_t channel, std::span compares) { + host::PwmDmaPort& port = host::Board::pwm_dma(handle, channel); + port.touched = true; + port.compares.assign(compares.begin(), compares.end()); + port.period = handle->Instance->ARR + 1; + port.busy = true; + port.transfers++; + + const uint64_t cycles_per_count = static_cast(handle->Instance->PSC + 1) * + host::Clock::instance().cycles_per_microsecond() * 1000000ULL / + handle->Instance->kernel_clock; + + port.transfer_end = host::Clock::instance().now() + compares.size() * port.period * cycles_per_count; +} +} // namespace + +PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.timer_channel} { + if (this->handle->State == HAL_TIM_STATE_RESET) { + config.init_function(); + } + + this->initialized = this->handle->State == HAL_TIM_STATE_READY; +} + +void PwmDma::start_dma(std::span buffer) { + if (this->is_busy()) { + return; + } + + start(this->handle, this->channel, buffer); +} + +void PwmDma::start_dma(std::span buffer) { + if (this->is_busy()) { + return; + } + + start(this->handle, this->channel, buffer); +} + +void PwmDma::stop_dma() { + host::Board::pwm_dma(this->handle, this->channel).busy = false; +} + +uint32_t PwmDma::get_compare(float duty_cycle) const { + const float scaled = duty_cycle * static_cast(this->handle->Instance->ARR + 1) * 0.01F; + + // NOLINTNEXTLINE(bugprone-incorrect-roundings): the rounding the firmware's own driver does. + return static_cast(scaled + 0.5F); +} + +bool PwmDma::is_busy() { + host::PwmDmaPort& port = host::Board::pwm_dma(this->handle, this->channel); + + if (port.busy and host::Clock::instance().now() >= port.transfer_end) { + port.busy = false; + } + + return port.busy; +} + +bool PwmDma::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/spi.cpp b/micras_hal/host/src/spi.cpp new file mode 100644 index 0000000..fc7f1f0 --- /dev/null +++ b/micras_hal/host/src/spi.cpp @@ -0,0 +1,74 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/hal/spi.hpp" + +namespace micras::hal { +std::array Spi::transferring{}; + +Spi::Spi(const Config& config) : + handle{config.handle}, + cs_gpio{config.cs_gpio}, + timeout{config.timeout}, + clock_polarity{config.clock_polarity}, + clock_phase{config.clock_phase} { + this->unselect_device(); + + if (this->handle->State == HAL_SPI_STATE_RESET) { + config.init_function(); + } + + this->initialized = this->handle->State == HAL_SPI_STATE_READY; +} + +Spi::~Spi() { + std::ranges::replace(transferring, this, static_cast(nullptr)); +} + +bool Spi::select_device() { + this->handle->Init.CLKPolarity = this->clock_polarity; + this->handle->Init.CLKPhase = this->clock_phase; + this->cs_gpio.write(false); + return true; +} + +void Spi::unselect_device() { + this->cs_gpio.write(true); +} + +// NOLINTBEGIN(readability-convert-member-functions-to-static): hal::Spi's interface, declared by the firmware. +bool Spi::transmit(std::span /*data*/) { + return false; +} + +bool Spi::receive(std::span /*data*/) { + return false; +} + +bool Spi::transmit_receive(std::span /*transmitted*/, std::span /*received*/) { + return false; +} + +// NOLINTEND(readability-convert-member-functions-to-static) + +bool Spi::start_transfer(std::span /*transmitted*/, std::span /*received*/) { + this->transfer = Transfer::FAILED; + return false; +} + +Spi::Transfer Spi::get_transfer() const { + return this->transfer; +} + +void Spi::on_transfer_end(const SPI_HandleTypeDef* /*handle*/, bool /*succeeded*/) { } + +bool Spi::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/timer.cpp b/micras_hal/host/src/timer.cpp new file mode 100644 index 0000000..27187a9 --- /dev/null +++ b/micras_hal/host/src/timer.cpp @@ -0,0 +1,34 @@ +/** + * @file + */ + +#include + +#include + +#include "micras/hal/host/clock.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::hal { +uint32_t Timer::cycles_per_microsecond{1}; + +void Timer::init() { + cycles_per_microsecond = SystemCoreClock / 1000000; +} + +uint32_t Timer::get_counter() { + return host::Clock::instance().read_cycles(); +} + +uint32_t Timer::get_counter_ms() { + return host::Clock::instance().read_ms(); +} + +uint32_t Timer::to_microseconds(uint32_t cycles) { + return cycles / cycles_per_microsecond; +} + +uint32_t Timer::to_cycles(uint32_t microseconds) { + return microseconds * cycles_per_microsecond; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/uart_dma.cpp b/micras_hal/host/src/uart_dma.cpp new file mode 100644 index 0000000..f8af1e5 --- /dev/null +++ b/micras_hal/host/src/uart_dma.cpp @@ -0,0 +1,82 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/hal/host/board.hpp" +#include "micras/hal/uart_dma.hpp" + +namespace micras::hal { +UartDma::UartDma(const Config& config) : handle{config.handle} { + if (this->handle->gState == HAL_UART_STATE_RESET) { + config.init_function(); + } + + host::Board::uart(this->handle).baud_rate = this->handle->Init.BaudRate; +} + +bool UartDma::start_rx(std::span buffer) { + host::UartPort& port = host::Board::uart(this->handle); + port.touched = true; + port.rx_buffer = buffer; + port.rx_head = 0; + + this->rx_buffer = buffer; + this->rx_tail = 0; + this->handle->RxState = HAL_UART_STATE_BUSY_RX; + this->initialized = not buffer.empty(); + + return this->initialized; +} + +std::size_t UartDma::rx_head() const { + return host::Board::uart(this->handle).rx_head; +} + +std::size_t UartDma::read(std::span into) { + if (this->rx_buffer.empty()) { + return 0; + } + + const std::size_t size = this->rx_buffer.size(); + const std::size_t available = (this->rx_head() + size - this->rx_tail) % size; + const std::size_t taken = std::min(available, into.size()); + const std::size_t first = std::min(taken, size - this->rx_tail); + + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access): bounded just above. + std::memcpy(into.data(), &this->rx_buffer[this->rx_tail], first); + + if (first < taken) { + std::memcpy(&into[first], this->rx_buffer.data(), taken - first); + } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + + this->rx_tail = (this->rx_tail + taken) % size; + + return taken; +} + +bool UartDma::start_tx(std::span from) { + if (from.empty() or this->is_transmitting()) { + return false; + } + + host::UartPort& port = host::Board::uart(this->handle); + port.touched = true; + port.tx.insert(port.tx.end(), from.begin(), from.end()); + return true; +} + +bool UartDma::is_transmitting() const { + return host::Board::uart(this->handle).transmitting(); +} + +bool UartDma::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal From 150d960b1ea92cb4791fdd01dc51b393dad390dd Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Sep 2026 20:36:03 +0000 Subject: [PATCH 508/561] =?UTF-8?q?=F0=9F=90=9B=20Address=20the=20review?= =?UTF-8?q?=20of=20the=20low=20level=20layer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The initialization now gates on every proxy the robot drives with, rather than on the inertial measurement unit alone, and on the last reset not having come from the watchdog, so that a fault that keeps resetting the board stops in the error state instead of looping through boots. SPI3 keeps driving its pins while it is disabled, which the HAL does after every transfer. Without it SCK floated between transfers, still high after one of the inertial measurement unit in mode 3, and fell only after the chip select of an encoder, inside the frame where the AS5047U samples. The encoders also get their error register read before being configured, since that read is what clears it, and 1 us of chip select high between frames where the datasheet asks for 350 ns. The inertial measurement unit selects its high accuracy data rate set while both sensors are powered down, which the ST driver's own data rate setters do not, and checks every return value and reads both data rates back. A hard fault holds every actuator off while it waits for the watchdog, since the reset that follows leaves the pins floating rather than off. The watchdog is frozen while a debugger halts the core. The route search at the end of the exploration runs under the wider timeout the flash erase uses, and the robot is a static object instead of 4.6 KB of the stack. The 550 MHz clock tree needs the CPU_FREQ_BOOST option byte, which comes cleared from the factory and is programmed once per chip on the bench. The initialization fails on a chip without it instead of running it out of its datasheet. The supply source is pinned to direct SMPS instead of left to the default. The filter cutoffs are 4 Hz and 8 Hz, close to the 3.98 Hz and 7.95 Hz the firmware ran at its real 500 Hz loop, and are checked against half of their sampling rate at compile time. The tests run at the loop period the filters are designed for. The snapshot copy of the converters and the response of the inertial measurement unit are fenced against compiler reordering, the torque buffer starts at zero, and math::sqrt scales the value to [1/4, 4) before iterating, which keeps it within an ulp of the root over the whole range. --- micras_core/include/micras/core/math.hpp | 46 ++++++++++++++++--- micras_hal/include/micras/hal/adc_dma.hpp | 7 ++- micras_hal/include/micras/hal/mcu.hpp | 43 +++++++++++++++++ micras_hal/stm32/src/adc_dma.cpp | 8 +++- micras_hal/stm32/src/mcu.cpp | 40 ++++++++++++++++ micras_proxy/include/micras/proxy/imu.hpp | 13 +++++- .../include/micras/proxy/rotary_sensor.hpp | 13 ++++++ .../micras/proxy/rotary_sensor_reg.hpp | 1 + .../include/micras/proxy/torque_sensors.hpp | 2 +- micras_proxy/src/imu.cpp | 40 ++++++++++------ micras_proxy/src/rotary_sensor.cpp | 4 ++ 11 files changed, 191 insertions(+), 26 deletions(-) diff --git a/micras_core/include/micras/core/math.hpp b/micras_core/include/micras/core/math.hpp index 0310d53..652e406 100644 --- a/micras_core/include/micras/core/math.hpp +++ b/micras_core/include/micras/core/math.hpp @@ -7,6 +7,7 @@ #include #include +#include #include /** @@ -34,9 +35,16 @@ constexpr T abs(T value) { /** * @brief Square root by Newton iterations. * + * @details Newton's method only converges quickly once the estimate is close to the root, so the + * value is first brought to [1/4, 4) by powers of four, whose square roots are exact powers of two + * that scale the result back. From an estimate above the root, which the mean of the value and one + * always is, every iteration lowers the estimate until rounding stops it, which is where the + * iterations end: within an ulp of the root, in about six iterations for a double. + * * @tparam T Floating point type. * @param value Non negative value. - * @return The square root of the value, or zero for a non positive value. + * @return The square root of the value, zero for a non positive value, and the value itself for an + * infinity or a NaN. */ template constexpr T sqrt(T value) { @@ -44,19 +52,45 @@ constexpr T sqrt(T value) { return T{0}; } - T estimate = value > T{1} ? value : T{1}; + if (not(value <= std::numeric_limits::max())) { + return value; + } + + T scale{1}; + + while (value >= T{0x1p64}) { + value *= T{0x1p-64}; + scale *= T{0x1p32}; + } + + while (value < T{0x1p-64}) { + value *= T{0x1p64}; + scale *= T{0x1p-32}; + } + + while (value >= T{4}) { + value /= T{4}; + scale *= T{2}; + } + + while (value < T{0.25}) { + value *= T{4}; + scale /= T{2}; + } + + T estimate = (value + T{1}) / T{2}; - for (uint8_t i = 0; i < 64; i++) { + for (uint8_t i = 0; i < 16; i++) { const T next = (estimate + value / estimate) / T{2}; - if (abs(next - estimate) <= estimate * T{1e-16}) { - return next; + if (next >= estimate) { + break; } estimate = next; } - return estimate; + return estimate * scale; } /** diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 5221f8f..44942d3 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -85,9 +85,12 @@ class AdcDma { * @brief Read the last complete sequence. * * @note The snapshot may be replaced while it is being read, which is detected with the - * sequence counter and answered by reading it again. + * sequence counter and answered by reading it again. The counter is volatile but the snapshot + * is not, so the copy is fenced on both sides to keep the compiler from moving it past either + * read of the counter. * - * @param destination Buffer of the size of the snapshot that receives it. + * @param destination Buffer of the size of the snapshot that receives it, of which a smaller + * one receives only what fits. * @return The number of sequences completed so far, which tells whether this one is new. */ uint32_t read_snapshot(std::span destination) const; diff --git a/micras_hal/include/micras/hal/mcu.hpp b/micras_hal/include/micras/hal/mcu.hpp index 9d0e341..a48b33b 100644 --- a/micras_hal/include/micras/hal/mcu.hpp +++ b/micras_hal/include/micras/hal/mcu.hpp @@ -29,11 +29,18 @@ class Mcu { * to no symbol the application generates. The two clock functions are separate fields because * the order matters: the clock tree has to be running before the timebase is resolved from the * core clock frequency, and before any peripheral is initialized. + * + * @note The core clock of some parts is only within their datasheet above a threshold, 520 MHz + * on the STM32H72x and STM32H73x, when an option byte says so, and that option byte comes + * cleared from the factory. A board whose clock tree needs it sets cpu_frequency_boost, the + * option byte is programmed once per chip with STM32CubeProgrammer, and a chip without it is + * reported by is_cpu_frequency_supported. */ struct Config { InitFunction clock_init; InitFunction peripheral_clock_init; std::span peripheral_inits; + bool cpu_frequency_boost; }; /** @@ -48,6 +55,10 @@ class Mcu { * initialization function of its own configuration, so peripheral_inits carries only what is * left over: the pin configuration, the DMA controllers and anything else without an owner. * + * @note The watchdog is frozen while the core is halted by a debugger, since otherwise any + * breakpoint would reset the microcontroller as soon as the timeout ran out. Without a debugger + * the setting has no effect. + * * @param config Initialization functions of the board, which may leave peripheral_clock_init * null on a part where every peripheral runs from a bus clock. */ @@ -81,6 +92,38 @@ class Mcu { * @brief Refresh the independent watchdog. */ static void refresh_watchdog(); + + /** + * @brief Check whether the last reset was caused by the independent watchdog. + * + * @note The reset flags are read and cleared by init, so this describes the last reset only, + * and a watchdog reset followed by a reset from the button reads as a normal start. + * + * @return True if the watchdog reset the microcontroller, false otherwise. + */ + static bool was_reset_by_watchdog(); + + /** + * @brief Check whether the core runs within its datasheet. + * + * @note The option byte is only read, never programmed, so that nothing but a deliberate step on + * the bench changes it. + * + * @return False if the clock tree of the board needs the CPU frequency boost and the option byte + * that allows it is not set, true otherwise. + */ + static bool is_cpu_frequency_supported(); + +private: + /** + * @brief Whether the last reset was caused by the independent watchdog. + */ + static bool watchdog_reset; + + /** + * @brief Whether the core runs within its datasheet. + */ + static bool cpu_frequency_supported; }; } // namespace micras::hal diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 77d0e51..25e0cdc 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -81,10 +82,13 @@ bool AdcDma::start_dma(std::span buffer, std::span snapshot) } uint32_t AdcDma::read_snapshot(std::span destination) const { - uint32_t before = this->sequence; + const std::span source = this->snapshot.first(std::min(this->snapshot.size(), destination.size())); + uint32_t before = this->sequence; while (true) { - std::ranges::copy(this->snapshot, destination.begin()); + std::atomic_signal_fence(std::memory_order_seq_cst); + std::ranges::copy(source, destination.begin()); + std::atomic_signal_fence(std::memory_order_seq_cst); const uint32_t after = this->sequence; diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp index 8901728..d9887b1 100644 --- a/micras_hal/stm32/src/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -60,11 +60,43 @@ static constexpr uint32_t watchdog_timeout_us{1000}; */ static constexpr uint32_t inactive_compare{0xFFFFFFFF}; +bool Mcu::watchdog_reset{}; +bool Mcu::cpu_frequency_supported{}; + +/** + * @brief Check whether the option byte that lets the core run above its default maximum frequency is + * set. + * + * @return True if the option byte is set, false otherwise or on a part without it. + */ +static bool cpu_frequency_boosted() { +#ifdef FLASH_OPTSR2_CPUFREQ_BOOST + return (FLASH->OPTSR2_CUR & FLASH_OPTSR2_CPUFREQ_BOOST) != 0; +#else + return false; +#endif +} + void Mcu::init(const Config& config) { +#ifdef IWDG1 + watchdog_reset = __HAL_RCC_GET_FLAG(RCC_FLAG_IWDG1RST) != 0; +#else + watchdog_reset = __HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) != 0; +#endif + __HAL_RCC_CLEAR_RESET_FLAGS(); + SCB_EnableICache(); HAL_Init(); +#ifdef IWDG1 + __HAL_DBGMCU_FREEZE_IWDG1(); +#else + __HAL_DBGMCU_FREEZE_IWDG(); +#endif + + cpu_frequency_supported = not config.cpu_frequency_boost or cpu_frequency_boosted(); + config.clock_init(); if (config.peripheral_clock_init != nullptr) { @@ -120,4 +152,12 @@ void Mcu::set_watchdog_timeout(uint32_t timeout_ms) { void Mcu::refresh_watchdog() { watchdog_instance()->KR = watchdog_key_reload; } + +bool Mcu::was_reset_by_watchdog() { + return watchdog_reset; +} + +bool Mcu::is_cpu_frequency_supported() { + return cpu_frequency_supported; +} } // namespace micras::hal diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index a193929..85cf376 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -53,7 +53,12 @@ class Imu { /** * @brief Construct a new Imu object. * - * @param config Configuration for the IMU. + * @note The high accuracy data rate set is chosen while both sensors are still powered down, + * since the datasheet only allows high accuracy mode to change in power down, and the data rates + * are read back afterwards: a sensor that kept the default set runs at 7680 Hz instead of + * 8000 Hz, and fails the initialization instead of passing silently. + * + * @param config Configuration for the IMU, whose two data rates have to share one set. */ explicit Imu(const Config& config); @@ -158,6 +163,12 @@ class Imu { static constexpr uint8_t acceleration_offset{10}; ///@} + /** + * @brief Bits of the high accuracy data rate configuration register that select the set, which + * the data rate identifiers of the driver carry in their high nibble. + */ + static constexpr uint8_t haodr_sel_mask{0x03}; + /** * @brief Bits of the status register that tell which sensor has a new sample. */ diff --git a/micras_proxy/include/micras/proxy/rotary_sensor.hpp b/micras_proxy/include/micras/proxy/rotary_sensor.hpp index add63ab..97a6813 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor.hpp @@ -71,6 +71,11 @@ class RotarySensor { /** * @brief Construct a new RotarySensor object. * + * @note The error register is read before anything is written, since reading it is what clears + * it, and the sensor flags an error in every answer while it holds one. An error left over from + * the power up, or from a frame garbled while the bus was being set up, would otherwise fail the + * read back of the resolution. + * * @param config Configuration for the rotary sensor. */ explicit RotarySensor(const Config& config); @@ -123,6 +128,11 @@ class RotarySensor { */ static constexpr uint8_t frame_size{3}; + /** + * @brief Time the chip select is kept high after a frame, in microseconds. + */ + static constexpr uint32_t min_deselect_time_us{1}; + /** * @brief Number of edges the quadrature decoder counts per pulse of a single channel. */ @@ -147,6 +157,9 @@ class RotarySensor { /** * @brief Exchange one frame with the sensor. * + * @note The chip select is kept high for a while after the frame, since the sensor needs 350 ns + * between two frames and the next one would otherwise start within about 200 ns. + * * @param frame The frame to send. * @return The frame received while sending, or nothing if the transfer failed. */ diff --git a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp index 7819dcd..a30078d 100644 --- a/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp +++ b/micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp @@ -102,6 +102,7 @@ struct Registers { * @brief Register addresses in the rotary sensor memory. */ ///@{ + static constexpr uint16_t errfl_addr{0x0001}; static constexpr uint16_t disable_addr{0x0015}; static constexpr uint16_t zposm_addr{0x0016}; static constexpr uint16_t zposl_addr{0x0017}; diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 30232a8..48a5f6d 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -105,7 +105,7 @@ class TTorqueSensors { /** * @brief Buffer to store the ADC values. */ - std::array buffer; + std::array buffer{}; /** * @brief Reading of each sensor when no current is flowing. diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index ad20f6c..5aae772 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -3,6 +3,7 @@ */ #include +#include #include #include @@ -30,25 +31,35 @@ Imu::Imu(const Config& config) : return; } - lsm6dsv_sw_por(&(this->dev_ctx)); - lsm6dsv_block_data_update_set(&(this->dev_ctx), PROPERTY_ENABLE); + uint8_t haodr_cfg = (static_cast(config.gyroscope_data_rate) >> 4) & haodr_sel_mask; - lsm6dsv_gy_mode_set(&(this->dev_ctx), config.gyroscope_mode); - lsm6dsv_xl_mode_set(&(this->dev_ctx), config.accelerometer_mode); + int32_t status = lsm6dsv_sw_por(&(this->dev_ctx)); + status |= lsm6dsv_block_data_update_set(&(this->dev_ctx), PROPERTY_ENABLE); + status |= lsm6dsv_write_reg(&(this->dev_ctx), LSM6DSV_HAODR_CFG, &haodr_cfg, 1); - lsm6dsv_gy_data_rate_set(&(this->dev_ctx), config.gyroscope_data_rate); - lsm6dsv_xl_data_rate_set(&(this->dev_ctx), config.accelerometer_data_rate); + status |= lsm6dsv_gy_mode_set(&(this->dev_ctx), config.gyroscope_mode); + status |= lsm6dsv_xl_mode_set(&(this->dev_ctx), config.accelerometer_mode); - lsm6dsv_gy_full_scale_set(&(this->dev_ctx), config.gyroscope_scale); - lsm6dsv_xl_full_scale_set(&(this->dev_ctx), config.accelerometer_scale); + status |= lsm6dsv_gy_data_rate_set(&(this->dev_ctx), config.gyroscope_data_rate); + status |= lsm6dsv_xl_data_rate_set(&(this->dev_ctx), config.accelerometer_data_rate); - lsm6dsv_filt_settling_mask_set(&dev_ctx, {.drdy = 1, .ois_drdy = 0, .irq_xl = 0, .irq_g = 0}); + status |= lsm6dsv_gy_full_scale_set(&(this->dev_ctx), config.gyroscope_scale); + status |= lsm6dsv_xl_full_scale_set(&(this->dev_ctx), config.accelerometer_scale); - lsm6dsv_filt_gy_lp1_set(&(this->dev_ctx), PROPERTY_ENABLE); - lsm6dsv_filt_gy_lp1_bandwidth_set(&(this->dev_ctx), config.gyroscope_filter); - lsm6dsv_filt_xl_lp2_set(&(this->dev_ctx), PROPERTY_ENABLE); - lsm6dsv_filt_xl_lp2_bandwidth_set(&(this->dev_ctx), config.accelerometer_filter); - this->initialized = true; + status |= lsm6dsv_filt_settling_mask_set(&dev_ctx, {.drdy = 1, .ois_drdy = 0, .irq_xl = 0, .irq_g = 0}); + + status |= lsm6dsv_filt_gy_lp1_set(&(this->dev_ctx), PROPERTY_ENABLE); + status |= lsm6dsv_filt_gy_lp1_bandwidth_set(&(this->dev_ctx), config.gyroscope_filter); + status |= lsm6dsv_filt_xl_lp2_set(&(this->dev_ctx), PROPERTY_ENABLE); + status |= lsm6dsv_filt_xl_lp2_bandwidth_set(&(this->dev_ctx), config.accelerometer_filter); + + lsm6dsv_data_rate_t gyroscope_data_rate{}; + lsm6dsv_data_rate_t accelerometer_data_rate{}; + status |= lsm6dsv_gy_data_rate_get(&(this->dev_ctx), &gyroscope_data_rate); + status |= lsm6dsv_xl_data_rate_get(&(this->dev_ctx), &accelerometer_data_rate); + + this->initialized = status == 0 and gyroscope_data_rate == config.gyroscope_data_rate and + accelerometer_data_rate == config.accelerometer_data_rate; } bool Imu::check_whoami() { @@ -68,6 +79,7 @@ void Imu::update() { } if (transfer == hal::Spi::Transfer::COMPLETE) { + std::atomic_signal_fence(std::memory_order_seq_cst); this->read_response(); } diff --git a/micras_proxy/src/rotary_sensor.cpp b/micras_proxy/src/rotary_sensor.cpp index c8b277e..f391e21 100644 --- a/micras_proxy/src/rotary_sensor.cpp +++ b/micras_proxy/src/rotary_sensor.cpp @@ -8,6 +8,7 @@ #include #include "micras/proxy/rotary_sensor.hpp" +#include "micras/proxy/stopwatch.hpp" namespace micras::proxy { RotarySensor::RotarySensor(const Config& config) : spi{config.spi}, encoder{config.encoder}, crc{config.crc} { @@ -15,6 +16,8 @@ RotarySensor::RotarySensor(const Config& config) : spi{config.spi}, encoder{conf return; } + this->read_register(Registers::errfl_addr); + this->write_register(Registers::disable_addr, config.registers.disable.raw); this->write_register(Registers::zposm_addr, config.registers.zposm.raw); this->write_register(Registers::zposl_addr, config.registers.zposl.raw); @@ -71,6 +74,7 @@ std::optional RotarySensor::exchange_frame(const std::arrayspi.transmit_receive(frame, received); this->spi.unselect_device(); + proxy::Stopwatch::sleep_us(min_deselect_time_us); if (not transferred) { return std::nullopt; From 3c16178b2f2c0f26ab712804809e55eb035171fa Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Sep 2026 22:11:01 -0300 Subject: [PATCH 509/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Design=20the=20tur?= =?UTF-8?q?n=20tables=20once,=20and=20take=20out=20what=20the=20navigation?= =?UTF-8?q?=20never=20uses?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The turn tables and the dynamics built on them are computed in src/dynamics_config.cpp alone, instead of in every source that includes the constants. That source still takes the compiler about 0.9 GB and 7 s; every other one now takes 0.2 GB and under a second, where each took 0.9 GB and 7 s. On the Cortex-M7 the configuration is 2.6 KB of constant data with no initializer. The turn margins move to turn_margins.hpp, so the turn designer reads them without compiling the designs it is there to replace, and a design that no longer fits can be designed again. The racing line measures, resamples, and measures and averages the curvature in one pass each instead of four budgeted phases, since each costs little per sample; the line and its speeds come out the same, and a fast run is planned 0.3 to 1 s sooner. The diagonal segment kind, which everything drove as a straight, the heading of a turn shape nobody read, the always-on use of edges and the smoothing that was always one sample are gone. The grip left on a curve is computed in one place, and the speed of a turn from the curve limits. The comments that told how the code got here, or quoted results of past runs, now say what it does. --- micras_nav/include/micras/nav/controller.hpp | 10 +- micras_nav/include/micras/nav/curve_speed.hpp | 2 +- .../micras/nav/drive_identification.hpp | 4 +- micras_nav/include/micras/nav/explorer.hpp | 6 +- .../include/micras/nav/impl/localizer.tpp | 2 +- .../include/micras/nav/impl/mission.tpp | 4 +- .../include/micras/nav/impl/racing_line.tpp | 160 +++++++----------- micras_nav/include/micras/nav/localizer.hpp | 13 +- micras_nav/include/micras/nav/mission.hpp | 5 - .../include/micras/nav/motion_limits.hpp | 24 ++- micras_nav/include/micras/nav/planner.hpp | 10 +- micras_nav/include/micras/nav/racing_line.hpp | 93 +++------- micras_nav/include/micras/nav/segment.hpp | 13 +- .../include/micras/nav/speed_profile.hpp | 11 +- micras_nav/include/micras/nav/turn_table.hpp | 6 +- micras_nav/src/drive_identification.cpp | 6 +- micras_nav/src/executor.cpp | 4 +- micras_nav/src/motion_limits.cpp | 22 ++- micras_nav/src/route_compiler.cpp | 9 +- micras_nav/src/velocity_planner.cpp | 11 +- 20 files changed, 165 insertions(+), 250 deletions(-) diff --git a/micras_nav/include/micras/nav/controller.hpp b/micras_nav/include/micras/nav/controller.hpp index 367772f..32ee2d2 100644 --- a/micras_nav/include/micras/nav/controller.hpp +++ b/micras_nav/include/micras/nav/controller.hpp @@ -30,8 +30,7 @@ namespace micras::nav { * In a curve the tires slide to the outside at a speed the lateral compliance of the model gives, * so the robot has to point into the curve by that speed over its own, which is the compliance * times the angular speed of the reference, to move along the path. That angle is added to the - * orientation the feedback aims at, and its rate to the angular speed, instead of being left for - * an error across the path to build up. + * orientation the feedback aims at, and its rate to the angular speed. * * @note The gains are not tuned: they follow from the model of the drive train and from the natural * frequency and damping asked of each axis. @@ -112,10 +111,9 @@ class Controller { * @note The feed forward alone may ask for more voltage than there is. Slowing the clock of * the reference scales its linear and angular speeds together, so the robot stays on the same * path and only takes longer, instead of cutting the turn it is in. With the limits of a run - * derived from the same model this should stay at one. The reference this is computed from was - * already played at the scale in force, so the scale is corrected from that one rather than - * computed afresh, and settles where the demand meets the voltage available instead of - * alternating between one and a fraction every iteration. + * derived from the same model this should stay at one. The reference was already played at the + * scale in force, so the scale is updated from that one and settles where the demand meets the + * voltage available. * * @return The factor to apply to the elapsed time of the reference, in (0, 1]. */ diff --git a/micras_nav/include/micras/nav/curve_speed.hpp b/micras_nav/include/micras/nav/curve_speed.hpp index 2939272..522a9f3 100644 --- a/micras_nav/include/micras/nav/curve_speed.hpp +++ b/micras_nav/include/micras/nav/curve_speed.hpp @@ -22,7 +22,7 @@ namespace micras::nav { * before, with the grip the curve leaves there, and a pass from the end lowers it to what can be * braked from before the one after. Between two samples the acceleration is constant. A turn is * therefore braked into as long as its curvature leaves grip to spare and accelerated out of as - * soon as it does, instead of being driven at the speed of its tightest point. + * soon as it does. * * The passes are written over any curve, given as a function of the index of a sample that returns * the curvature at that sample and the sharpness between it and the next one, and over storage that diff --git a/micras_nav/include/micras/nav/drive_identification.hpp b/micras_nav/include/micras/nav/drive_identification.hpp index 0d5a44a..a413192 100644 --- a/micras_nav/include/micras/nav/drive_identification.hpp +++ b/micras_nav/include/micras/nav/drive_identification.hpp @@ -174,10 +174,8 @@ class DriveIdentification { /** * @brief Move on to the next phase, after a rest. - * - * @param measurements The current measurements. */ - void advance(const Measurements& measurements); + void advance(); /** * @brief Parameters of the procedure, with the physical description of the robot. diff --git a/micras_nav/include/micras/nav/explorer.hpp b/micras_nav/include/micras/nav/explorer.hpp index 77ed5c0..acbcc60 100644 --- a/micras_nav/include/micras/nav/explorer.hpp +++ b/micras_nav/include/micras/nav/explorer.hpp @@ -22,13 +22,13 @@ namespace micras::nav { * never seen is taken as absent. If that route crosses no unknown wall, the map already proves that * nothing faster exists and the search is over. Otherwise the cells next to the unknown walls it * crosses are what is worth visiting, and the question is asked again once the map has changed. The - * test uses the very cost the fast run is chosen by, so a faster route cannot stay hidden, and only - * cells on some best conceivable route are ever targeted. + * test uses the cost of the planner, whose candidates the fast run is chosen among, so a faster + * route cannot stay hidden, and only cells on some best conceivable route are ever targeted. * * The fastest route depends on the profile of the run, so the test is repeated for every profile the * map has to be good for and the targets are put together. * - * @note The planner is advanced a bounded number of nodes per call, which lets all of this happen + * @note The planner is advanced a bounded number of edges per call, which lets all of this happen * while the robot is moving. * * @tparam width The width of the maze in cells. diff --git a/micras_nav/include/micras/nav/impl/localizer.tpp b/micras_nav/include/micras/nav/impl/localizer.tpp index 662dee0..e75d78a 100644 --- a/micras_nav/include/micras/nav/impl/localizer.tpp +++ b/micras_nav/include/micras/nav/impl/localizer.tpp @@ -44,7 +44,7 @@ void Localizer::correct( const RayHit hit = wall_model.cast(sampled, sensor, maze); - if (this->config.use_edges and wall_model.is_side_looking(sensor)) { + if (wall_model.is_side_looking(sensor)) { this->track_edge(sensor, reading, sampled, hit, wall_model, maze); } diff --git a/micras_nav/include/micras/nav/impl/mission.tpp b/micras_nav/include/micras/nav/impl/mission.tpp index 53c683a..dd33a01 100644 --- a/micras_nav/include/micras/nav/impl/mission.tpp +++ b/micras_nav/include/micras/nav/impl/mission.tpp @@ -438,13 +438,13 @@ void TMission::decide_at_entry() { ); turn.turn = TurnId::SS90S; - if (shape.pre > min_straight) { + if (shape.pre > RouteCompiler::min_straight) { move.add(make_segment(SegmentKind::STRAIGHT, shape.pre, entry)); } move.add(turn); - if (shape.post > min_straight) { + if (shape.post > RouteCompiler::min_straight) { move.add(make_segment( SegmentKind::STRAIGHT, shape.post, this->get_entry_pose(*next).compose({.position = {.x = -shape.post, .y = 0.0F}, .orientation = 0.0F}) diff --git a/micras_nav/include/micras/nav/impl/racing_line.tpp b/micras_nav/include/micras/nav/impl/racing_line.tpp index c7ef2b6..c0db9a8 100644 --- a/micras_nav/include/micras/nav/impl/racing_line.tpp +++ b/micras_nav/include/micras/nav/impl/racing_line.tpp @@ -132,26 +132,14 @@ bool TRacingLine::step() { this->solve_window(); return false; - case Phase::MEASURE: - this->measure(budget); - break; - case Phase::RESAMPLE: - this->resample(budget); - break; + this->resample(); + return false; case Phase::CHECK: this->check(budget); break; - case Phase::CURVATURE: - this->measure_curvature(budget); - break; - - case Phase::SMOOTH: - this->smooth_curvature(budget); - break; - case Phase::SPEED: this->plan_speeds(); return this->phase == Phase::IDLE; @@ -331,9 +319,7 @@ void TRacingLine::solve_window() { this->window_start += stride; if (this->window_start + fixed_samples >= this->line.size) { - this->cursor = 0; - this->measured_length = 0.0F; - this->phase = Phase::MEASURE; + this->phase = Phase::RESAMPLE; return; } @@ -341,78 +327,64 @@ void TRacingLine::solve_window() { } template -void TRacingLine::measure(uint32_t& budget) { - for (; budget > 0 and this->cursor + 1U < this->line.size; budget--, this->cursor++) { - this->measured_length += std::hypot( - this->line.xs.at(this->cursor + 1U) - this->line.xs.at(this->cursor), - this->line.ys.at(this->cursor + 1U) - this->line.ys.at(this->cursor) - ); - } +void TRacingLine::resample() { + float length = 0.0F; - if (this->cursor + 1U < this->line.size) { - return; + for (uint16_t i = 0; i + 1U < this->line.size; i++) { + length += + std::hypot(this->line.xs.at(i + 1U) - this->line.xs.at(i), this->line.ys.at(i + 1U) - this->line.ys.at(i)); } - const auto samples = static_cast(std::lround(this->measured_length / this->config.spacing)) + 1; + const auto samples = static_cast(std::lround(length / this->config.spacing)) + 1; if (samples > Line::max_samples or samples < 2 * fixed_samples + 2) { this->finish(false); return; } - this->resampled_size = static_cast(samples); - this->cursor = 0; - this->read_index = 0; - this->read_length = 0.0F; - this->phase = Phase::RESAMPLE; -} + const auto size = static_cast(samples); + const float spacing = length / static_cast(size - 1); -template -void TRacingLine::resample(uint32_t& budget) { - const float spacing = this->measured_length / static_cast(this->resampled_size - 1); + uint16_t read_index = 0; + float read_length = 0.0F; - for (; budget > 0 and this->cursor < this->resampled_size; budget--, this->cursor++) { - if (this->cursor + 1U == this->resampled_size) { - this->line.curvatures.at(this->cursor) = this->line.xs.at(this->line.size - 1); - this->line.speeds.at(this->cursor) = this->line.ys.at(this->line.size - 1); + for (uint16_t i = 0; i < size; i++) { + if (i + 1U == size) { + this->line.curvatures.at(i) = this->line.xs.at(this->line.size - 1); + this->line.speeds.at(i) = this->line.ys.at(this->line.size - 1); continue; } - const float distance = static_cast(this->cursor) * spacing; + const float distance = static_cast(i) * spacing; float step_length = 0.0F; while (true) { step_length = std::hypot( - this->line.xs.at(this->read_index + 1U) - this->line.xs.at(this->read_index), - this->line.ys.at(this->read_index + 1U) - this->line.ys.at(this->read_index) + this->line.xs.at(read_index + 1U) - this->line.xs.at(read_index), + this->line.ys.at(read_index + 1U) - this->line.ys.at(read_index) ); - if (this->read_length + step_length >= distance or this->read_index + 2U >= this->line.size) { + if (read_length + step_length >= distance or read_index + 2U >= this->line.size) { break; } - this->read_length += step_length; - this->read_index++; + read_length += step_length; + read_index++; } const float fraction = - step_length > 0.0F ? std::clamp((distance - this->read_length) / step_length, 0.0F, 1.0F) : 0.0F; - const uint16_t from = this->read_index; - - this->line.curvatures.at(this->cursor) = - this->line.xs.at(from) + fraction * (this->line.xs.at(from + 1U) - this->line.xs.at(from)); - this->line.speeds.at(this->cursor) = - this->line.ys.at(from) + fraction * (this->line.ys.at(from + 1U) - this->line.ys.at(from)); - } + step_length > 0.0F ? std::clamp((distance - read_length) / step_length, 0.0F, 1.0F) : 0.0F; - if (this->cursor < this->resampled_size) { - return; + this->line.curvatures.at(i) = this->line.xs.at(read_index) + + fraction * (this->line.xs.at(read_index + 1U) - this->line.xs.at(read_index)); + this->line.speeds.at(i) = this->line.ys.at(read_index) + + fraction * (this->line.ys.at(read_index + 1U) - this->line.ys.at(read_index)); } - std::copy_n(this->line.curvatures.begin(), this->resampled_size, this->line.xs.begin()); - std::copy_n(this->line.speeds.begin(), this->resampled_size, this->line.ys.begin()); - this->line.size = this->resampled_size; + std::copy_n(this->line.curvatures.begin(), size, this->line.xs.begin()); + std::copy_n(this->line.speeds.begin(), size, this->line.ys.begin()); + this->line.size = size; this->line.spacing = spacing; this->sweeps++; @@ -442,62 +414,60 @@ void TRacingLine::check(uint32_t& budget) { } if (this->cursor == end) { - this->cursor = 0; - this->phase = Phase::CURVATURE; + this->phase = Phase::SPEED; } } template -void TRacingLine::measure_curvature(uint32_t& budget) { +void TRacingLine::measure_curvature() { const uint16_t last = this->line.size - 1; - for (; budget > 0 and this->cursor <= last; budget--, this->cursor++) { - if (this->cursor == last) { - this->line.times.at(this->cursor) = 0.0F; - continue; - } - - const float heading = std::atan2( - this->line.ys.at(this->cursor + 1U) - this->line.ys.at(this->cursor), - this->line.xs.at(this->cursor + 1U) - this->line.xs.at(this->cursor) + const auto heading_after = [this](uint16_t index) { + return std::atan2( + this->line.ys.at(index + 1U) - this->line.ys.at(index), + this->line.xs.at(index + 1U) - this->line.xs.at(index) ); + }; - this->line.times.at(this->cursor) = - this->cursor == 0 ? 0.0F : core::math::wrap_angle(heading - this->previous_heading) / this->line.spacing; - this->previous_heading = heading; - } - - if (this->cursor > last) { - this->cursor = 0; - this->phase = Phase::SMOOTH; - } -} + float heading = heading_after(0); + float before = 0.0F; + float at = 0.0F; -template -void TRacingLine::smooth_curvature(uint32_t& budget) { - const int32_t last = this->line.size - 1; - const int32_t reach = this->config.smoothing; + for (uint16_t i = 0; i <= last; i++) { + float after = 0.0F; - for (; budget > 0 and this->cursor < this->line.size; budget--, this->cursor++) { - const int32_t from = std::max(this->cursor - reach, 0); - const int32_t to = std::min(this->cursor + reach, last); + if (i + 1U < last) { + const float next = heading_after(i + 1U); + after = core::math::wrap_angle(next - heading) / this->line.spacing; + heading = next; + } - float sum = 0.0F; + float sum = 0.0F; + uint8_t count = 0; - for (int32_t i = from; i <= to; i++) { - sum += this->line.times.at(i); + if (i > 0) { + sum += before; + count++; } - this->line.curvatures.at(this->cursor) = sum / static_cast(to - from + 1); - } + sum += at; + count++; - if (this->cursor == this->line.size) { - this->phase = Phase::SPEED; + if (i < last) { + sum += after; + count++; + } + + this->line.curvatures.at(i) = sum / static_cast(count); + before = at; + at = after; } } template void TRacingLine::plan_speeds() { + this->measure_curvature(); + const uint16_t size = this->line.size; const float spacing = this->line.spacing; diff --git a/micras_nav/include/micras/nav/localizer.hpp b/micras_nav/include/micras/nav/localizer.hpp index fc3c1b2..912efb7 100644 --- a/micras_nav/include/micras/nav/localizer.hpp +++ b/micras_nav/include/micras/nav/localizer.hpp @@ -64,12 +64,12 @@ class Localizer { * enough to land partly on the floor, the reading loses light the model of the sensor does not * know about, and the range comes out long by several percent. * - * The ends of the side walls are used as references along the path when use_edges is set. An - * end is accepted while the robot moves forward faster than the edge speed, since the only time - * it reverses is to park, from a pose it has just measured. The wall has to be closer than the - * edge range, so that nothing can be hiding its end, and the end has to be found within the edge - * window of where it was expected. An end is a single event rather than a stream of readings, so it has - * a cap of its own. The range tolerance, a constant part plus a part proportional to the range, + * The ends of the side walls are used as references along the path. An end is accepted while + * the robot moves forward faster than the edge speed, since the only time it reverses is to + * park, from a pose it has just measured. The wall has to be closer than the edge range, so + * that nothing can be hiding its end, and the end has to be found within the edge window of + * where it was expected. An end is a single event rather than a stream of readings, so it has a + * cap of its own. The range tolerance, a constant part plus a part proportional to the range, * tells a reading that is on a wall from one that is past its end. */ struct Config { @@ -88,7 +88,6 @@ class Localizer { float range_correlation; float max_range; float rest_window; - bool use_edges; float edge_deviation; float edge_window; float edge_speed; diff --git a/micras_nav/include/micras/nav/mission.hpp b/micras_nav/include/micras/nav/mission.hpp index 407b4df..17a39f3 100644 --- a/micras_nav/include/micras/nav/mission.hpp +++ b/micras_nav/include/micras/nav/mission.hpp @@ -232,11 +232,6 @@ class TMission { */ static constexpr uint8_t max_move_segments{6}; - /** - * @brief Shortest straight worth a segment of its own, in meters. - */ - static constexpr float min_straight{1.0e-4F}; - /** * @brief Distance within which a segment is taken to start where the watched cell is entered. */ diff --git a/micras_nav/include/micras/nav/motion_limits.hpp b/micras_nav/include/micras/nav/motion_limits.hpp index 9bf5969..af96378 100644 --- a/micras_nav/include/micras/nav/motion_limits.hpp +++ b/micras_nav/include/micras/nav/motion_limits.hpp @@ -13,8 +13,8 @@ namespace micras::nav { * @brief How a run trades time for safety. * * @note The utilization is the fraction of the available traction the run may ask for, in (0, 1]. - * Whether the run is risky selects the turn table built with the smaller safety margin. The racing - * line replaces the route by the smoothest line through + * The fan adds its downforce to the traction. Whether the run is risky selects the turn table built + * with the smaller safety margin. The racing line replaces the route by the smoothest line through * its cells, which only a fast run planned with the robot stopped can drive. The speed limit is what * makes the search run slow: a fast run leaves it above what the robot can reach. */ @@ -62,12 +62,11 @@ struct MotionLimits { * changing speed. * * @details The speed at a point is limited by the lateral acceleration its curvature asks for and - * by the angular acceleration its sharpness asks for, each against what the tires can give, which - * is the rule a turn has always been driven by. What is left of the grip there can change the - * speed: the lateral part takes its share as on a friction circle, and the angular part, which the - * two tires make by pushing in opposite directions, takes it from the push each tire has left. A - * change of speed on a curve also changes the angular speed, and that angular acceleration is - * counted too. + * by the angular acceleration its sharpness asks for, each against what the tires can give. What is + * left of the grip there can change the speed: the lateral part takes its share as on a friction + * circle, and the angular part, which the two tires make by pushing in opposite directions, takes + * it from the push each tire has left. A change of speed on a curve also changes the angular speed, + * and that angular acceleration is counted too. * * @note On a straight nothing is taken and the limits are those of the linear motion. */ @@ -98,6 +97,15 @@ struct CurveLimits { */ float get_deceleration(float speed, const Bending& bending) const; + /** + * @brief Get the share of the grip left for a change of speed at a point, at a speed. + * + * @param speed The speed there. + * @param bending How the path bends there. + * @return The share, which is zero or less where the grip is all used. + */ + float get_spare(float speed, const Bending& bending) const; + MotionLimits linear; float lateral; float angular; diff --git a/micras_nav/include/micras/nav/planner.hpp b/micras_nav/include/micras/nav/planner.hpp index 713af63..b7e8f24 100644 --- a/micras_nav/include/micras/nav/planner.hpp +++ b/micras_nav/include/micras/nav/planner.hpp @@ -59,7 +59,8 @@ struct Route { * actually executes, a run of steps ahead followed by a turn, and it exists only if every wall it * crosses passes the wall assumption. Its cost is the time of the run, from the evaluator the * executor plays back, plus the time of the turn at the speed the run profile gives it, so the - * fastest path of the graph is the fastest route the robot can drive. Diagonals, the turns of 135 + * fastest path of the graph is the fastest route the robot can drive, to within what the velocity + * planner refines among the candidates. Diagonals, the turns of 135 * degrees and the two sizes of the turn of 90 degrees are part of the search rather than a rewrite * of its result. * @@ -95,10 +96,9 @@ class TPlanner { /** * @brief Largest number of labels held at once. * - * @note On ten contest mazes, whole and with a random part of their walls unknown, the most a - * search held was 4519, with every turn and profile, and 4 of 5490 searches held more than 4500. - * A search that needs more drops the costliest labels waiting to be expanded, and reports that it - * is no longer exact. + * @note Sized with a margin over the largest search seen on contest mazes, whole and with part + * of their walls unknown. A search that needs more drops the costliest labels waiting to be + * expanded, and reports that it is no longer exact. */ static constexpr uint16_t max_labels{6144}; diff --git a/micras_nav/include/micras/nav/racing_line.hpp b/micras_nav/include/micras/nav/racing_line.hpp index b00afb0..f65e69d 100644 --- a/micras_nav/include/micras/nav/racing_line.hpp +++ b/micras_nav/include/micras/nav/racing_line.hpp @@ -36,12 +36,13 @@ namespace micras::nav { * windows overlap and sweep the line from its start to its end, and the line is resampled at equal * steps after each sweep. A slide is limited per sweep, and the bounds are found again for every * window, since the robot needs more room as the line turns it. After the last sweep every sample - * is checked, the curvature is measured and lightly smoothed, and the speeds are planned along it - * with the same rule as a turn. + * is checked, the curvature is measured and averaged over three samples, and the speeds are planned + * along it with the same rule as a turn. * - * Nothing here is bounded to one iteration of the loop as a whole: every phase advances by a budget - * of samples per call, except for the speeds, which take a few milliseconds in one call, the robot - * being stopped. + * Nothing here is bounded to one iteration of the loop as a whole. Sampling the route, finding the + * bounds and checking the line cost the most per sample, and advance by a budget of samples per + * call. Solving a window, resampling the line and planning the speeds take one call each, of a few + * milliseconds at most, the robot being stopped. * * @note The first and last samples never move: the line starts where the robot stands and ends * where the route comes to rest. @@ -56,18 +57,17 @@ class TRacingLine { * @brief Configuration struct for the racing line. * * @note The margin is the distance the line keeps between the outline of the robot and any - * obstacle wherever the route leaves room for it, with the risky switch too: at the margin of - * the risky turns the line hit a wall on 4 to 6 of 10 mazes. The least margins are what the - * final check requires everywhere, without and with the risky switch: the margins the turns were - * designed with, less what the resampling and the headings of the final line use up. Where the - * route itself comes closer than the margin, which the risky turns do, the line does not move. The trust is - * how far a sample may slide in one sweep, the scan step the resolution of the bounds, and the - * length weight the weight of the first differences, in units of the spacing. A sweep that - * slides no sample farther than the convergence tolerance ends the optimization. The smoothing - * is the number of samples on each side the curvature is averaged over. The lateral share is the - * part of the lateral grip of the run the curves of the line are planned with: the tires slide - * sideways in proportion to what they are asked for, which the pose estimate does not see, and - * the line holds its curves far longer than a turn does. + * obstacle wherever the route leaves room for it, with the risky switch too. The least margins + * are what the final check requires everywhere, without and with the risky switch: the margins + * the turns were designed with, less what the resampling and the headings of the final line use + * up. Where the route itself comes closer than the margin, which the risky turns do, the line + * does not move. The trust is how far a sample may slide in one sweep, the scan step the + * resolution of the bounds, and the length weight the weight of the first differences, in units + * of the spacing. A sweep that slides no sample farther than the convergence tolerance ends the + * optimization. The lateral share is the part of the lateral grip of the run the curves of the + * line are planned with: the tires slide sideways in proportion to what they are asked for, + * which the pose estimate only predicts, and the line holds its curves far longer than a turn + * does. */ // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) no defaults, so that a missing field is a warning struct Config { @@ -81,7 +81,6 @@ class TRacingLine { float convergence; float lateral_share; uint8_t max_sweeps; - uint8_t smoothing; uint16_t samples_per_step; }; @@ -192,12 +191,9 @@ class TRacingLine { SAMPLE = 1, BOUNDS = 2, SOLVE = 3, - MEASURE = 4, - RESAMPLE = 5, - CHECK = 6, - CURVATURE = 7, - SMOOTH = 8, - SPEED = 9, + RESAMPLE = 4, + CHECK = 5, + SPEED = 6, }; /** @@ -219,19 +215,13 @@ class TRacingLine { */ void solve_window(); - /** - * @brief Measure the length of the line for the resampling. - * - * @param budget The number of samples left in this call, which is decreased. - */ - void measure(uint32_t& budget); - /** * @brief Resample the line at equal steps, then start the next sweep or the checks. * - * @param budget The number of samples left in this call, which is decreased. + * @note The new samples are written to the curvatures and the speeds of the line, which are + * not in use until the speeds are planned, and copied back once they are all known. */ - void resample(uint32_t& budget); + void resample(); /** * @brief Check the clearance of the next samples of the line. @@ -241,21 +231,13 @@ class TRacingLine { void check(uint32_t& budget); /** - * @brief Measure the curvature at the next samples of the line. - * - * @param budget The number of samples left in this call, which is decreased. - */ - void measure_curvature(uint32_t& budget); - - /** - * @brief Smooth the curvature at the next samples of the line. - * - * @param budget The number of samples left in this call, which is decreased. + * @brief Measure the curvature of the line, from the turn of its heading between samples, + * averaged over each sample and its two neighbors. */ - void smooth_curvature(uint32_t& budget); + void measure_curvature(); /** - * @brief Plan the speeds along the line, time it and mark it ready. + * @brief Measure the curvature, plan the speeds along the line, time it and mark it ready. */ void plan_speeds(); @@ -407,22 +389,6 @@ class TRacingLine { float segment_start{}; ///@} - /** - * @brief Length of the line being resampled, and the distance along it at the sample read. - */ - ///@{ - float measured_length{}; - float read_length{}; - ///@} - - /** - * @brief Sample of the line being resampled that is read, and the number of samples written. - */ - ///@{ - uint16_t read_index{}; - uint16_t resampled_size{}; - ///@} - /** * @brief Number of sweeps done, and the farthest slide of the sweep in progress. */ @@ -431,11 +397,6 @@ class TRacingLine { float sweep_slide{}; ///@} - /** - * @brief Heading of the line at the sample before, for the curvature. - */ - float previous_heading{}; - /** * @brief First sample of the window and its number of samples. */ diff --git a/micras_nav/include/micras/nav/segment.hpp b/micras_nav/include/micras/nav/segment.hpp index 7278d6a..ffb4c8e 100644 --- a/micras_nav/include/micras/nav/segment.hpp +++ b/micras_nav/include/micras/nav/segment.hpp @@ -15,13 +15,12 @@ namespace micras::nav { * @brief Kinds of motion a route is made of. */ enum class SegmentKind : uint8_t { - STRAIGHT = 0, // Straight line along the grid. - DIAGONAL = 1, // Straight line along a diagonal. - TURN = 2, // Slalom turn from the turn table, braked and accelerated as its curvature allows. - SPIN = 3, // Rotation in place. - STOP = 4, // Stand still for a given time. - ATTACH = 5, // Stand still facing a wall until the pose settles, or for a given time at most. - LINE = 6, // The racing line, from the start to the goal, at the speeds it was planned with. + STRAIGHT = 0, // Straight line, along the grid or along a diagonal. + TURN = 1, // Slalom turn from the turn table, braked and accelerated as its curvature allows. + SPIN = 2, // Rotation in place. + STOP = 3, // Stand still for a given time. + ATTACH = 4, // Stand still facing a wall until the pose settles, or for a given time at most. + LINE = 5, // The racing line, from the start to the goal, at the speeds it was planned with. }; /** diff --git a/micras_nav/include/micras/nav/speed_profile.hpp b/micras_nav/include/micras/nav/speed_profile.hpp index 63defe6..37e8998 100644 --- a/micras_nav/include/micras/nav/speed_profile.hpp +++ b/micras_nav/include/micras/nav/speed_profile.hpp @@ -15,11 +15,12 @@ namespace micras::nav { * @brief Fastest motion along one axis between two speeds, as a function of time. * * @details The motion accelerates as hard as the limits allow, cruises if it reaches the speed - * limit and brakes at the last moment, which is the time optimal way to cover a distance. It is the - * single evaluator of the navigation: the planner prices a straight with duration(), the velocity - * planner chains straights with get_reachable_speed() and get_brakeable_speed(), and the executor - * plays the very same object back with sample(), so that the time a route is chosen by is the time - * it takes. The axis is a distance for a straight and an angle for a rotation in place. + * limit and brakes at the last moment, which is the time optimal way to cover a distance. It times + * and plays back the straights and the rotations in place: the planner prices a straight with + * duration(), the velocity planner chains straights with get_reachable_speed() and + * get_brakeable_speed(), and the executor plays the very same object back with sample(), so that the + * time a route is chosen by is the time it takes. The axis is a distance for a straight and an angle + * for a rotation in place. */ class SpeedProfile { public: diff --git a/micras_nav/include/micras/nav/turn_table.hpp b/micras_nav/include/micras/nav/turn_table.hpp index f9003b4..05bf981 100644 --- a/micras_nav/include/micras/nav/turn_table.hpp +++ b/micras_nav/include/micras/nav/turn_table.hpp @@ -283,9 +283,8 @@ struct TurnShape { float straight_length{}; /** - * @brief Heading at the exit relative to the entry, and the largest curvature and sharpness. + * @brief Largest curvature and sharpness. */ - float angle{}; float curvature{}; float sharpness{}; @@ -309,7 +308,6 @@ struct TurnShape { * @details The angles are those of the turn to the left, positive to the left, and the curvatures * are the peaks asked of each bend. The straight between the bends and the one after them follow * from the place of the exit node, so a design is only the choice of how the turn swings. - * */ struct TwoBendDesign { float first_angle; @@ -581,7 +579,6 @@ class TurnTable { shape.bends.at(0) = to_bend(first, first_angle); shape.bends.at(1) = to_bend(second, second_angle); shape.number_of_bends = 2; - shape.angle = static_cast(first_angle + second_angle); shape.curvature = std::max(shape.bends.at(0).curvature, shape.bends.at(1).curvature); shape.sharpness = std::max(shape.bends.at(0).sharpness, shape.bends.at(1).sharpness); shape.pre = design.pre; @@ -727,7 +724,6 @@ class TurnTable { TurnShape shape{}; shape.bends.at(0) = to_bend(candidate, angle); shape.number_of_bends = 1; - shape.angle = static_cast(angle); shape.curvature = static_cast(candidate.curvature); shape.sharpness = static_cast(candidate.sharpness); shape.pre = static_cast(candidate.pre); diff --git a/micras_nav/src/drive_identification.cpp b/micras_nav/src/drive_identification.cpp index f23e070..1f7c3db 100644 --- a/micras_nav/src/drive_identification.cpp +++ b/micras_nav/src/drive_identification.cpp @@ -62,7 +62,7 @@ Controller::Command DriveIdentification::update(const Measurements& measurements if (std::abs(linear_speed) > this->config.breakaway_speed or command >= this->config.linear_command) { this->breakaway_voltage = command * to_volts; - this->advance(measurements); + this->advance(); return {}; } @@ -87,7 +87,7 @@ Controller::Command DriveIdentification::update(const Measurements& measurements not rotating and std::abs(this->distance) > this->config.max_distance and (this->distance > 0.0F) == positive; if (this->phase_time >= 2.0F * this->config.step_time or too_far) { - this->advance(measurements); + this->advance(); return {}; } @@ -189,7 +189,7 @@ bool DriveIdentification::Fit::solve(Axis& axis) const { return axis.speed_constant > 0.0F and axis.acceleration_constant > 0.0F; } -void DriveIdentification::advance(const Measurements& /*measurements*/) { +void DriveIdentification::advance() { this->phase = static_cast(static_cast(this->phase) + 1); this->phase_time = 0.0F; this->rest_left = this->config.rest_time; diff --git a/micras_nav/src/executor.cpp b/micras_nav/src/executor.cpp index 6bbe473..40369ae 100644 --- a/micras_nav/src/executor.cpp +++ b/micras_nav/src/executor.cpp @@ -139,7 +139,6 @@ void Executor::start_next() { switch (segment.kind) { case SegmentKind::STRAIGHT: - case SegmentKind::DIAGONAL: this->speed_profile = SpeedProfile{ std::abs(segment.length), segment.start_speed, segment.end_speed, this->dynamics.get_linear_limits(this->run_profile) @@ -173,8 +172,7 @@ Reference Executor::evaluate() const { const float side = std::copysign(1.0F, segment.length); switch (segment.kind) { - case SegmentKind::STRAIGHT: - case SegmentKind::DIAGONAL: { + case SegmentKind::STRAIGHT: { const SpeedProfile::Sample sample = this->speed_profile.sample(this->clock); return { diff --git a/micras_nav/src/motion_limits.cpp b/micras_nav/src/motion_limits.cpp index 1b0e50f..9194540 100644 --- a/micras_nav/src/motion_limits.cpp +++ b/micras_nav/src/motion_limits.cpp @@ -36,9 +36,7 @@ float CurveLimits::get_speed_limit(const Bending& bending) const { } float CurveLimits::get_acceleration(float speed, const Bending& bending) const { - const float lateral_use = speed * speed * std::abs(bending.curvature) / this->lateral; - const float angular_use = speed * speed * std::abs(bending.sharpness) / this->angular; - const float spare = std::sqrt(std::max(1.0F - lateral_use * lateral_use, 0.0F)) - angular_use; + const float spare = this->get_spare(speed, bending); if (spare <= 0.0F) { return 0.0F; @@ -50,9 +48,7 @@ float CurveLimits::get_acceleration(float speed, const Bending& bending) const { } float CurveLimits::get_deceleration(float speed, const Bending& bending) const { - const float lateral_use = speed * speed * std::abs(bending.curvature) / this->lateral; - const float angular_use = speed * speed * std::abs(bending.sharpness) / this->angular; - const float spare = std::sqrt(std::max(1.0F - lateral_use * lateral_use, 0.0F)) - angular_use; + const float spare = this->get_spare(speed, bending); if (spare <= 0.0F) { return 0.0F; @@ -61,6 +57,13 @@ float CurveLimits::get_deceleration(float speed, const Bending& bending) const { return spare / (1.0F / this->linear.deceleration + std::abs(bending.curvature) / this->angular); } +float CurveLimits::get_spare(float speed, const Bending& bending) const { + const float lateral_use = speed * speed * std::abs(bending.curvature) / this->lateral; + const float angular_use = speed * speed * std::abs(bending.sharpness) / this->angular; + + return std::sqrt(std::max(1.0F - lateral_use * lateral_use, 0.0F)) - angular_use; +} + Dynamics::Dynamics(const Config& config) : model{config.model}, turns{config.turns}, @@ -112,12 +115,7 @@ const TurnShape& Dynamics::get_turn(const RunProfile& profile, TurnId turn) cons float Dynamics::get_turn_speed(const RunProfile& profile, TurnId turn) const { const TurnShape& shape = this->get_turn(profile, turn); - const float lateral = profile.utilization * this->model.traction_acceleration(profile.fan); - const float angular = profile.utilization * this->model.traction_angular_acceleration(profile.fan); - - return std::min( - {std::sqrt(lateral / shape.curvature), std::sqrt(angular / shape.sharpness), - this->get_linear_limits(profile).max_speed} + return this->get_curve_limits(profile).get_speed_limit({.curvature = shape.curvature, .sharpness = shape.sharpness} ); } diff --git a/micras_nav/src/route_compiler.cpp b/micras_nav/src/route_compiler.cpp index 9a1debd..5d8d2de 100644 --- a/micras_nav/src/route_compiler.cpp +++ b/micras_nav/src/route_compiler.cpp @@ -2,7 +2,6 @@ * @file */ -#include #include #include #include @@ -26,10 +25,10 @@ void RouteCompiler::compile( return node.to_pose(cell_size).compose({.position = {.x = -distance, .y = 0.0F}, .orientation = 0.0F}); }; - const auto emit_straight = [&segments](const Pose& start, float length, bool diagonal) { + const auto emit_straight = [&segments](const Pose& start, float length) { if (length > min_straight) { segments.push_back({ - .kind = diagonal ? SegmentKind::DIAGONAL : SegmentKind::STRAIGHT, + .kind = SegmentKind::STRAIGHT, .turn = TurnId::SS90S, .length = length, .start_speed = 0.0F, @@ -60,7 +59,7 @@ void RouteCompiler::compile( const TurnShape& shape = dynamics.get_turn(profile, step.turn); - emit_straight(straight_start, straight_length + shape.pre, node.is_diagonal()); + emit_straight(straight_start, straight_length + shape.pre); segments.push_back({ .kind = SegmentKind::TURN, @@ -76,6 +75,6 @@ void RouteCompiler::compile( straight_length = shape.post; } - emit_straight(straight_start, straight_length + route.stop_distance, node.is_diagonal()); + emit_straight(straight_start, straight_length + route.stop_distance); } } // namespace micras::nav diff --git a/micras_nav/src/velocity_planner.cpp b/micras_nav/src/velocity_planner.cpp index 2be0c70..8dec7a8 100644 --- a/micras_nav/src/velocity_planner.cpp +++ b/micras_nav/src/velocity_planner.cpp @@ -25,15 +25,11 @@ float VelocityPlanner::plan( const MotionLimits linear_limits = dynamics.get_linear_limits(profile); const CurveLimits curve_limits = dynamics.get_curve_limits(profile); - const auto is_straight = [](const Segment& segment) { - return segment.kind == SegmentKind::STRAIGHT or segment.kind == SegmentKind::DIAGONAL; - }; - for (Segment& segment : route) { segment.start_speed = 0.0F; segment.end_speed = 0.0F; - if (is_straight(segment)) { + if (segment.kind == SegmentKind::STRAIGHT) { segment.start_speed = linear_limits.max_speed; segment.end_speed = linear_limits.max_speed; } else if (segment.kind == SegmentKind::TURN) { @@ -49,7 +45,7 @@ float VelocityPlanner::plan( for (Segment& segment : route | std::views::reverse) { segment.end_speed = std::min(segment.end_speed, next_speed); - if (is_straight(segment)) { + if (segment.kind == SegmentKind::STRAIGHT) { segment.start_speed = std::min( segment.start_speed, SpeedProfile::get_brakeable_speed(std::abs(segment.length), segment.end_speed, linear_limits) @@ -71,7 +67,7 @@ float VelocityPlanner::plan( for (Segment& segment : route) { segment.start_speed = std::min(segment.start_speed, previous_speed); - if (is_straight(segment)) { + if (segment.kind == SegmentKind::STRAIGHT) { segment.end_speed = std::min( segment.end_speed, SpeedProfile::get_reachable_speed(std::abs(segment.length), segment.start_speed, linear_limits) @@ -100,7 +96,6 @@ float VelocityPlanner::plan( float VelocityPlanner::get_duration(const Segment& segment, const Dynamics& dynamics, const RunProfile& profile) { switch (segment.kind) { case SegmentKind::STRAIGHT: - case SegmentKind::DIAGONAL: return SpeedProfile{ std::abs(segment.length), segment.start_speed, segment.end_speed, dynamics.get_linear_limits(profile) } From 3fa4b67dddc8ffdfed575e0894a79926a0fa3cb5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Sep 2026 22:11:10 -0300 Subject: [PATCH 510/561] =?UTF-8?q?=E2=9C=A8=20Watch=20everything=20throug?= =?UTF-8?q?h=20the=20variable=20pool,=20and=20run=20the=20fan=20in=20maint?= =?UTF-8?q?enance=20by=20its=20switch?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The monitor_* globals are gone. Half of them copied variables the pool already had; the rest are pool variables now: the localizer's bias, deviations and corrections, the wall sensors' reference readings and calibration spreads, the time of the planned route, which is zero while there is none, and the results of the drive identification and the gyroscope calibration, which the link and the simulator can read. The pool holds up to 96 variables. The flash finds what it saves by name, so maps saved before still load. A maintenance procedure used to run the fan only when the objective left over from the last search was a solve. It now runs the fan whenever its switch is on; a search never does. The navigation's copy of the slow wall distance, which nothing in it read, is gone. --- micras_nav/include/micras/nav/measurements.hpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/micras_nav/include/micras/nav/measurements.hpp b/micras_nav/include/micras/nav/measurements.hpp index cbf8ae1..6c1bf68 100644 --- a/micras_nav/include/micras/nav/measurements.hpp +++ b/micras_nav/include/micras/nav/measurements.hpp @@ -14,11 +14,9 @@ namespace micras::nav { /** * @brief Reading of one wall sensor. * - * @details The distance is measured along the optical axis, in meters, and comes in two versions: - * a fast one for everything that is a position, and a slow one for deciding whether there is - * something there at all. It is valid when the receiver sees anything above its noise, so a reading - * that is not valid means that nothing is within range. A receiver that saturates reports the - * shortest distance it can measure. + * @details The distance is measured along the optical axis, in meters. It is valid when the receiver + * sees anything above its noise, so a reading that is not valid means that nothing is within range. A receiver that + * saturates reports the shortest distance it can measure. * * @note The new flag is set for a single iteration after the sensor produces a value. An estimator * that is fed the same sample twice counts it as two independent observations and becomes @@ -26,7 +24,6 @@ namespace micras::nav { */ struct WallReading { float distance; - float slow_distance; bool valid; bool is_new; }; From ec2b8bf41f880ff8b5653502466a0d642cb21752 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Sep 2026 22:11:18 -0300 Subject: [PATCH 511/561] =?UTF-8?q?=F0=9F=93=9D=20Say=20what=20the=20rest?= =?UTF-8?q?=20of=20the=20firmware=20does,=20not=20how=20it=20got=20there?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The comments that told history or were wrong now describe the current state: the timing of the addressable LEDs, the Butterworth recursion, the state machine, the tick, a parameter of the frame writer, the frame test vectors. The comment inside the pool's read-only overload moves to its documentation, and the commented-out code of the rotary sensor test is gone. The README lists micras_comm, describes the maintenance results as pool variables and the host tests, and the documentation covers micras_comm. --- micras_comm/include/micras/comm/frame.hpp | 2 +- micras_core/include/micras/core/butterworth_filter.hpp | 4 +--- micras_core/include/micras/core/fsm.hpp | 2 +- micras_core/include/micras/core/impl/variable_pool.tpp | 3 --- micras_core/include/micras/core/variable_pool.hpp | 4 +++- micras_proxy/include/micras/proxy/tick.hpp | 2 +- tests/test_frame.cpp | 5 ++--- 7 files changed, 9 insertions(+), 13 deletions(-) diff --git a/micras_comm/include/micras/comm/frame.hpp b/micras_comm/include/micras/comm/frame.hpp index 7927d15..8120388 100644 --- a/micras_comm/include/micras/comm/frame.hpp +++ b/micras_comm/include/micras/comm/frame.hpp @@ -48,7 +48,7 @@ class Writer { /** * @brief Append a string with no length prefix. * - * @param text Text to append. + * @param value Text to append. */ void text(std::string_view value); diff --git a/micras_core/include/micras/core/butterworth_filter.hpp b/micras_core/include/micras/core/butterworth_filter.hpp index 56add89..2df5dbd 100644 --- a/micras_core/include/micras/core/butterworth_filter.hpp +++ b/micras_core/include/micras/core/butterworth_filter.hpp @@ -100,9 +100,7 @@ class ButterworthFilter { * which is the same transfer function. The direct form computes the output as the small * difference of two large terms, and gets its unity gain from three rounded coefficients * adding up exactly, which single precision stops doing when the cutoff is a thousand times - * below the sampling rate: a 7.64 Hz filter sampled at 10 kHz comes out with a gain error of - * 0.35 % and blind to changes of 0.04 %. In this form the gain is one by construction and the - * same filter is good to 0.001 %. + * below the sampling rate. In this form the gain is one by construction. * * @param x0 Last measure. * @return Filtered value. diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 70267a7..cf68ea1 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -68,7 +68,7 @@ class FsmState { * * @note The states are stored in an array indexed by their own id, so running the machine costs an * array access rather than a hash lookup. That only works because the ids are a dense enumeration - * from zero, which the number of states asserts. + * from zero. * * @tparam num_of_states Number of states, and therefore one past the largest valid id. */ diff --git a/micras_core/include/micras/core/impl/variable_pool.tpp b/micras_core/include/micras/core/impl/variable_pool.tpp index 83817f6..442ceaa 100644 --- a/micras_core/include/micras/core/impl/variable_pool.tpp +++ b/micras_core/include/micras/core/impl/variable_pool.tpp @@ -41,9 +41,6 @@ VariableId VariablePool::add(std::string_view prefix, std::string_view name, con return invalid_id; } - // The pool holds one mutable address for every kind of variable, so that no consumer has to - // branch on constness. Casting it away is safe because this overload has just cleared the only - // flag through which anything can write. *variable = { .prefix = prefix, .name = name, diff --git a/micras_core/include/micras/core/variable_pool.hpp b/micras_core/include/micras/core/variable_pool.hpp index 8bc45b2..4cd7793 100644 --- a/micras_core/include/micras/core/variable_pool.hpp +++ b/micras_core/include/micras/core/variable_pool.hpp @@ -224,7 +224,9 @@ class VariablePool { * @brief Register a read only variable. * * @note The write flag is cleared whatever the caller asked for, so that constness is enough to - * state the intent at the registration site. + * state the intent at the registration site. The pool holds one mutable address for every kind + * of variable, so that no consumer has to branch on constness, and casting the constness away is + * safe because the flag through which anything can write is cleared. * * @tparam T Type of the variable. * @param prefix Prefix of the name, usually identifying the owner. diff --git a/micras_proxy/include/micras/proxy/tick.hpp b/micras_proxy/include/micras/proxy/tick.hpp index 7106e21..6605e31 100644 --- a/micras_proxy/include/micras/proxy/tick.hpp +++ b/micras_proxy/include/micras/proxy/tick.hpp @@ -18,7 +18,7 @@ namespace micras::proxy { * would jitter by a percent. * * @note The loop stays a plain loop: the wait spins on the cycle counter and no interrupt is - * involved. A simulator implements the wait by advancing its physics by one period. + * involved. */ class Tick { public: diff --git a/tests/test_frame.cpp b/tests/test_frame.cpp index edae639..15d1654 100644 --- a/tests/test_frame.cpp +++ b/tests/test_frame.cpp @@ -22,9 +22,8 @@ struct Vector { std::vector frame; }; -// The same table lives in micras-monitor, as src/lib/comm/__tests__/frameVectors.ts. Every writer -// and reader disagreement this project has had was invisible until something was on the wire, so -// the two implementations are pinned to the same bytes rather than to each other's good intentions. +// The same table lives in micras-monitor, as src/lib/comm/__tests__/frameVectors.ts, so that both +// implementations are pinned to the same bytes. const std::vector vectors{ {"hello", MessageType{0x01}, {}, {4, 1, 1, 1, 0}}, {"hello_ack", From 251688a7a263d8ece769faebae674540ced66000 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Sep 2026 22:12:11 -0300 Subject: [PATCH 512/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Cut=20what=20nothi?= =?UTF-8?q?ng=20uses=20from=20the=20simulator,=20and=20fix=20what=20the=20?= =?UTF-8?q?review=20found?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The firmware moves three commits on: the turn tables designed once, everything watched through the variable pool, and comments that describe the current state. The Micras target compiles without the job pool it needed when every source designed the turns, and the turn designer links the firmware alone. Fixed: - wall-calibration traced its rays through the whole maze, since the geom group of the paint was left at 0; the robot and the paint now share one group the range sensors skip, and the tool prints the gains robot.toml holds back. - Tab reached every other camera; it now cycles through the free one and each camera of the model. - A step asked for while the run was running fired once it paused; stepping now only works while paused. - Touching the panel marked the run interactive but left the scenario driving the inputs; the application now hands the board over. Removed, as nothing used them: push, whose force a window cleared after one tick; --video-trail-color and --video-tint; the provenance record of the robot description, max_motor_speed, imu.chip_z and the battery's load and resistance; the forward axis and three probes of the ground truth; Target::check_options and default_scenario, Wiring::inputs, observers and monitor_inbound; the listener that named unbound ports, which the target now does at the end of the run; counters of the host HAL nothing read, the unused vendor constants and the clock's quantum parameter; Noise::enabled and SerialBus::sent_bytes. Merged: the recorder is created in one step with its overlay and trail; the window and the recorder share the camera lookup; the three loaders share one file reader; the scenario hooks and the event log share one type of state names; the joints are looked up by the world; DigitalInput has its own header; the panel spec moves to the view, its only user; the tests share their helpers. The switch columns are named dip_fan, dip_racing_line, dip_boost and dip_risky, and the build checks that the target names every firmware state. Every checked run keeps its bytes but for those columns and the new pool variables. --- .../host/include/micras/hal/host/board.hpp | 2 +- .../host/include/micras/hal/host/clock.hpp | 11 +++-- .../host/include/micras/hal/host/ports.hpp | 40 +------------------ micras_hal/host/include/stm32_host.h | 19 --------- micras_hal/host/src/adc_dma.cpp | 2 - micras_hal/host/src/clock.cpp | 4 +- micras_hal/host/src/flash.cpp | 2 - micras_hal/host/src/mcu.cpp | 1 - micras_hal/host/src/ports.cpp | 5 +-- micras_hal/host/src/pwm.cpp | 1 - micras_hal/host/src/pwm_dma.cpp | 1 - micras_hal/host/src/uart_dma.cpp | 2 - 12 files changed, 10 insertions(+), 80 deletions(-) diff --git a/micras_hal/host/include/micras/hal/host/board.hpp b/micras_hal/host/include/micras/hal/host/board.hpp index 1d62ef8..86c1e9d 100644 --- a/micras_hal/host/include/micras/hal/host/board.hpp +++ b/micras_hal/host/include/micras/hal/host/board.hpp @@ -134,7 +134,7 @@ class Board { /** * @brief Forget every port and name. * - * @note For tests. References to ports obtained before are dangling after. + * @note Called at the start of every run. References to ports obtained before are dangling after. */ static void reset(); }; diff --git a/micras_hal/host/include/micras/hal/host/clock.hpp b/micras_hal/host/include/micras/hal/host/clock.hpp index 108d738..4abe3fe 100644 --- a/micras_hal/host/include/micras/hal/host/clock.hpp +++ b/micras_hal/host/include/micras/hal/host/clock.hpp @@ -14,9 +14,9 @@ namespace micras::hal::host { /** * @brief Counts the core cycles the firmware sees, and hands time over to the world. * - * @note Every read of the timer costs a fixed quantum of simulated time, one - * microsecond unless configured otherwise, since a read on the robot also - * takes time and a loop that polls the timer must see it advance. When a + * @note Every read of the timer costs a quantum of simulated time, one + * microsecond, since a read on the robot also takes time and a loop that + * polls the timer must see it advance. When a * read crosses the end of a step, the handover callback runs: whatever * drives the world advances it by one step and returns, and the read * returns after it. So a busy wait runs the world exactly as long as it @@ -40,12 +40,11 @@ class Clock { static Clock& instance(); /** - * @brief Set the core frequency and the cost of one read. + * @brief Set the core frequency, which makes a read cost one microsecond of cycles. * * @param cycles_per_microsecond Core clock, in cycles per microsecond. - * @param quantum_us Simulated time one read costs, in microseconds. */ - void configure(uint32_t cycles_per_microsecond, uint32_t quantum_us = 1); + void configure(uint32_t cycles_per_microsecond); /** * @brief Hand time over to the world at every step. diff --git a/micras_hal/host/include/micras/hal/host/ports.hpp b/micras_hal/host/include/micras/hal/host/ports.hpp index f192967..c7ae30b 100644 --- a/micras_hal/host/include/micras/hal/host/ports.hpp +++ b/micras_hal/host/include/micras/hal/host/ports.hpp @@ -102,11 +102,6 @@ struct PwmDmaPort : Port { * @brief Host clock cycle the current transfer ends at, one timer period per value. */ uint64_t transfer_end{0}; - - /** - * @brief Number of transfers the firmware started. - */ - uint32_t transfers{0}; }; /** @@ -125,21 +120,11 @@ struct AdcPort : Port { std::span buffer32; ///@} - /** - * @brief Number of conversions in one sequence, from the ADC configuration. - */ - uint32_t conversions{0}; - /** * @brief Called at the end of a sequence, as the conversion complete interrupt would. */ std::function complete; - /** - * @brief Number of sequences finished. - */ - uint32_t sequences{0}; - /** * @brief Get how many conversions the buffer holds. * @@ -161,7 +146,7 @@ struct AdcPort : Port { /** * @brief End the sequence, as the DMA interrupt does. */ - void finish_sequence(); + void finish_sequence() const; }; /** @@ -183,16 +168,6 @@ struct UartPort : Port { */ std::deque tx; - /** - * @brief Bytes received while the receiver was not started. - */ - uint32_t lost_bytes{0}; - - /** - * @brief Baud rate from the UART configuration. - */ - uint32_t baud_rate{0}; - /** * @brief Put one received byte where the DMA would. * @@ -216,14 +191,6 @@ struct FlashPort : Port { * @brief Contents, erased to 0xFF. */ std::vector bytes; - - /** - * @brief Number of program and erase operations performed. - */ - ///@{ - uint32_t writes{0}; - uint32_t erases{0}; - ///@} }; /** @@ -240,11 +207,6 @@ struct McuPort : Port { */ uint64_t last_refresh{0}; - /** - * @brief Number of refreshes. - */ - uint64_t refreshes{0}; - /** * @brief Number of times the watchdog would have reset the chip. */ diff --git a/micras_hal/host/include/stm32_host.h b/micras_hal/host/include/stm32_host.h index cecec7f..244bb50 100644 --- a/micras_hal/host/include/stm32_host.h +++ b/micras_hal/host/include/stm32_host.h @@ -17,20 +17,6 @@ #include -/***************************************** - * Status - *****************************************/ - -/** - * @brief Result of a vendor HAL call. - */ -enum HAL_StatusTypeDef : uint8_t { - HAL_OK = 0, - HAL_ERROR = 1, - HAL_BUSY = 2, - HAL_TIMEOUT = 3, -}; - /***************************************** * GPIO *****************************************/ @@ -81,7 +67,6 @@ struct TIM_TypeDef { uint32_t CCER; uint32_t PSC; uint32_t ARR; - uint32_t CNT; uint32_t kernel_clock; }; @@ -100,7 +85,6 @@ struct TIM_Base_InitTypeDef { enum HAL_TIM_StateTypeDef : uint8_t { HAL_TIM_STATE_RESET = 0, HAL_TIM_STATE_READY = 1, - HAL_TIM_STATE_BUSY = 2, }; /** @@ -200,7 +184,6 @@ struct SPI_HandleTypeDef { enum HAL_UART_StateTypeDef : uint8_t { HAL_UART_STATE_RESET = 0x00, HAL_UART_STATE_READY = 0x20, - HAL_UART_STATE_BUSY_TX = 0x21, HAL_UART_STATE_BUSY_RX = 0x22, }; @@ -279,6 +262,4 @@ struct FMAC_HandleTypeDef { uint32_t State; }; -inline constexpr uint32_t HAL_FMAC_STATE_RESET{0x00000000U}; - #endif // STM32_HOST_H diff --git a/micras_hal/host/src/adc_dma.cpp b/micras_hal/host/src/adc_dma.cpp index 6114873..15c8eb2 100644 --- a/micras_hal/host/src/adc_dma.cpp +++ b/micras_hal/host/src/adc_dma.cpp @@ -31,7 +31,6 @@ bool AdcDma::start_dma(std::span buffer) { port.touched = true; port.buffer32 = buffer; port.buffer16 = {}; - port.conversions = this->handle->Init.NbrOfConversion; port.complete = [adc = this->handle] { on_sequence_complete(adc); }; return true; } @@ -41,7 +40,6 @@ bool AdcDma::start_dma(std::span buffer) { port.touched = true; port.buffer16 = buffer; port.buffer32 = {}; - port.conversions = this->handle->Init.NbrOfConversion; port.complete = [adc = this->handle] { on_sequence_complete(adc); }; return true; } diff --git a/micras_hal/host/src/clock.cpp b/micras_hal/host/src/clock.cpp index 051c506..27206dc 100644 --- a/micras_hal/host/src/clock.cpp +++ b/micras_hal/host/src/clock.cpp @@ -12,9 +12,9 @@ Clock& Clock::instance() { return clock; } -void Clock::configure(uint32_t cycles_per_microsecond, uint32_t quantum_us) { +void Clock::configure(uint32_t cycles_per_microsecond) { this->cycles_per_us = cycles_per_microsecond; - this->quantum = static_cast(quantum_us) * cycles_per_microsecond; + this->quantum = cycles_per_microsecond; } void Clock::set_handover(uint32_t step_us, Handover handover) { diff --git a/micras_hal/host/src/flash.cpp b/micras_hal/host/src/flash.cpp index 9404eed..302cf50 100644 --- a/micras_hal/host/src/flash.cpp +++ b/micras_hal/host/src/flash.cpp @@ -102,7 +102,6 @@ Flash::Status Flash::write(uint32_t address, std::span data) { const std::span bytes{std::bit_cast(word.data()), FlashWord::size}; std::ranges::copy(bytes, target.begin()); - port.writes++; } return Status::OK; @@ -126,7 +125,6 @@ Flash::Status Flash::erase_sectors(uint16_t start_sector, uint16_t number_of_sec const auto count = static_cast(number_of_sectors) * sector_size; std::fill(port.bytes.begin() + first, port.bytes.begin() + first + count, FlashWord::erased_value); - port.erases++; return Status::OK; } diff --git a/micras_hal/host/src/mcu.cpp b/micras_hal/host/src/mcu.cpp index cbbb810..cf3543e 100644 --- a/micras_hal/host/src/mcu.cpp +++ b/micras_hal/host/src/mcu.cpp @@ -73,6 +73,5 @@ void Mcu::refresh_watchdog() { } mcu.last_refresh = clock.now(); - mcu.refreshes++; } } // namespace micras::hal diff --git a/micras_hal/host/src/ports.cpp b/micras_hal/host/src/ports.cpp index 0ec8aea..3483ab3 100644 --- a/micras_hal/host/src/ports.cpp +++ b/micras_hal/host/src/ports.cpp @@ -13,9 +13,7 @@ void AdcPort::write(std::size_t index, uint32_t counts) const { } } -void AdcPort::finish_sequence() { - this->sequences++; - +void AdcPort::finish_sequence() const { if (this->complete) { this->complete(); } @@ -23,7 +21,6 @@ void AdcPort::finish_sequence() { void UartPort::receive(uint8_t byte) { if (this->rx_buffer.empty()) { - this->lost_bytes++; return; } diff --git a/micras_hal/host/src/pwm.cpp b/micras_hal/host/src/pwm.cpp index 750e885..8d21122 100644 --- a/micras_hal/host/src/pwm.cpp +++ b/micras_hal/host/src/pwm.cpp @@ -67,7 +67,6 @@ void Pwm::set_frequency(uint32_t frequency) { const uint32_t prescaler = this->handle->Instance->PSC; this->handle->Instance->ARR = this->handle->Instance->kernel_clock / ((prescaler + 1) * frequency) - 1; - this->handle->Instance->CNT = 0; host::PwmPort& port = host::Board::pwm(this->handle, this->channel); port.touched = true; diff --git a/micras_hal/host/src/pwm_dma.cpp b/micras_hal/host/src/pwm_dma.cpp index c69462c..c96863a 100644 --- a/micras_hal/host/src/pwm_dma.cpp +++ b/micras_hal/host/src/pwm_dma.cpp @@ -25,7 +25,6 @@ void start(TIM_HandleTypeDef* handle, uint32_t channel, std::span compa port.compares.assign(compares.begin(), compares.end()); port.period = handle->Instance->ARR + 1; port.busy = true; - port.transfers++; const uint64_t cycles_per_count = static_cast(handle->Instance->PSC + 1) * host::Clock::instance().cycles_per_microsecond() * 1000000ULL / diff --git a/micras_hal/host/src/uart_dma.cpp b/micras_hal/host/src/uart_dma.cpp index f8af1e5..6e4f9b3 100644 --- a/micras_hal/host/src/uart_dma.cpp +++ b/micras_hal/host/src/uart_dma.cpp @@ -16,8 +16,6 @@ UartDma::UartDma(const Config& config) : handle{config.handle} { if (this->handle->gState == HAL_UART_STATE_RESET) { config.init_function(); } - - host::Board::uart(this->handle).baud_rate = this->handle->Init.BaudRate; } bool UartDma::start_rx(std::span buffer) { From 3eeb3a5baa38b81147d91c4e7e8512d9a590d392 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Sep 2026 22:20:07 -0300 Subject: [PATCH 513/561] =?UTF-8?q?=F0=9F=8E=A8=20Format=20with=20the=20cl?= =?UTF-8?q?ang-format=20the=20CI=20runs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The CI formats with clang-format 22, which breaks the conditions of a few statements and the designated initializers of the board's configuration differently from clang-format 18. --- micras_nav/include/micras/nav/impl/mission.tpp | 15 ++++++++++----- .../include/micras/nav/impl/racing_line.tpp | 2 +- micras_nav/src/motion_limits.cpp | 3 ++- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/micras_nav/include/micras/nav/impl/mission.tpp b/micras_nav/include/micras/nav/impl/mission.tpp index dd33a01..b9546c1 100644 --- a/micras_nav/include/micras/nav/impl/mission.tpp +++ b/micras_nav/include/micras/nav/impl/mission.tpp @@ -222,7 +222,8 @@ void TMission::start(core::Objective objective) { this->flood(); Move move{}; - move.add(make_segment(SegmentKind::STRAIGHT, cell_size - this->config.start_offset, this->get_start_pose()) + move.add( + make_segment(SegmentKind::STRAIGHT, cell_size - this->config.start_offset, this->get_start_pose()) ); this->execute(move, 0.0F, this->search_speed); break; @@ -277,8 +278,10 @@ TMission::Status TMission::update( if (wall == WallState::NO_WALL) { this->watching_front = false; - } else if (wall == WallState::WALL or - this->executor.get_reference().distance >= cell_size / 2.0F - braking - this->config.commit_margin) { + } else if ( + wall == WallState::WALL or + this->executor.get_reference().distance >= cell_size / 2.0F - braking - this->config.commit_margin + ) { this->divert_to_center(); } } @@ -533,8 +536,10 @@ bool TMission::finish_at_entry() { if (this->objective == core::Objective::EXPLORE and this->maze.is_goal(this->cell.position)) { this->add_stop_at_center(move); - } else if (this->objective == core::Objective::RETURN and this->explorer.is_complete() and - this->cell.position == this->maze.get_start().position) { + } else if ( + this->objective == core::Objective::RETURN and this->explorer.is_complete() and + this->cell.position == this->maze.get_start().position + ) { const GridPose parked{.position = this->cell.position, .orientation = this->maze.get_start().orientation}; this->add_stop_at_center(move); diff --git a/micras_nav/include/micras/nav/impl/racing_line.tpp b/micras_nav/include/micras/nav/impl/racing_line.tpp index c0db9a8..2cbf3ba 100644 --- a/micras_nav/include/micras/nav/impl/racing_line.tpp +++ b/micras_nav/include/micras/nav/impl/racing_line.tpp @@ -535,7 +535,7 @@ void TRacingLine::solve_slides(std::span normals_x, const uint16_t first = this->window_start; const auto size = static_cast(normals_x.size()); const double length_weight = static_cast(this->config.length_weight) * - static_cast(this->line.spacing) * static_cast(this->line.spacing); + static_cast(this->line.spacing) * static_cast(this->line.spacing); this->diagonal.fill(0.0); this->first_band.fill(0.0); diff --git a/micras_nav/src/motion_limits.cpp b/micras_nav/src/motion_limits.cpp index 9194540..d514e4e 100644 --- a/micras_nav/src/motion_limits.cpp +++ b/micras_nav/src/motion_limits.cpp @@ -115,7 +115,8 @@ const TurnShape& Dynamics::get_turn(const RunProfile& profile, TurnId turn) cons float Dynamics::get_turn_speed(const RunProfile& profile, TurnId turn) const { const TurnShape& shape = this->get_turn(profile, turn); - return this->get_curve_limits(profile).get_speed_limit({.curvature = shape.curvature, .sharpness = shape.sharpness} + return this->get_curve_limits(profile).get_speed_limit( + {.curvature = shape.curvature, .sharpness = shape.sharpness} ); } From 7f412b36ea4b8c3574c95f75f687c0458debc7a6 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Sep 2026 22:39:43 -0300 Subject: [PATCH 514/561] =?UTF-8?q?=F0=9F=90=9B=20Build,=20test=20and=20li?= =?UTF-8?q?nt=20the=20navigation=20the=20way=20the=20CI=20does?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The CI builds for the Cortex-M7 with every warning an error and lints with clang-tidy 22, which the navigation of the last commits had never been through: - The racing line solver indexes its window with unsigned counters, and the few places that need a signed offset check its sign before converting it once; the planner converts the index of an edge pair once. Every value stays the same. - The float fields the turn designer reads into doubles are cast explicitly, the count of samples along a turn is one truncation, and the rounding of the number of revolutions, which a constant expression cannot do with lround, is marked as what it is. - The curve speeds walk their spans with iterators and read an interval as a two element subspan instead of indexing them. - curve_speed.tpp no longer includes the header that includes it, and micras.cpp includes the variable pool it names. - The controller test lost the maximum speed the segments no longer have, and hands the executor an empty racing line. The turns are designed in config/dynamics_config.cpp, compiled once as an object library that the executable and every test link, since the tests need the dynamics too and designing the turns in each of them would take most of a gigabyte each. --- .../include/micras/nav/impl/curve_speed.tpp | 1 - .../include/micras/nav/impl/planner.tpp | 8 +-- .../include/micras/nav/impl/racing_line.tpp | 51 +++++++++++-------- micras_nav/include/micras/nav/racing_line.hpp | 2 +- micras_nav/include/micras/nav/turn_table.hpp | 30 ++++++----- micras_nav/src/curve_speed.cpp | 19 ++++--- 6 files changed, 63 insertions(+), 48 deletions(-) diff --git a/micras_nav/include/micras/nav/impl/curve_speed.tpp b/micras_nav/include/micras/nav/impl/curve_speed.tpp index 200df49..a0935f6 100644 --- a/micras_nav/include/micras/nav/impl/curve_speed.tpp +++ b/micras_nav/include/micras/nav/impl/curve_speed.tpp @@ -10,7 +10,6 @@ #include #include -#include "micras/nav/curve_speed.hpp" #include "micras/nav/motion_limits.hpp" #include "micras/nav/turn_table.hpp" diff --git a/micras_nav/include/micras/nav/impl/planner.tpp b/micras_nav/include/micras/nav/impl/planner.tpp index 8f94896..e591f33 100644 --- a/micras_nav/include/micras/nav/impl/planner.tpp +++ b/micras_nav/include/micras/nav/impl/planner.tpp @@ -336,10 +336,10 @@ float TPlanner::get_offset(uint8_t arrival) const { template TPlanner::Edge TPlanner::get_edge(uint8_t arrival, uint8_t speed_ratio, uint8_t run, TurnId turn) { - const bool at_full_speed = arrival == rest or speed_ratio == full_speed; - const int16_t pair = edge_pairs.index.at(arrival).at(std::to_underlying(turn)); - float& cached_cost = this->edge_costs.at(pair).at(run); - uint8_t& cached_speed_ratio = this->edge_speed_ratios.at(pair).at(run); + const bool at_full_speed = arrival == rest or speed_ratio == full_speed; + const auto pair = static_cast(edge_pairs.index.at(arrival).at(std::to_underlying(turn))); + float& cached_cost = this->edge_costs.at(pair).at(run); + uint8_t& cached_speed_ratio = this->edge_speed_ratios.at(pair).at(run); if (at_full_speed and cached_cost >= 0.0F) { return {.cost = cached_cost, .speed_ratio = cached_speed_ratio}; diff --git a/micras_nav/include/micras/nav/impl/racing_line.tpp b/micras_nav/include/micras/nav/impl/racing_line.tpp index 2cbf3ba..9531f86 100644 --- a/micras_nav/include/micras/nav/impl/racing_line.tpp +++ b/micras_nav/include/micras/nav/impl/racing_line.tpp @@ -304,7 +304,10 @@ void TRacingLine::solve_window() { continue; } - for (int32_t k = std::max(j - 4, 0); k <= std::min(j + 2, size - 1); k++) { + const auto from = static_cast(std::max(j - 4, 0)); + const auto to = static_cast(std::min(j + 2, size - 1)); + + for (std::size_t k = from; k <= to; k++) { this->slides.at(k) *= 0.5; } } @@ -532,10 +535,10 @@ float TRacingLine::get_heading(uint16_t index) const { template void TRacingLine::solve_slides(std::span normals_x, std::span normals_y) { - const uint16_t first = this->window_start; - const auto size = static_cast(normals_x.size()); - const double length_weight = static_cast(this->config.length_weight) * - static_cast(this->line.spacing) * static_cast(this->line.spacing); + const uint16_t first = this->window_start; + const std::size_t size = normals_x.size(); + const double length_weight = static_cast(this->config.length_weight) * + static_cast(this->line.spacing) * static_cast(this->line.spacing); this->diagonal.fill(0.0); this->first_band.fill(0.0); @@ -553,16 +556,18 @@ void TRacingLine::solve_slides(std::span normals_x, } for (std::size_t k = 0; k < weights.size(); k++) { - const int32_t j = row + static_cast(k) - first; + const int32_t offset = row + static_cast(k) - first; - if (j < 0 or j >= size) { + if (offset < 0 or static_cast(offset) >= size) { continue; } + const auto j = static_cast(offset); + this->linear.at(j) += scale * weights[k] * (base_x * normals_x[j] + base_y * normals_y[j]); for (std::size_t l = k; l < weights.size(); l++) { - const int32_t m = row + static_cast(l) - first; + const std::size_t m = j + (l - k); if (m >= size) { continue; @@ -585,15 +590,17 @@ void TRacingLine::solve_slides(std::span normals_x, constexpr std::array second_difference{1.0, -2.0, 1.0}; constexpr std::array first_difference{-1.0, 1.0}; - for (int32_t row = first - 2; row <= first + size - 1; row++) { + const auto last_row = static_cast(first + size - 1); + + for (int32_t row = first - 2; row <= last_row; row++) { add_row(row, second_difference, 1.0); } - for (int32_t row = first - 1; row <= first + size - 1; row++) { + for (int32_t row = first - 1; row <= last_row; row++) { add_row(row, first_difference, length_weight); } - const auto product = [this, size](int32_t j, std::span values) { + const auto product = [this, size](std::size_t j, std::span values) { double result = this->diagonal.at(j) * values[j]; if (j + 1 < size) { @@ -618,14 +625,14 @@ void TRacingLine::solve_slides(std::span normals_x, const auto objective = [&](std::span values) { double result = 0.0; - for (int32_t j = 0; j < size; j++) { + for (std::size_t j = 0; j < size; j++) { result += values[j] * (product(j, values) + 2.0 * this->linear.at(j)); } return result; }; - const auto coupling = [this](int32_t from, int32_t to) { + const auto coupling = [this](std::size_t from, std::size_t to) { if (to - from == 1) { return this->first_band.at(from); } @@ -635,18 +642,18 @@ void TRacingLine::solve_slides(std::span normals_x, this->slides.fill(0.0); - const std::span current{this->slides.data(), static_cast(size)}; - const std::span candidate{this->trial.data(), static_cast(size)}; + const std::span current{this->slides.data(), size}; + const std::span candidate{this->trial.data(), size}; double value = 0.0; for (uint8_t iteration = 0; iteration < max_newton_iterations; iteration++) { uint8_t free = 0; - for (int32_t j = 0; j < size; j++) { + for (std::size_t j = 0; j < size; j++) { const double slope = product(j, current) + this->linear.at(j); - const double low = this->lower.at(j); - const double high = this->upper.at(j); + const auto low = static_cast(this->lower.at(j)); + const auto high = static_cast(this->upper.at(j)); this->gradient.at(j) = slope; this->direction.at(j) = 0.0; @@ -707,7 +714,7 @@ void TRacingLine::solve_slides(std::span normals_x, this->trial.at(c) = result; } - for (int32_t c = free - 1; c >= 0; c--) { + for (std::size_t c = free; c-- > 0;) { double result = this->trial.at(c) / this->factor_diagonal.at(c); if (c + 1 < free) { @@ -729,7 +736,7 @@ void TRacingLine::solve_slides(std::span normals_x, double decrease = 0.0; moved = 0.0; - for (int32_t j = 0; j < size; j++) { + for (std::size_t j = 0; j < size; j++) { this->trial.at(j) = std::clamp( this->slides.at(j) + step * this->direction.at(j), static_cast(this->lower.at(j)), static_cast(this->upper.at(j)) @@ -766,8 +773,8 @@ bool TRacingLine::is_clear(const core::Vector& position, float co const float half_cell = cell_size / 2.0F; const float half_wall = this->dynamics.get_model().maze.wall_thickness / 2.0F; - const auto post_x = static_cast(2 * std::lround(position.x / cell_size)); - const auto post_y = static_cast(2 * std::lround(position.y / cell_size)); + const int32_t post_x = 2 * static_cast(std::round(position.x / cell_size)); + const int32_t post_y = 2 * static_cast(std::round(position.y / cell_size)); const core::Vector post{.x = static_cast(post_x) * half_cell, .y = static_cast(post_y) * half_cell}; diff --git a/micras_nav/include/micras/nav/racing_line.hpp b/micras_nav/include/micras/nav/racing_line.hpp index f65e69d..fc48e96 100644 --- a/micras_nav/include/micras/nav/racing_line.hpp +++ b/micras_nav/include/micras/nav/racing_line.hpp @@ -349,7 +349,7 @@ class TRacingLine { /** * @brief Walls the line may cross, one bit per point of the lattice. */ - std::array openings{}; + std::array(lattice_width) * lattice_height) + 7) / 8> openings{}; /** * @brief Map of the walls, while the line is optimized. diff --git a/micras_nav/include/micras/nav/turn_table.hpp b/micras_nav/include/micras/nav/turn_table.hpp index 05bf981..7e2ead2 100644 --- a/micras_nav/include/micras/nav/turn_table.hpp +++ b/micras_nav/include/micras/nav/turn_table.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -358,7 +359,7 @@ class TurnTable { this->shapes.at(i) = designer.design(static_cast(i)); } - std::ranges::copy(two_bend_shapes, this->shapes.begin() + first_two_bend_turn); + std::ranges::copy(two_bend_shapes, std::next(this->shapes.begin(), first_two_bend_turn)); } /** @@ -529,8 +530,8 @@ class TurnTable { } const Layout layout = this->get_layout(get_primitive(turn)); - const double total = shape.total_length(); - const auto samples = static_cast(total / sample_spacing) + 2; + const auto total = static_cast(shape.total_length()); + const auto samples = static_cast((total / sample_spacing) + 2.0); for (uint16_t i = 0; i < samples; i++) { if (not this->is_clear(get_placement(shape, layout, total * i / (samples - 1)), layout)) { @@ -561,19 +562,22 @@ class TurnTable { return {}; } - const double first_angle = design.first_angle; - const double second_angle = design.second_angle; + const auto first_angle = static_cast(design.first_angle); + const auto second_angle = static_cast(design.second_angle); const double turned = (first_angle + second_angle) / (std::numbers::pi / 4.0) - primitive.rotation; const double revolutions = turned / 8.0; - const double rounded = revolutions < 0.0 ? -static_cast(static_cast(0.5 - revolutions)) : - static_cast(static_cast(revolutions + 0.5)); + // NOLINTNEXTLINE(bugprone-incorrect-roundings) lround is not constexpr, and the value is positive + const auto whole = static_cast(static_cast(core::math::abs(revolutions) + 0.5)); + const double rounded = revolutions < 0.0 ? -whole : whole; if (core::math::abs(turned - 8.0 * rounded) > 1.0e-4) { return {}; } - const Candidate first = this->make_curve(core::math::abs(first_angle), design.first_curvature); - const Candidate second = this->make_curve(core::math::abs(second_angle), design.second_curvature); + const Candidate first = + this->make_curve(core::math::abs(first_angle), static_cast(design.first_curvature)); + const Candidate second = + this->make_curve(core::math::abs(second_angle), static_cast(design.second_curvature)); TurnShape shape{}; shape.bends.at(0) = to_bend(first, first_angle); @@ -597,11 +601,11 @@ class TurnTable { const auto first_end = shape.bends.at(0).template sample(1.0e3); const auto second_end = shape.bends.at(1).template sample(1.0e3); const double remaining_x = - layout.exit_x - shape.pre * core::math::cos(entry) - + layout.exit_x - static_cast(shape.pre) * core::math::cos(entry) - (core::math::cos(entry) * first_end.x - core::math::sin(entry) * first_end.y) - (core::math::cos(middle) * second_end.x - core::math::sin(middle) * second_end.y); const double remaining_y = - layout.exit_y - shape.pre * core::math::sin(entry) - + layout.exit_y - static_cast(shape.pre) * core::math::sin(entry) - (core::math::sin(entry) * first_end.x + core::math::cos(entry) * first_end.y) - (core::math::sin(middle) * second_end.x + core::math::cos(middle) * second_end.y); @@ -823,8 +827,8 @@ class TurnTable { static constexpr Placement get_placement(const TurnShape& shape, const Layout& layout, double distance) { const double cosine = core::math::cos(layout.entry_heading); const double sine = core::math::sin(layout.entry_heading); - const double pre = shape.pre; - const double length = shape.length(); + const auto pre = static_cast(shape.pre); + const auto length = static_cast(shape.length()); double local_x = distance; double local_y = 0.0; diff --git a/micras_nav/src/curve_speed.cpp b/micras_nav/src/curve_speed.cpp index 04cdfc4..a21285d 100644 --- a/micras_nav/src/curve_speed.cpp +++ b/micras_nav/src/curve_speed.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include "micras/nav/curve_speed.hpp" @@ -109,10 +110,12 @@ void CurveSpeed::integrate(std::span speeds, float spacing, std::sp return; } - times[0] = 0.0F; + times.front() = 0.0F; - for (std::size_t i = 1; i < times.size(); i++) { - times[i] = times[i - 1] + 2.0F * spacing / std::max(speeds[i - 1] + speeds[i], min_speed_sum); + auto speed = speeds.begin(); + + for (auto time = times.begin(); std::next(time) != times.end(); ++time, ++speed) { + *std::next(time) = *time + 2.0F * spacing / std::max(*speed + *std::next(speed), min_speed_sum); } } @@ -128,10 +131,12 @@ SpeedProfile::Sample std::size_t index = next == times.begin() ? 0 : static_cast(next - times.begin()) - 1; index = std::min(index, times.size() - 2); - const float start = speeds[index]; - const float end = speeds[index + 1]; - const float acceleration = (end * end - start * start) / (2.0F * spacing); - const float elapsed = std::min(time - times[index], times[index + 1] - times[index]); + const std::span interval_speeds = speeds.subspan(index, 2); + const std::span interval_times = times.subspan(index, 2); + const float start = interval_speeds.front(); + const float end = interval_speeds.back(); + const float acceleration = (end * end - start * start) / (2.0F * spacing); + const float elapsed = std::min(time - interval_times.front(), interval_times.back() - interval_times.front()); return { .distance = static_cast(index) * spacing + From 4784abad5a1630aad87b0d51799f4be7300a6e4d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 26 Sep 2026 22:49:32 -0300 Subject: [PATCH 515/561] =?UTF-8?q?=E2=AC=86=EF=B8=8F=20Follow=20the=20fir?= =?UTF-8?q?mware=20onto=20the=20merged=20low=20level=20review?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The firmware's high-level-review is rebased onto feature/bluetooth-link, which follows main with the low level review merged, and builds, tests and lints as the firmware's CI does. The host Mcu answers the two questions the initialization now asks: no reset came from the watchdog, since a simulated run always starts cold and counts a watchdog expiry instead of acting on it, and the core runs within its datasheet. The turns are designed in the firmware's config/dynamics_config.cpp, which the target now compiles. The checked runs drive exactly as before; the battery voltage column follows the 4 Hz filter of the low level review. --- micras_hal/host/src/mcu.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/micras_hal/host/src/mcu.cpp b/micras_hal/host/src/mcu.cpp index cf3543e..00d48dd 100644 --- a/micras_hal/host/src/mcu.cpp +++ b/micras_hal/host/src/mcu.cpp @@ -74,4 +74,12 @@ void Mcu::refresh_watchdog() { mcu.last_refresh = clock.now(); } + +bool Mcu::was_reset_by_watchdog() { + return false; +} + +bool Mcu::is_cpu_frequency_supported() { + return true; +} } // namespace micras::hal From 93c3b354365b7e52c3770c701548fb2335c9eee1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Sep 2026 01:14:55 -0300 Subject: [PATCH 516/561] =?UTF-8?q?=F0=9F=90=9B=20Restart=20a=20converter?= =?UTF-8?q?=20that=20an=20overrun=20stopped?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit After an overrun the ADC stops requesting DMA transfers until its overrun flag is cleared, and nothing cleared it: no ADC interrupt was enabled, so an overrun froze that converter's buffer for good while was_initialized() stayed true. The wall sensors then marked every reading as old and the robot drove on odometry alone, and the battery and torque readings stayed where they were. - The ADC interrupts are enabled in both CubeMX projects. HAL_ADC_Start_DMA enables only the overrun interrupt, so they fire on an overrun and never on a conversion. - The error callback marks the converter, and the owner's next update restarts it outside the interrupt, since stopping and starting it waits on the hardware. Clearing only the flag would leave one conversion missing and every following value in the place of another channel; a restart begins the buffer again at its first channel. A transfer error of the DMA reaches the same callback, and the error state it leaves in the vendor HAL is cleared before the start. - Every converter registers itself when it is constructed, not only the wall sensors', so the interrupts find all three and every one counts its sequences. - adc_restarts in the variable pool counts the restarts since power on; anything but zero means a converter stopped. Closes #57 --- micras_hal/include/micras/hal/adc_dma.hpp | 69 +++++++++++++++++- micras_hal/stm32/src/adc_dma.cpp | 73 +++++++++++++++---- micras_proxy/include/micras/proxy/battery.hpp | 2 + .../micras/proxy/impl/torque_sensors.tpp | 2 + .../micras/proxy/impl/wall_sensors.tpp | 2 + .../include/micras/proxy/torque_sensors.hpp | 2 + .../include/micras/proxy/wall_sensors.hpp | 3 +- micras_proxy/src/battery.cpp | 1 + 8 files changed, 134 insertions(+), 20 deletions(-) diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 44942d3..1da2ce6 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -32,6 +32,9 @@ class AdcDma { /** * @brief Construct a new AdcDma object. * + * @note The object registers itself for the interrupts to find it, and a converter that finds + * no free place is not initialized. + * * @param config ADC DMA configuration struct. */ explicit AdcDma(const Config& config); @@ -100,6 +103,23 @@ class AdcDma { */ void stop_dma(); + /** + * @brief Restart the conversions if an error stopped them. + * + * @details After an overrun the converter stops requesting transfers until its overrun flag is + * cleared, and clearing only the flag would leave one conversion missing and every following + * value in the place of another channel. A restart begins the buffer again at its first + * channel. The error callback only marks the converter, since stopping and starting it waits on + * the hardware, and this restarts it outside the interrupt. A transfer error of the DMA calls + * the error callback too, and leaves the vendor HAL in an error state that only a new + * initialization clears and that turns every complete transfer into another error, so that + * state is cleared before the start. + * + * @note To be called once per iteration by the owner of the converter. No sequence completes + * until then, so read_snapshot reports none as new. + */ + void recover(); + /** * @brief Get the maximum reading of the ADC. * @@ -125,7 +145,16 @@ class AdcDma { bool was_initialized() const; /** - * @brief Copy the buffer of the converter that completed a sequence to its snapshot. + * @brief Get the number of times any converter was restarted. + * + * @note Anything but zero means that a converter stopped, and how often. + * + * @return Number of restarts since power on. + */ + static uint32_t get_restarts(); + + /** + * @brief Count a complete sequence of a converter and copy its buffer to its snapshot. * * @note To be called by the conversion complete callback only. * @@ -133,17 +162,39 @@ class AdcDma { */ static void on_sequence_complete(const ADC_HandleTypeDef* handle); + /** + * @brief Mark a converter that an error stopped, for recover to restart it. + * + * @note To be called by the error callback only. + * + * @param handle Handle of the converter that stopped. + */ + static void on_error(const ADC_HandleTypeDef* handle); + private: /** - * @brief Largest number of converters that can keep a snapshot. + * @brief Find the object of a converter. + * + * @param handle Handle of the converter. + * @return Object of the converter, or nullptr if none was constructed for it. + */ + static AdcDma* find(const ADC_HandleTypeDef* handle); + + /** + * @brief Largest number of converters. */ static constexpr uint8_t max_instances{4}; /** - * @brief Converters that keep a snapshot, for the interrupt to find the one that completed. + * @brief Every converter, for the interrupts to find the one that completed or stopped. */ static std::array instances; + /** + * @brief Number of times any converter was restarted. + */ + static uint32_t restarts; + /** * @brief Maximum ADC reading. */ @@ -160,7 +211,12 @@ class AdcDma { ADC_HandleTypeDef* handle; /** - * @brief Destination buffer of the DMA. + * @brief Destination of the DMA as the vendor HAL takes it, whose size is the number of transfers. + */ + std::span transfer; + + /** + * @brief Destination buffer of the DMA, of a converter that keeps a snapshot. */ std::span buffer; @@ -174,6 +230,11 @@ class AdcDma { */ volatile uint32_t sequence{}; + /** + * @brief Flag set by the error callback when an error stopped the converter. + */ + volatile bool stopped{}; + /** * @brief Flag to check if the ADC was calibrated and started. */ diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 25e0cdc..2f55e3c 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -21,10 +21,21 @@ extern "C" { void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) { micras::hal::AdcDma::on_sequence_complete(hadc); } + +/** + * @brief Callback of the vendor HAL for an overrun of a converter or a transfer error of its DMA. + * + * @param hadc Handle of the converter. + */ +// NOLINTNEXTLINE(readability-identifier-naming) the name is fixed by the vendor HAL +void HAL_ADC_ErrorCallback(ADC_HandleTypeDef* hadc) { + micras::hal::AdcDma::on_error(hadc); +} } namespace micras::hal { std::array AdcDma::instances{}; +uint32_t AdcDma::restarts{}; AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { @@ -40,6 +51,13 @@ AdcDma::AdcDma(const Config& config) : #error "ADC calibration is only supported for STM32H7 and STM32G4 platforms." #endif + auto* const slot = std::ranges::find(instances, nullptr); + + if (slot == instances.end()) { + return; + } + + *slot = this; this->initialized = status == HAL_OK; } @@ -48,6 +66,8 @@ AdcDma::~AdcDma() { } bool AdcDma::start_dma(std::span buffer) { + this->transfer = buffer; + if (HAL_ADC_Start_DMA(this->handle, buffer.data(), buffer.size()) != HAL_OK) { this->initialized = false; return false; @@ -69,15 +89,6 @@ bool AdcDma::start_dma(std::span buffer, std::span snapshot) this->buffer = buffer; this->snapshot = snapshot; - auto* const slot = std::ranges::find(instances, nullptr); - - if (slot == instances.end()) { - this->initialized = false; - return false; - } - - *slot = this; - return this->start_dma(buffer); } @@ -101,19 +112,51 @@ uint32_t AdcDma::read_snapshot(std::span destination) const { } void AdcDma::on_sequence_complete(const ADC_HandleTypeDef* handle) { - for (AdcDma* instance : instances) { - if (instance != nullptr and instance->handle == handle) { - std::ranges::copy(instance->buffer, instance->snapshot.begin()); - instance->sequence = instance->sequence + 1; - return; - } + AdcDma* const instance = find(handle); + + if (instance == nullptr) { + return; + } + + std::ranges::copy(instance->buffer, instance->snapshot.begin()); + instance->sequence = instance->sequence + 1; +} + +void AdcDma::on_error(const ADC_HandleTypeDef* handle) { + AdcDma* const instance = find(handle); + + if (instance != nullptr) { + instance->stopped = true; } } +AdcDma* AdcDma::find(const ADC_HandleTypeDef* handle) { + const auto* const found = std::ranges::find_if(instances, [handle](const AdcDma* instance) { + return instance != nullptr and instance->handle == handle; + }); + + return found == instances.end() ? nullptr : *found; +} + void AdcDma::stop_dma() { HAL_ADC_Stop_DMA(this->handle); } +void AdcDma::recover() { + if (not this->stopped) { + return; + } + + HAL_ADC_Stop_DMA(this->handle); + this->handle->State = this->handle->State & ~(HAL_ADC_STATE_ERROR_DMA | HAL_ADC_STATE_ERROR_INTERNAL); + this->stopped = HAL_ADC_Start_DMA(this->handle, this->transfer.data(), this->transfer.size()) != HAL_OK; + restarts++; +} + +uint32_t AdcDma::get_restarts() { + return restarts; +} + uint16_t AdcDma::get_max_reading() const { return this->max_reading; } diff --git a/micras_proxy/include/micras/proxy/battery.hpp b/micras_proxy/include/micras/proxy/battery.hpp index ce3a56f..0244a06 100644 --- a/micras_proxy/include/micras/proxy/battery.hpp +++ b/micras_proxy/include/micras/proxy/battery.hpp @@ -34,6 +34,8 @@ class Battery { /** * @brief Update the battery reading. + * + * @note Restarts the converter first if an error stopped it. */ void update(); diff --git a/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp b/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp index b033e0f..64889a7 100644 --- a/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp +++ b/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp @@ -28,6 +28,8 @@ void TTorqueSensors::calibrate() { template void TTorqueSensors::update() { + this->adc.recover(); + for (uint8_t i = 0; i < num_of_sensors; i++) { this->filters.at(i).update(this->get_adc_reading(i)); } diff --git a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp index 0fb240c..77d3298 100644 --- a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp +++ b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp @@ -115,6 +115,8 @@ void TWallSensors::turn_off() { template void TWallSensors::update() { + this->adc.recover(); + const uint32_t current_sequence = this->adc.read_snapshot(this->scans); const bool is_new = current_sequence != this->sequence; diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 48a5f6d..824a18c 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -46,6 +46,8 @@ class TTorqueSensors { /** * @brief Update the torque sensors readings. + * + * @note Restarts the converter first if an error stopped it. */ void update(); diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index c81eebf..2456c40 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -101,7 +101,8 @@ class TWallSensors { * @brief Update the wall sensors readings. * * @note Nothing is recomputed unless the converter completed a sequence since the last call, - * so this can be called faster than the sensors produce values. + * so this can be called faster than the sensors produce values. Restarts the converter first if + * an error stopped it. */ void update(); diff --git a/micras_proxy/src/battery.cpp b/micras_proxy/src/battery.cpp index 57f443a..5a50a01 100644 --- a/micras_proxy/src/battery.cpp +++ b/micras_proxy/src/battery.cpp @@ -12,6 +12,7 @@ Battery::Battery(const Config& config) : initialized{this->adc.start_dma({&(this->raw_reading), 1}) and this->adc.was_initialized()} { } void Battery::update() { + this->adc.recover(); this->filter.update(this->get_adc_reading()); } From a3a4bc84a763c86f9940e363bebdd973db449560 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Sep 2026 01:21:59 -0300 Subject: [PATCH 517/561] =?UTF-8?q?=E2=AC=86=EF=B8=8F=20Follow=20the=20fir?= =?UTF-8?q?mware's=20restart=20of=20a=20stopped=20converter?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The firmware now restarts an ADC that an overrun stopped (Team-Micras/MicrasFirmware#57). The host AdcDma follows its header: every converter registers itself when it is constructed, the error callback's mark and recover() exist, and get_restarts() reads the count, which no host converter ever moves. adc_restarts is a new variable column, and announcing it on the link costs 15 bytes of credit, so link_credit starts at 51 instead of 66; every other column of the reference runs is byte identical. --- micras_hal/host/src/adc_dma.cpp | 59 ++++++++++++++++++++++++--------- 1 file changed, 44 insertions(+), 15 deletions(-) diff --git a/micras_hal/host/src/adc_dma.cpp b/micras_hal/host/src/adc_dma.cpp index 15c8eb2..1a6761b 100644 --- a/micras_hal/host/src/adc_dma.cpp +++ b/micras_hal/host/src/adc_dma.cpp @@ -12,6 +12,7 @@ namespace micras::hal { std::array AdcDma::instances{}; +uint32_t AdcDma::restarts{}; AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, reference_voltage{config.reference_voltage}, handle{config.handle} { @@ -19,6 +20,13 @@ AdcDma::AdcDma(const Config& config) : config.init_function(); } + auto* const slot = std::ranges::find(instances, nullptr); + + if (slot == instances.end()) { + return; + } + + *slot = this; this->initialized = this->handle->State == HAL_ADC_STATE_READY; } @@ -53,15 +61,6 @@ bool AdcDma::start_dma(std::span buffer, std::span snapshot) this->buffer = buffer; this->snapshot = snapshot; - auto* const slot = std::ranges::find(instances, nullptr); - - if (slot == instances.end()) { - this->initialized = false; - return false; - } - - *slot = this; - return this->start_dma(buffer); } @@ -82,15 +81,32 @@ uint32_t AdcDma::read_snapshot(std::span destination) const { } void AdcDma::on_sequence_complete(const ADC_HandleTypeDef* handle) { - for (AdcDma* instance : instances) { - if (instance != nullptr and instance->handle == handle) { - std::ranges::copy(instance->buffer, instance->snapshot.begin()); - instance->sequence = instance->sequence + 1; - return; - } + AdcDma* const instance = find(handle); + + if (instance == nullptr) { + return; + } + + std::ranges::copy(instance->buffer, instance->snapshot.begin()); + instance->sequence = instance->sequence + 1; +} + +void AdcDma::on_error(const ADC_HandleTypeDef* handle) { + AdcDma* const instance = find(handle); + + if (instance != nullptr) { + instance->stopped = true; } } +AdcDma* AdcDma::find(const ADC_HandleTypeDef* handle) { + const auto* const found = std::ranges::find_if(instances, [handle](const AdcDma* instance) { + return instance != nullptr and instance->handle == handle; + }); + + return found == instances.end() ? nullptr : *found; +} + void AdcDma::stop_dma() { host::AdcPort& port = host::Board::adc(this->handle); port.buffer16 = {}; @@ -98,6 +114,19 @@ void AdcDma::stop_dma() { port.complete = nullptr; } +void AdcDma::recover() { + if (not this->stopped) { + return; + } + + this->stopped = false; + restarts++; +} + +uint32_t AdcDma::get_restarts() { + return restarts; +} + uint16_t AdcDma::get_max_reading() const { return this->max_reading; } From f6df2b1ee6231dac1469fb957e22fa07ebc99d58 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Sep 2026 06:03:33 -0300 Subject: [PATCH 518/561] =?UTF-8?q?=F0=9F=90=9B=20Answer=20the=20review=20?= =?UTF-8?q?of=20the=20navigation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The review of #54 found the robot would reset forever at boot, drive a motor at full supply indefinitely when held, keep a practice maze forever, and a handful of smaller faults. What changed, and the choices behind it: - The watchdog starts with the stopped timeout, and the constructor drops it to the loop's once every member is built: the IMU alone waits 40 ms. - Motors saturated for 100 ms, the IMU silent for 5 ms, or a crash stop the robot in ERROR. The error clamp of the controller never bounded the output, and its note said it did. - The first search after a reset clears the map restored from the flash; a fast run still uses it. The run profile is no longer saved, so at boot it is what the switches say. - After a run of rejections with the robot moving, the localizer lets the position loose so that it can converge to the walls again. - The time scale is solved directly, as the largest at which the feed forward of both wheels fits in the voltage, from the reference at the full pace. It no longer iterates from the previous scale, which oscillated, and the reference gets the rate of the scale times the speed. - The planner prices each turn with a factor for the speed it gains where its curvature allows, kept per profile, in the edge costs, the dominance test and the gaps alike. - An explorer round runs to its end when the map changes, so that one ends at all while the robot moves; only a round on an unchanged map proves the search over. - A straight into a cell whose front wall is unknown is driven at the search speed, which makes the stop at the commit point exact. - The executor drops finished segments, so a long search never reallocates. - The ADC publishes nothing while a converter is stopped, the whole plan runs under the extended watchdog, and the tick restarts after a save and the boot, which stop counting as missed ticks. - The LED is on while the flash is written, a failed save is counted, and the link's save reports it. - Drive identification ends a step once the robot would coast past its limits. - The current is read signed about the midpoint of its amplifiers, zeroed before every run. - The flags of a variable are packed into a defined byte: the bit cast carried four undefined bits into the schema hash and onto the link. The host tests run in CI, which is how this showed up. - The calibration of the side wall sensors wants a corridor two cells long, as the README now says. --- micras_comm/src/link.cpp | 2 +- .../include/micras/core/variable_pool.hpp | 15 +++ micras_core/src/variable_pool.cpp | 3 +- micras_hal/include/micras/hal/adc_dma.hpp | 4 +- micras_hal/stm32/src/adc_dma.cpp | 2 +- micras_nav/include/micras/nav/controller.hpp | 47 +++++++-- .../micras/nav/drive_identification.hpp | 11 ++- micras_nav/include/micras/nav/executor.hpp | 11 ++- micras_nav/include/micras/nav/explorer.hpp | 6 +- .../include/micras/nav/impl/explorer.tpp | 9 +- .../include/micras/nav/impl/localizer.tpp | 7 +- .../include/micras/nav/impl/mission.tpp | 18 +++- .../include/micras/nav/impl/planner.tpp | 78 ++++++++++++--- micras_nav/include/micras/nav/localizer.hpp | 25 ++++- micras_nav/include/micras/nav/mission.hpp | 10 +- .../include/micras/nav/motion_limits.hpp | 8 ++ micras_nav/include/micras/nav/planner.hpp | 57 ++++++++++- micras_nav/include/micras/nav/segment.hpp | 4 +- micras_nav/include/micras/nav/wall_model.hpp | 6 +- micras_nav/src/controller.cpp | 95 +++++++++++++++++-- micras_nav/src/drive_identification.cpp | 4 +- micras_nav/src/executor.cpp | 13 ++- micras_nav/src/localizer.cpp | 26 +++++ micras_nav/src/motion_limits.cpp | 6 ++ micras_nav/src/route_compiler.cpp | 3 + micras_nav/src/velocity_planner.cpp | 15 ++- .../micras/proxy/impl/torque_sensors.tpp | 1 + micras_proxy/include/micras/proxy/tick.hpp | 8 ++ .../include/micras/proxy/torque_sensors.hpp | 14 ++- micras_proxy/src/tick.cpp | 4 + 30 files changed, 437 insertions(+), 75 deletions(-) diff --git a/micras_comm/src/link.cpp b/micras_comm/src/link.cpp index d37f571..7969e2e 100644 --- a/micras_comm/src/link.cpp +++ b/micras_comm/src/link.cpp @@ -331,7 +331,7 @@ void Link::send_schema_page() { } writer.u8(std::to_underlying(variable.type)); - writer.u8(std::bit_cast(variable.access)); + writer.u8(variable.access.to_byte()); writer.u8(name_size); writer.text(variable.prefix); writer.text(variable.name); diff --git a/micras_core/include/micras/core/variable_pool.hpp b/micras_core/include/micras/core/variable_pool.hpp index 4cd7793..3513c78 100644 --- a/micras_core/include/micras/core/variable_pool.hpp +++ b/micras_core/include/micras/core/variable_pool.hpp @@ -140,6 +140,21 @@ struct Access { * @brief Whether the variable is part of the flash image. */ bool persist : 1 {}; + + /** + * @brief Get the flags as one byte, the first one in the lowest bit. + * + * @note The byte of the struct itself also holds four bits no field owns, whose value is not + * defined, so it is never copied as it is. + * + * @return The flags. + */ + constexpr uint8_t to_byte() const { + return static_cast( + static_cast(this->stream) | static_cast(this->write) << 1U | + static_cast(this->idle) << 2U | static_cast(this->persist) << 3U + ); + } }; /** diff --git a/micras_core/src/variable_pool.cpp b/micras_core/src/variable_pool.cpp index 092859f..beb0a29 100644 --- a/micras_core/src/variable_pool.cpp +++ b/micras_core/src/variable_pool.cpp @@ -2,7 +2,6 @@ * @file */ -#include #include #include #include @@ -131,7 +130,7 @@ uint32_t VariablePool::schema_hash() const { hash = mix(hash, variable.prefix); hash = mix(hash, variable.name); hash = mix(hash, std::to_underlying(variable.type)); - hash = mix(hash, std::bit_cast(variable.access)); + hash = mix(hash, variable.access.to_byte()); } return hash; diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 1da2ce6..1428455 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -156,7 +156,9 @@ class AdcDma { /** * @brief Count a complete sequence of a converter and copy its buffer to its snapshot. * - * @note To be called by the conversion complete callback only. + * @note To be called by the conversion complete callback only. Nothing is counted while the + * converter is stopped: after an overrun the vendor HAL clears the flag and the transfers go + * on out of line with the sequence until the converter is restarted. * * @param handle Handle of the converter that completed a sequence. */ diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 2f55e3c..e29b1ee 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -114,7 +114,7 @@ uint32_t AdcDma::read_snapshot(std::span destination) const { void AdcDma::on_sequence_complete(const ADC_HandleTypeDef* handle) { AdcDma* const instance = find(handle); - if (instance == nullptr) { + if (instance == nullptr or instance->stopped) { return; } diff --git a/micras_nav/include/micras/nav/controller.hpp b/micras_nav/include/micras/nav/controller.hpp index 32ee2d2..d23c66d 100644 --- a/micras_nav/include/micras/nav/controller.hpp +++ b/micras_nav/include/micras/nav/controller.hpp @@ -40,8 +40,10 @@ class Controller { /** * @brief Closed loop behavior asked of one axis. * - * @note The natural frequency is in rad/s. The error is clamped before it is used, which bounds - * what the feedback can ask for when the robot is held or has crashed. + * @note The natural frequency is in rad/s. The error is clamped before it is used, which keeps a + * large error from swamping the rest of the command. It does not keep the motors from + * saturating when the robot is held, since the proportional term alone can reach the supply + * within the clamp; the robot stops on the saturation timeout for that. */ struct Axis { float natural_frequency; @@ -55,7 +57,9 @@ class Controller { * @note The steering gain is in radians of offset per meter of error and the offset is limited * to the largest steering. Below the blend speed the steering fades out, since turning in place * cannot reduce an error across the path. The friction speed is the wheel speed over which the - * static friction compensation goes from nothing to all of it. + * static friction compensation goes from nothing to all of it. The time scale may change by at + * most the largest time scale rate per second, since a change of the time scale is itself an + * acceleration of the reference, of that rate times the speed. */ struct Config { RobotModel model; @@ -66,6 +70,7 @@ class Controller { float steering_blend_speed; float friction_speed; float voltage_reserve; + float max_time_scale_rate; }; /** @@ -99,21 +104,27 @@ class Controller { /** * @brief Compute the command for this instant. * - * @param reference What the robot should be doing, in the maze frame. + * @note The reference is played at the time scale found for it, which scales its speeds by the + * scale and its accelerations by the square of the scale, plus the rate of the scale times the + * speed. + * + * @param unscaled What the robot should be doing, in the maze frame, at the full pace. * @param estimate What the robot is doing, as far as it is known. + * @param elapsed_time Time since the previous update, in seconds. * @return The command for the locomotion. */ - Command update(const Reference& reference, const State& estimate); + Command update(const Reference& unscaled, const State& estimate, float elapsed_time); /** * @brief Get how much slower the reference has to be played for the motors to follow it. * * @note The feed forward alone may ask for more voltage than there is. Slowing the clock of * the reference scales its linear and angular speeds together, so the robot stays on the same - * path and only takes longer, instead of cutting the turn it is in. With the limits of a run - * derived from the same model this should stay at one. The reference was already played at the - * scale in force, so the scale is updated from that one and settles where the demand meets the - * voltage available. + * path and only takes longer, instead of cutting the turn it is in. While the grip limits the + * planned motions this stays at one; where the motors limit them, a straight uses the whole of + * the voltage on the forward axis, and any rotation on top needs it. The scale is the largest + * one at which the feed forward of both wheels fits in the voltage available, found from the + * reference at the full pace, so it does not depend on the scale of the previous iteration. * * @return The factor to apply to the elapsed time of the reference, in (0, 1]. */ @@ -145,6 +156,18 @@ class Controller { */ static Gains compute_gains(const Axis& axis, float speed_constant, float acceleration_constant); + /** + * @brief Find the largest time scale at which the feed forward fits in the voltage available. + * + * @note The feed forward of each wheel is a quadratic in the scale: the friction does not scale, + * the speed term scales with it and the acceleration term with its square. So the scale is the + * largest root in (0, 1] of the wheels reaching the limit, or one if they never do. + * + * @param reference What the robot should be doing, at the full pace. + * @return The time scale, in (0, 1]. + */ + float find_time_scale(const Reference& reference) const; + /** * @brief Parameters of the controller, with the physical description of the robot. */ @@ -165,6 +188,12 @@ class Controller { */ float time_scale{1.0F}; + /** + * @brief Smallest time scale, which is only reached if the friction alone takes more than the + * voltage available. + */ + static constexpr float min_time_scale{0.1F}; + /** * @brief Errors and terms of the last update. */ diff --git a/micras_nav/include/micras/nav/drive_identification.hpp b/micras_nav/include/micras/nav/drive_identification.hpp index a413192..6384196 100644 --- a/micras_nav/include/micras/nav/drive_identification.hpp +++ b/micras_nav/include/micras/nav/drive_identification.hpp @@ -28,8 +28,10 @@ namespace micras::nav { * The constants come out in volts, and get_model() turns them into the physical constants of the * robot model, so the result of a run is a set of numbers to type into the configuration. * - * @note The robot travels forward by about the distance limit and comes back, so it needs that much - * free floor ahead of it. It should run with the fan as it is used in a fast run. + * @note The robot stays between where it started and the distance limit ahead of it, as far as the + * model it starts from is right: a step ends early enough for the robot to coast to a stop inside + * that stretch. It needs that much free floor ahead of it, and some margin on both sides for a + * model that is off. It should run with the fan as it is used in a fast run. */ class DriveIdentification { public: @@ -38,8 +40,9 @@ class DriveIdentification { * * @note The commands are in percent of the motor supply. The ramp rate is in percent per * second, and the wheels count as turning once they are faster than the breakaway speed. Each - * level of a step lasts for the step time, a step forward or backward also ends when the - * distance limit is reached, and the robot rests for the rest time between two steps. + * level of a step lasts for the step time, a step forward also ends once the robot would coast + * past the distance limit and a step backward once it would coast past where it started, and the + * robot rests for the rest time between two steps. */ struct Config { RobotModel model; diff --git a/micras_nav/include/micras/nav/executor.hpp b/micras_nav/include/micras/nav/executor.hpp index 5ed4836..09d03c1 100644 --- a/micras_nav/include/micras/nav/executor.hpp +++ b/micras_nav/include/micras/nav/executor.hpp @@ -63,9 +63,9 @@ class Executor { /** * @brief Queue segments after the ones already queued. * - * @note Nothing is allocated as long as the number of segments waiting stays within the - * capacity, which a route planned with the robot stopped may exceed and the few segments - * queued per cell while exploring never do. + * @note The segments already done are dropped first, so nothing is allocated as long as the + * number of segments waiting stays within the capacity, which a route planned with the robot + * stopped may exceed and the few segments queued per cell while exploring never do. * * @param segments The segments, with their speeds already planned. */ @@ -75,12 +75,13 @@ class Executor { * @brief Advance the clock and get the reference for this instant. * * @note A time scale below one plays the reference in slow motion: the clock advances by less - * than the time that passed and the speeds are scaled to match, so the path stays the same. + * than the time that passed. The reference comes out at the full pace, and the controller + * scales its speeds to match, so the path stays the same. * * @param elapsed_time Time since the last update, in seconds. * @param time_scale Factor applied to the elapsed time, in (0, 1]. * @param estimate The estimated state of the robot, which only tells when it has settled. - * @return The reference in the maze frame. + * @return The reference in the maze frame, at the full pace. */ Reference update(float elapsed_time, float time_scale, const State& estimate); diff --git a/micras_nav/include/micras/nav/explorer.hpp b/micras_nav/include/micras/nav/explorer.hpp index acbcc60..e97098f 100644 --- a/micras_nav/include/micras/nav/explorer.hpp +++ b/micras_nav/include/micras/nav/explorer.hpp @@ -29,7 +29,11 @@ namespace micras::nav { * map has to be good for and the targets are put together. * * @note The planner is advanced a bounded number of edges per call, which lets all of this happen - * while the robot is moving. + * while the robot is moving. A round of every profile takes seconds, and a wall is decided about + * once per cell, so a round is not restarted when the map changes: it runs to its end and the next + * one starts from the new map, which keeps the targets at most one round old. The planner reads the + * map as it goes, so a round the map changed during may name targets but never proves the search + * over; only a round on an unchanged map does. * * @tparam width The width of the maze in cells. * @tparam height The height of the maze in cells. diff --git a/micras_nav/include/micras/nav/impl/explorer.tpp b/micras_nav/include/micras/nav/impl/explorer.tpp index c7cfcb0..944db45 100644 --- a/micras_nav/include/micras/nav/impl/explorer.tpp +++ b/micras_nav/include/micras/nav/impl/explorer.tpp @@ -36,11 +36,6 @@ bool TExplorer::update(const TMaze& maze, uint32_t return false; } - if (this->planning and maze.get_revision() != this->round_revision) { - this->planning = false; - this->profile_index = 0; - } - if (not this->planning) { if (this->answered and maze.get_revision() == this->answered_revision) { return false; @@ -87,6 +82,10 @@ bool TExplorer::update(const TMaze& maze, uint32_t this->profile_index = 0; + if (this->number_collected == 0 and maze.get_revision() != this->round_revision) { + return false; + } + const bool changed = not this->answered or this->number_collected != this->number_of_targets or not std::equal( diff --git a/micras_nav/include/micras/nav/impl/localizer.tpp b/micras_nav/include/micras/nav/impl/localizer.tpp index e75d78a..f498b6f 100644 --- a/micras_nav/include/micras/nav/impl/localizer.tpp +++ b/micras_nav/include/micras/nav/impl/localizer.tpp @@ -28,7 +28,8 @@ void Localizer::correct( return; } - const float gate = this->is_stationary() ? this->config.stationary_gate : this->config.gate; + const bool stationary = this->is_stationary(); + const float gate = stationary ? this->config.stationary_gate : this->config.gate; for (uint8_t sensor = 0; sensor < number_of_wall_sensors; sensor++) { const WallReading& reading = measurements.walls.at(sensor); @@ -57,11 +58,13 @@ void Localizer::correct( const float deviation = wall_model.get_range_deviation(hit.range); - this->update( + const bool accepted = this->update( wall_model.get_range(reading.distance, sensor, hit) - hit.range, {hit.jacobian.at(0), hit.jacobian.at(1), hit.jacobian.at(2), 0.0F}, deviation * deviation * this->config.range_correlation, gate, this->config.max_position_correction ); + + this->count_range(accepted, stationary); } } diff --git a/micras_nav/include/micras/nav/impl/mission.tpp b/micras_nav/include/micras/nav/impl/mission.tpp index b9546c1..e77836f 100644 --- a/micras_nav/include/micras/nav/impl/mission.tpp +++ b/micras_nav/include/micras/nav/impl/mission.tpp @@ -216,6 +216,12 @@ void TMission::start(core::Objective objective) { switch (objective) { case core::Objective::EXPLORE: { + if (not this->map_cleared) { + this->maze.reset(); + this->observer.reset(); + this->map_cleared = true; + } + this->executor.reset(this->get_start_pose(), this->config.search_profile); this->cell = this->maze.get_start().front(); this->at_center = false; @@ -403,10 +409,17 @@ void TMission::decide_at_entry() { Move move{}; if (next->orientation == this->cell.orientation) { - move.add(make_segment(SegmentKind::STRAIGHT, cell_size, entry)); - this->execute(move, this->search_speed, this->search_speed); + Segment straight = make_segment(SegmentKind::STRAIGHT, cell_size, entry); this->watching_front = this->maze.get_wall(this->cell) == WallState::UNKNOWN; + + if (this->watching_front) { + straight.max_speed = this->search_speed; + } + + move.add(straight); + this->execute(move, this->search_speed, this->search_speed); + this->watched_cell = this->cell; this->cell = *next; return; @@ -603,6 +616,7 @@ Segment TMission::make_segment(SegmentKind kind, float length, co .length = length, .start_speed = 0.0F, .end_speed = 0.0F, + .max_speed = std::numeric_limits::infinity(), .start = start, }; } diff --git a/micras_nav/include/micras/nav/impl/planner.tpp b/micras_nav/include/micras/nav/impl/planner.tpp index e591f33..0bb536f 100644 --- a/micras_nav/include/micras/nav/impl/planner.tpp +++ b/micras_nav/include/micras/nav/impl/planner.tpp @@ -13,6 +13,7 @@ #include #include +#include "micras/nav/curve_speed.hpp" #include "micras/nav/lattice.hpp" #include "micras/nav/maze.hpp" #include "micras/nav/motion_limits.hpp" @@ -36,6 +37,8 @@ void TPlanner::begin( this->turn_speeds.at(i) = this->dynamics.get_turn_speed(profile, static_cast(i)); } + this->load_turn_factors(); + this->number_of_usable_turns.fill(0); for (uint8_t i = 0; i < number_of_turns; i++) { @@ -319,6 +322,44 @@ float TPlanner::get_stop_distance(const LatticePose& node) const return (static_cast(cells) - 0.5F) * cell_size; } +template +float TPlanner::get_turn_time(TurnId turn, float length, float speed) const { + return this->turn_factors.at(std::to_underlying(turn)) * length / speed; +} + +template +void TPlanner::load_turn_factors() { + const RunProfile& profile = this->run_profile; + + const auto* const kept = std::ranges::find_if(this->kept_factors, [&profile](const TurnFactors& entry) { + return entry.valid and entry.profile.fan == profile.fan and entry.profile.risky == profile.risky and + entry.profile.utilization == profile.utilization and entry.profile.max_speed == profile.max_speed; + }); + + if (kept != this->kept_factors.end()) { + this->turn_factors = kept->factors; + return; + } + + const CurveLimits curve_limits = this->dynamics.get_curve_limits(profile); + + for (uint8_t i = 0; i < number_of_turns; i++) { + const auto turn = static_cast(i); + const TurnShape& shape = this->dynamics.get_turn(profile, turn); + const float speed = this->turn_speeds.at(i); + + this->turn_factors.at(i) = 1.0F; + + if (shape.valid and speed > 0.0F and shape.length() > 0.0F) { + this->turn_factors.at(i) = + CurveSpeed{shape, speed, speed, curve_limits}.duration() * speed / shape.length(); + } + } + + this->kept_factors.at(this->next_kept) = {.profile = profile, .factors = this->turn_factors, .valid = true}; + this->next_kept = static_cast((this->next_kept + 1) % kept_profiles); +} + template float TPlanner::get_speed(uint8_t arrival, uint8_t speed_ratio) const { if (arrival == rest) { @@ -358,8 +399,10 @@ TPlanner::Edge const float brakeable = SpeedProfile::get_brakeable_speed(distance, nominal_speed, this->limits); if (start_speed > brakeable) { - const float previous_length = this->dynamics.get_turn(this->run_profile, to_turn(arrival)).length(); - penalty = previous_length * (1.0F / brakeable - 1.0F / start_speed); + const TurnId previous = to_turn(arrival); + const float previous_length = this->dynamics.get_turn(this->run_profile, previous).length(); + penalty = this->get_turn_time(previous, previous_length, brakeable) - + this->get_turn_time(previous, previous_length, start_speed); start_speed = brakeable; } @@ -367,8 +410,8 @@ TPlanner::Edge std::min(nominal_speed, SpeedProfile::get_reachable_speed(distance, start_speed, this->limits)); const Edge edge{ - .cost = SpeedProfile{distance, start_speed, end_speed, this->limits}.duration() + shape.length() / end_speed + - penalty, + .cost = SpeedProfile{distance, start_speed, end_speed, this->limits}.duration() + + this->get_turn_time(turn, shape.length(), end_speed) + penalty, .speed_ratio = static_cast(std::lround(static_cast(full_speed) * end_speed / nominal_speed)), }; @@ -445,12 +488,17 @@ void TPlanner::compare_arrivals(uint8_t first, uint8_t second, ui const float kept_speed = this->get_speed(arrival, kept.speed_ratio); const float dropped_speed = this->get_speed(arrival, dropped.speed_ratio); - gap = - std::max(gap, (kept.cost - shape.length() / kept_speed) - (dropped.cost - shape.length() / dropped_speed)); + gap = std::max( + gap, (kept.cost - this->get_turn_time(turn, shape.length(), kept_speed)) - + (dropped.cost - this->get_turn_time(turn, shape.length(), dropped_speed)) + ); for (uint8_t j = 0; j < primitive.number_of_gates; j++) { const float remaining = shape.pre + shape.length() - std::max(shape.gates.at(j), shape.pre); - gap = std::max(gap, (kept.cost - remaining / kept_speed) - (dropped.cost - remaining / dropped_speed)); + gap = std::max( + gap, (kept.cost - this->get_turn_time(turn, remaining, kept_speed)) - + (dropped.cost - this->get_turn_time(turn, remaining, dropped_speed)) + ); } } @@ -485,10 +533,11 @@ bool TPlanner::dominates(const Label& first, const Label& second) return first.cost <= second.cost; } - const float length = this->dynamics.get_turn(this->run_profile, to_turn(first.arrival)).length(); + const TurnId turn = to_turn(first.arrival); + const float length = this->dynamics.get_turn(this->run_profile, turn).length(); - return first.cost - length / this->get_speed(first.arrival, first.speed_ratio) <= - second.cost - length / this->get_speed(second.arrival, second.speed_ratio); + return first.cost - this->get_turn_time(turn, length, this->get_speed(first.arrival, first.speed_ratio)) <= + second.cost - this->get_turn_time(turn, length, this->get_speed(second.arrival, second.speed_ratio)); } if (first.speed_ratio != full_speed) { @@ -498,10 +547,11 @@ bool TPlanner::dominates(const Label& first, const Label& second) float second_cost = second.cost; if (second.speed_ratio != full_speed) { - const float length = this->dynamics.get_turn(this->run_profile, to_turn(second.arrival)).length(); + const TurnId turn = to_turn(second.arrival); + const float length = this->dynamics.get_turn(this->run_profile, turn).length(); - second_cost -= length / this->get_speed(second.arrival, second.speed_ratio) - - length / this->get_speed(second.arrival, full_speed); + second_cost -= this->get_turn_time(turn, length, this->get_speed(second.arrival, second.speed_ratio)) - + this->get_turn_time(turn, length, this->get_speed(second.arrival, full_speed)); } return first.cost + this->gaps.at(first.arrival).at(second.arrival) <= second_cost; @@ -642,7 +692,7 @@ void TPlanner::relax_turn(uint16_t index, const LatticePose& entr const float stop_distance = this->get_stop_distance(exit); this->add_terminal({ - .cost = cost - remaining / speed, + .cost = cost - this->get_turn_time(turn, remaining, speed), .label = index, .run = run, .has_turn = true, diff --git a/micras_nav/include/micras/nav/localizer.hpp b/micras_nav/include/micras/nav/localizer.hpp index 912efb7..292f75b 100644 --- a/micras_nav/include/micras/nav/localizer.hpp +++ b/micras_nav/include/micras/nav/localizer.hpp @@ -71,6 +71,11 @@ class Localizer { * where it was expected. An end is a single event rather than a stream of readings, so it has a * cap of its own. The range tolerance, a constant part plus a part proportional to the range, * tells a reading that is on a wall from one that is past its end. + * + * A pose off by more than the gate allows rejects every reading, and nothing would bring it + * back. So after the recovery rejections in a row with the robot moving, the variance of the + * position grows by the square of the recovery deviation, which lets the next readings through + * and the pose converge to them again. */ struct Config { RobotModel model; @@ -95,6 +100,8 @@ class Localizer { float max_edge_correction; float range_tolerance; float relative_range_tolerance; + uint16_t recovery_rejections; + float recovery_deviation; uint8_t speed_window; }; @@ -102,13 +109,15 @@ class Localizer { * @brief Diagnostics of the filter, for a monitor. * * @note The innovation level is the running mean of the squared innovations in variances, which - * stays near one while the filter is consistent. + * stays near one while the filter is consistent. The recoveries count the times the position + * was let loose after too many rejections in a row. */ struct Status { float innovation_level; uint32_t accepted; uint32_t rejected; uint32_t edges; + uint32_t recoveries; }; /** @@ -322,6 +331,15 @@ class Localizer { */ void propagate(const Matrix& transition, const Matrix& noise_input, const Vector& noise_variances); + /** + * @brief Count a range the gate accepted or rejected, and let the position loose after too many + * rejections in a row with the robot moving. + * + * @param accepted Whether the range was accepted. + * @param stationary Whether the robot is standing still. + */ + void count_range(bool accepted, bool stationary); + /** * @brief Apply one scalar measurement, with the algorithm of Bierman. * @@ -441,6 +459,11 @@ class Localizer { * @brief Diagnostics of the filter. */ Status status{}; + + /** + * @brief Number of ranges rejected in a row with the robot moving. + */ + uint16_t rejection_streak{}; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/mission.hpp b/micras_nav/include/micras/nav/mission.hpp index 17a39f3..847c6da 100644 --- a/micras_nav/include/micras/nav/mission.hpp +++ b/micras_nav/include/micras/nav/mission.hpp @@ -167,7 +167,10 @@ class TMission { * @brief Start a run. * * @note Exploring and solving start from the start pose. Returning starts from wherever the - * exploration ended, with the robot stopped at the center of a cell. + * exploration ended, with the robot stopped at the center of a cell. The first exploration + * after a reset forgets the map restored from the flash, which may be of another maze and whose + * walls could never be revised; a fast run after a reset uses it, and so does an exploration + * that is tried again before the next reset. * * @param objective What the run is for. */ @@ -420,6 +423,11 @@ class TMission { */ bool watching_front{}; + /** + * @brief Whether an exploration has cleared the map since the last reset. + */ + bool map_cleared{}; + /** * @brief Cell being crossed while the wall ahead of it is watched. */ diff --git a/micras_nav/include/micras/nav/motion_limits.hpp b/micras_nav/include/micras/nav/motion_limits.hpp index af96378..4c40036 100644 --- a/micras_nav/include/micras/nav/motion_limits.hpp +++ b/micras_nav/include/micras/nav/motion_limits.hpp @@ -50,6 +50,14 @@ struct MotionLimits { */ float crossover_speed() const; + /** + * @brief Get the same limits with a lower top speed. + * + * @param speed The top speed, which is ignored where it is above the current one. + * @return The limits with the lower of the two top speeds. + */ + MotionLimits capped(float speed) const; + float max_speed; float acceleration; float deceleration; diff --git a/micras_nav/include/micras/nav/planner.hpp b/micras_nav/include/micras/nav/planner.hpp index b7e8f24..624cdc6 100644 --- a/micras_nav/include/micras/nav/planner.hpp +++ b/micras_nav/include/micras/nav/planner.hpp @@ -60,7 +60,11 @@ struct Route { * crosses passes the wall assumption. Its cost is the time of the run, from the evaluator the * executor plays back, plus the time of the turn at the speed the run profile gives it, so the * fastest path of the graph is the fastest route the robot can drive, to within what the velocity - * planner refines among the candidates. Diagonals, the turns of 135 + * planner refines among the candidates. That speed is the one of the tightest point of the turn, + * and the robot drives the rest of it faster, braking into that point and accelerating out of it, + * so the time of each turn is its length at that speed times a factor, the time the executor + * takes over the length at that speed. Without it every turn is overpriced, and a route of many + * turns, such as a diagonal one, loses to one of few that is slower. Diagonals, the turns of 135 * degrees and the two sizes of the turn of 90 degrees are part of the search rather than a rewrite * of its result. * @@ -379,6 +383,27 @@ class TPlanner { */ float get_stop_distance(const LatticePose& node) const; + /** + * @brief Get the time a part of a turn takes at a speed. + * + * @param turn The turn. + * @param length The length of the part of the turn, in meters. + * @param speed The speed the turn is priced at, in m/s. + * @return The time in seconds. + */ + float get_turn_time(TurnId turn, float length, float speed) const; + + /** + * @brief Compute the factor of the time of every turn for the run being planned, or take it + * from the ones computed before. + * + * @note Each factor times the whole motion of a turn, which is too much work for an iteration + * of the control loop, so the factors of the last few profiles are kept. The explorer goes + * through the same few profiles again and again, and only the first time is done with the robot + * stopped, at the goal. + */ + void load_turn_factors(); + /** * @brief Get the speed a label leaves its node with. * @@ -579,6 +604,36 @@ class TPlanner { */ std::array turn_speeds{}; + /** + * @brief Factors of the time of the turns of one run profile. + */ + struct TurnFactors { + RunProfile profile; + std::array factors; + bool valid; + }; + + /** + * @brief Number of profiles whose factors are kept, which covers every profile of the explorer + * and the fast run. + */ + static constexpr uint8_t kept_profiles{6}; + + /** + * @brief Factor of the time of each turn, for the run being planned. + */ + std::array turn_factors{}; + + /** + * @brief Factors of the profiles planned last. + */ + std::array kept_factors{}; + + /** + * @brief Slot of the kept factors to be replaced next. + */ + uint8_t next_kept{}; + /** * @brief Turns that can be driven on the run being planned, from a heading along the grid and * from a diagonal one. diff --git a/micras_nav/include/micras/nav/segment.hpp b/micras_nav/include/micras/nav/segment.hpp index ffb4c8e..ca249dd 100644 --- a/micras_nav/include/micras/nav/segment.hpp +++ b/micras_nav/include/micras/nav/segment.hpp @@ -30,7 +30,8 @@ enum class SegmentKind : uint8_t { * curve for a turn, negative for the mirror of the turn of the table, which is the turn to the right, * in radians for a rotation in place, positive to the left, in seconds for the two kinds that stand * still, and in meters for the racing line. The speeds are those at the ends of the segment and are - * filled in by the velocity planner, except for the racing line, which carries its own. + * filled in by the velocity planner, except for the racing line, which carries its own. The largest + * speed caps a straight below the limit of the run, and is infinite otherwise. * * @note The start is the pose the segment nominally starts from in the maze frame. It comes from the * route on the grid and never from a measurement, so the references generated from it do not move @@ -43,6 +44,7 @@ struct Segment { float length; float start_speed; float end_speed; + float max_speed; Pose start; }; diff --git a/micras_nav/include/micras/nav/wall_model.hpp b/micras_nav/include/micras/nav/wall_model.hpp index 0b66682..e8d2780 100644 --- a/micras_nav/include/micras/nav/wall_model.hpp +++ b/micras_nav/include/micras/nav/wall_model.hpp @@ -142,10 +142,12 @@ class WallModel { * @brief Get the range a sensor reads with the robot centered in a cell, facing along it. * * @note This is the pose the sensors are calibrated in: the side sensors against the walls of - * a corridor and the front ones against a wall ahead. + * a corridor and the front ones against a wall ahead. A side sensor points 45 degrees ahead and + * reaches the plane of the side walls past the cell the robot is in, so its range is the one to + * the side wall of the next cell: the corridor has to be two cells long, with no wall ahead. * * @param sensor The index of the sensor. - * @return The range to the wall the sensor points at, or zero if it points at none. + * @return The range to the plane of the wall the sensor points at. */ float get_centered_range(uint8_t sensor) const; diff --git a/micras_nav/src/controller.cpp b/micras_nav/src/controller.cpp index 0030b63..e36a3b4 100644 --- a/micras_nav/src/controller.cpp +++ b/micras_nav/src/controller.cpp @@ -3,13 +3,42 @@ */ #include +#include #include +#include +#include #include "micras/nav/controller.hpp" #include "micras/nav/segment.hpp" #include "micras/nav/state.hpp" namespace micras::nav { +/** + * @brief Find the real roots of a quadratic, in the form that keeps its precision. + * + * @param quadratic The coefficient of the square. + * @param linear The coefficient of the variable. + * @param constant The constant term. + * @return The roots, not a number where there is none, and only the first one of a linear equation. + */ +static std::array solve_quadratic(float quadratic, float linear, float constant) { + constexpr float none = std::numeric_limits::quiet_NaN(); + + if (std::abs(quadratic) < 1.0e-9F) { + return {std::abs(linear) > 0.0F ? -constant / linear : none, none}; + } + + const float discriminant = linear * linear - 4.0F * quadratic * constant; + + if (discriminant < 0.0F) { + return {none, none}; + } + + const float half = -0.5F * (linear + std::copysign(std::sqrt(discriminant), linear)); + + return {half / quadratic, std::abs(half) > 0.0F ? constant / half : none}; +} + Controller::Controller(const Config& config) : config{config}, linear_gains{compute_gains(config.linear, config.model.speed_constant(), config.model.acceleration_constant())}, @@ -17,9 +46,22 @@ Controller::Controller(const Config& config) : config.angular, config.model.angular_speed_constant(), config.model.angular_acceleration_constant() )} { } -Controller::Command Controller::update(const Reference& reference, const State& estimate) { +Controller::Command Controller::update(const Reference& unscaled, const State& estimate, float elapsed_time) { const RobotModel& model = this->config.model; + const float step = this->config.max_time_scale_rate * elapsed_time; + const float scale = std::clamp(this->find_time_scale(unscaled), this->time_scale - step, this->time_scale + step); + const float scale_rate = (scale - this->time_scale) / elapsed_time; + + this->time_scale = scale; + + Reference reference = unscaled; + reference.twist.linear = scale * unscaled.twist.linear; + reference.twist.angular = scale * unscaled.twist.angular; + reference.acceleration.linear = scale * scale * unscaled.acceleration.linear + scale_rate * unscaled.twist.linear; + reference.acceleration.angular = + scale * scale * unscaled.acceleration.angular + scale_rate * unscaled.twist.angular; + const float half_track = model.chassis.track_width / 2.0F; const float left_speed = reference.twist.linear - reference.twist.angular * half_track; const float right_speed = reference.twist.linear + reference.twist.angular * half_track; @@ -35,11 +77,6 @@ Controller::Command Controller::update(const Reference& reference, const State& model.angular_acceleration_constant() * reference.acceleration.angular + model.drive.static_friction_voltage * (right_friction - left_friction) / 2.0F; - const float available = model.drive.supply_voltage * (1.0F - this->config.voltage_reserve); - const float demand = std::abs(forward_feed_forward) + std::abs(rotation_feed_forward); - - this->time_scale = demand > 0.0F ? std::min(1.0F, this->time_scale * available / demand) : 1.0F; - const Pose seen = reference.pose.relative(estimate.pose); const float along_error = @@ -80,6 +117,52 @@ Controller::Command Controller::update(const Reference& reference, const State& }; } +float Controller::find_time_scale(const Reference& reference) const { + const RobotModel& model = this->config.model; + + const float half_track = model.chassis.track_width / 2.0F; + const float available = model.drive.supply_voltage * (1.0F - this->config.voltage_reserve); + + std::array, 2> wheels{}; + + for (uint8_t i = 0; i < 2; i++) { + const float side = i == 0 ? -1.0F : 1.0F; + const float speed = reference.twist.linear + side * reference.twist.angular * half_track; + + wheels.at(i) = { + model.drive.static_friction_voltage * std::clamp(speed / this->config.friction_speed, -1.0F, 1.0F), + model.speed_constant() * reference.twist.linear + + side * model.angular_speed_constant() * reference.twist.angular, + model.acceleration_constant() * reference.acceleration.linear + + side * model.angular_acceleration_constant() * reference.acceleration.angular, + }; + } + + const auto fits = [&wheels, available](float scale) { + return std::ranges::all_of(wheels, [scale, available](const std::array& wheel) { + return std::abs(wheel.at(0) + scale * (wheel.at(1) + scale * wheel.at(2))) <= available * 1.0001F; + }); + }; + + if (fits(1.0F)) { + return 1.0F; + } + + float best = min_time_scale; + + for (const std::array& wheel : wheels) { + for (const float limit : {available, -available}) { + for (const float root : solve_quadratic(wheel.at(2), wheel.at(1), wheel.at(0) - limit)) { + if (root > best and root < 1.0F and fits(root)) { + best = root; + } + } + } + } + + return best; +} + float Controller::get_time_scale() const { return this->time_scale; } diff --git a/micras_nav/src/drive_identification.cpp b/micras_nav/src/drive_identification.cpp index 1f7c3db..1f1fa64 100644 --- a/micras_nav/src/drive_identification.cpp +++ b/micras_nav/src/drive_identification.cpp @@ -83,8 +83,8 @@ Controller::Command DriveIdentification::update(const Measurements& measurements (rotating ? this->angular_fit : this->linear_fit) .add({this->sign_integral, this->position, speed - this->start_speed}, this->voltage_integral); - const bool too_far = - not rotating and std::abs(this->distance) > this->config.max_distance and (this->distance > 0.0F) == positive; + const float coast_end = this->distance + linear_speed * model.acceleration_constant() / model.speed_constant(); + const bool too_far = not rotating and (positive ? coast_end >= this->config.max_distance : coast_end <= 0.0F); if (this->phase_time >= 2.0F * this->config.step_time or too_far) { this->advance(); diff --git a/micras_nav/src/executor.cpp b/micras_nav/src/executor.cpp index 40369ae..fe75e79 100644 --- a/micras_nav/src/executor.cpp +++ b/micras_nav/src/executor.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include "micras/nav/curve_speed.hpp" @@ -43,6 +44,12 @@ void Executor::push(std::span segments) { this->durations.clear(); this->index = 0; this->queued_time = 0.0F; + } else { + const auto done = static_cast(this->index); + + this->segments.erase(this->segments.begin(), this->segments.begin() + done); + this->durations.erase(this->durations.begin(), this->durations.begin() + done); + this->index = 0; } for (const Segment& segment : segments) { @@ -91,10 +98,6 @@ Reference Executor::update(float elapsed_time, float time_scale, const State& es } this->reference = this->evaluate(); - this->reference.twist.linear *= time_scale; - this->reference.twist.angular *= time_scale; - this->reference.acceleration.linear *= time_scale * time_scale; - this->reference.acceleration.angular *= time_scale * time_scale; return this->reference; } @@ -141,7 +144,7 @@ void Executor::start_next() { case SegmentKind::STRAIGHT: this->speed_profile = SpeedProfile{ std::abs(segment.length), segment.start_speed, segment.end_speed, - this->dynamics.get_linear_limits(this->run_profile) + this->dynamics.get_linear_limits(this->run_profile).capped(segment.max_speed) }; this->duration = this->speed_profile.duration(); break; diff --git a/micras_nav/src/localizer.cpp b/micras_nav/src/localizer.cpp index cdd1fcf..75768d2 100644 --- a/micras_nav/src/localizer.cpp +++ b/micras_nav/src/localizer.cpp @@ -37,6 +37,7 @@ void Localizer::reset(const Pose& pose, const Measurements& measurements) { this->window_distance = 0.0F; this->window_duration = 0.0F; this->status = {}; + this->rejection_streak = 0; this->set_covariance({ this->config.initial_position_deviation * this->config.initial_position_deviation, @@ -251,6 +252,31 @@ void Localizer::propagate(const Matrix& transition, const Matrix& noise_input, c } } +void Localizer::count_range(bool accepted, bool stationary) { + if (accepted or stationary) { + this->rejection_streak = 0; + return; + } + + this->rejection_streak++; + + if (this->rejection_streak < this->config.recovery_rejections) { + return; + } + + Matrix identity{}; + + for (uint8_t i = 0; i < number_of_states; i++) { + identity.at(i).at(i) = 1.0F; + } + + const float variance = this->config.recovery_deviation * this->config.recovery_deviation; + + this->propagate(identity, identity, {variance, variance, 0.0F, 0.0F}); + this->rejection_streak = 0; + this->status.recoveries++; +} + bool Localizer::update( float innovation, const Vector& jacobian, float variance, float gate, float max_position_correction ) { diff --git a/micras_nav/src/motion_limits.cpp b/micras_nav/src/motion_limits.cpp index d514e4e..9799273 100644 --- a/micras_nav/src/motion_limits.cpp +++ b/micras_nav/src/motion_limits.cpp @@ -18,6 +18,12 @@ float MotionLimits::crossover_speed() const { return this->motor_speed * (1.0F - this->acceleration / this->motor_acceleration); } +MotionLimits MotionLimits::capped(float speed) const { + MotionLimits limits = *this; + limits.max_speed = std::min(limits.max_speed, speed); + return limits; +} + float CurveLimits::get_speed_limit(const Bending& bending) const { const float curvature = std::abs(bending.curvature); const float sharpness = std::abs(bending.sharpness); diff --git a/micras_nav/src/route_compiler.cpp b/micras_nav/src/route_compiler.cpp index 5d8d2de..08e7c7f 100644 --- a/micras_nav/src/route_compiler.cpp +++ b/micras_nav/src/route_compiler.cpp @@ -3,6 +3,7 @@ */ #include +#include #include #include @@ -33,6 +34,7 @@ void RouteCompiler::compile( .length = length, .start_speed = 0.0F, .end_speed = 0.0F, + .max_speed = std::numeric_limits::infinity(), .start = start, }); } @@ -67,6 +69,7 @@ void RouteCompiler::compile( .length = step.side == TurnSide::LEFT ? shape.length() : -shape.length(), .start_speed = 0.0F, .end_speed = 0.0F, + .max_speed = std::numeric_limits::infinity(), .start = retreat(node, -shape.pre), }); diff --git a/micras_nav/src/velocity_planner.cpp b/micras_nav/src/velocity_planner.cpp index 8dec7a8..345a26f 100644 --- a/micras_nav/src/velocity_planner.cpp +++ b/micras_nav/src/velocity_planner.cpp @@ -30,8 +30,8 @@ float VelocityPlanner::plan( segment.end_speed = 0.0F; if (segment.kind == SegmentKind::STRAIGHT) { - segment.start_speed = linear_limits.max_speed; - segment.end_speed = linear_limits.max_speed; + segment.start_speed = linear_limits.capped(segment.max_speed).max_speed; + segment.end_speed = segment.start_speed; } else if (segment.kind == SegmentKind::TURN) { const TurnShape& shape = dynamics.get_turn(profile, segment.turn); @@ -48,7 +48,9 @@ float VelocityPlanner::plan( if (segment.kind == SegmentKind::STRAIGHT) { segment.start_speed = std::min( segment.start_speed, - SpeedProfile::get_brakeable_speed(std::abs(segment.length), segment.end_speed, linear_limits) + SpeedProfile::get_brakeable_speed( + std::abs(segment.length), segment.end_speed, linear_limits.capped(segment.max_speed) + ) ); } else if (segment.kind == SegmentKind::TURN) { segment.start_speed = std::min( @@ -70,7 +72,9 @@ float VelocityPlanner::plan( if (segment.kind == SegmentKind::STRAIGHT) { segment.end_speed = std::min( segment.end_speed, - SpeedProfile::get_reachable_speed(std::abs(segment.length), segment.start_speed, linear_limits) + SpeedProfile::get_reachable_speed( + std::abs(segment.length), segment.start_speed, linear_limits.capped(segment.max_speed) + ) ); } else if (segment.kind == SegmentKind::TURN) { segment.end_speed = std::min( @@ -97,7 +101,8 @@ float VelocityPlanner::get_duration(const Segment& segment, const Dynamics& dyna switch (segment.kind) { case SegmentKind::STRAIGHT: return SpeedProfile{ - std::abs(segment.length), segment.start_speed, segment.end_speed, dynamics.get_linear_limits(profile) + std::abs(segment.length), segment.start_speed, segment.end_speed, + dynamics.get_linear_limits(profile).capped(segment.max_speed) } .duration(); diff --git a/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp b/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp index 64889a7..53da7c7 100644 --- a/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp +++ b/micras_proxy/include/micras/proxy/impl/torque_sensors.tpp @@ -14,6 +14,7 @@ namespace micras::proxy { template TTorqueSensors::TTorqueSensors(const Config& config) : adc{config.adc}, + base_reading{core::make_array(config.zero_reading)}, max_current{config.adc.reference_voltage / config.shunt_resistor}, max_torque{config.max_torque}, filters{core::make_array(config.filter)}, diff --git a/micras_proxy/include/micras/proxy/tick.hpp b/micras_proxy/include/micras/proxy/tick.hpp index 6605e31..36de2c2 100644 --- a/micras_proxy/include/micras/proxy/tick.hpp +++ b/micras_proxy/include/micras/proxy/tick.hpp @@ -44,6 +44,14 @@ class Tick { */ uint32_t wait(); + /** + * @brief Make the next tick one period from now. + * + * @note For after an operation that stalled the loop on purpose, with the robot stopped, so + * that the stall is not reported as missed ticks nor integrated as the time of one iteration. + */ + void restart(); + /** * @brief Get the time since the last tick. * diff --git a/micras_proxy/include/micras/proxy/torque_sensors.hpp b/micras_proxy/include/micras/proxy/torque_sensors.hpp index 824a18c..a458b02 100644 --- a/micras_proxy/include/micras/proxy/torque_sensors.hpp +++ b/micras_proxy/include/micras/proxy/torque_sensors.hpp @@ -20,10 +20,15 @@ class TTorqueSensors { public: /** * @brief Configuration struct for torque sensors. + * + * @note The zero reading is the reading with no current, as a fraction of the range of the + * converter, which is where a bidirectional amplifier sits. The currents and torques are signed + * about it. */ struct Config { hal::AdcDma::Config adc; float shunt_resistor; + float zero_reading; float max_torque; core::ButterworthFilter::Config filter; }; @@ -38,9 +43,10 @@ class TTorqueSensors { /** * @brief Calibrate the torque sensors. * - * @note The amplifiers on this board are bidirectional about half the reference voltage, so the - * uncalibrated reading carries a large offset and this has to be called before any reading is - * meaningful. Calling it again refines the baseline rather than discarding it. + * @note The readings start out signed about the zero reading of the configuration, which leaves + * the offset of each amplifier. This takes the current reading as the one of no current, so it + * is to be called with the motors disabled. Calling it again refines the baseline rather than + * discarding it. */ void calibrate(); @@ -115,7 +121,7 @@ class TTorqueSensors { std::array base_reading{}; /** - * @brief Value of the maximum current that can be measured by the sensor. + * @brief Current that moves the reading across the whole range of the converter, in amps. */ float max_current; diff --git a/micras_proxy/src/tick.cpp b/micras_proxy/src/tick.cpp index 17850d0..def4eec 100644 --- a/micras_proxy/src/tick.cpp +++ b/micras_proxy/src/tick.cpp @@ -20,6 +20,10 @@ uint32_t Tick::wait() { return ticks; } +void Tick::restart() { + this->last_tick = hal::Timer::get_counter(); +} + uint32_t Tick::elapsed_time_us() const { return hal::Timer::to_microseconds(hal::Timer::get_counter() - this->last_tick); } From f402f6f1029ece78afc3aea199b29243bdf51685 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Sep 2026 06:03:59 -0300 Subject: [PATCH 519/561] =?UTF-8?q?=E2=AC=86=EF=B8=8F=20Follow=20the=20fir?= =?UTF-8?q?mware's=20answer=20to=20the=20review=20of=20the=20navigation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The firmware answered the review of Team-Micras/MicrasFirmware#54 in one commit (9e307de). The host AdcDma publishes nothing while a converter is stopped, like the firmware's, and the current sense amplifiers are bidirectional, as the board's are: a channel reads half its range with no current and moves from there with the sign of the current, which the firmware now reads. It changes behaviour, so the baselines are a new version, v2. Everything but four changes was checked byte identical first: the reference runs differ only in the two new variable columns, failed_saves and localizer_recoveries, in link_credit, which announcing them costs, and in run_profile for the 0.1 s before a scenario sets the switches, since it is no longer restored from the flash. The four are the turn factor of the planner, the explorer round that runs to its end, the search speed cap into a cell with an unknown front wall, and the tick that restarts after a save and the boot. The contest stays clean on the ten mazes, with every switch on too. The fast runs are the same but for maze 1 with every switch, 11 ms faster, alljapan with the fan, 6 ms faster, and apec2019 with every switch, 127 ms slower: the turn factor changes the route there, and the racing line drawn through it comes out slower. The search takes 77 to 123 s, against 62 to 109 s: the cap costs about 10 s a maze, while the explorer and the turn factor together save 5 s over the ten. --- micras_hal/host/src/adc_dma.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/micras_hal/host/src/adc_dma.cpp b/micras_hal/host/src/adc_dma.cpp index 1a6761b..3c3bca2 100644 --- a/micras_hal/host/src/adc_dma.cpp +++ b/micras_hal/host/src/adc_dma.cpp @@ -83,7 +83,7 @@ uint32_t AdcDma::read_snapshot(std::span destination) const { void AdcDma::on_sequence_complete(const ADC_HandleTypeDef* handle) { AdcDma* const instance = find(handle); - if (instance == nullptr) { + if (instance == nullptr or instance->stopped) { return; } From 429018ab3d5fd88d2b2dc4fd016a82b2ee0552d1 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Sep 2026 11:21:33 -0300 Subject: [PATCH 520/561] =?UTF-8?q?=E2=8F=AA=20Price=20the=20turns=20at=20?= =?UTF-8?q?their=20speed=20again,=20without=20the=20factor?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The factor of f6df2b1 timed each turn entered and left at the speed of its tightest point. A turn of one bend cannot go faster anywhere at that speed, so its factor came out exactly one, and only the turns of two bends were discounted. What the planner really overprices is the robot entering a turn faster than that speed and braking inside it, which depends on the straights around the turn and is no single number per turn. So the correction only favored routes of two bend turns. On apec2019 with the fan and boost, four variants of such a route took every candidate slot, and a route of single turns 100 ms faster in exact time was never timed. The planner is back to what it was; a correction that holds is being looked into. --- .../include/micras/nav/impl/planner.tpp | 78 ++++--------------- micras_nav/include/micras/nav/planner.hpp | 57 +------------- 2 files changed, 15 insertions(+), 120 deletions(-) diff --git a/micras_nav/include/micras/nav/impl/planner.tpp b/micras_nav/include/micras/nav/impl/planner.tpp index 0bb536f..e591f33 100644 --- a/micras_nav/include/micras/nav/impl/planner.tpp +++ b/micras_nav/include/micras/nav/impl/planner.tpp @@ -13,7 +13,6 @@ #include #include -#include "micras/nav/curve_speed.hpp" #include "micras/nav/lattice.hpp" #include "micras/nav/maze.hpp" #include "micras/nav/motion_limits.hpp" @@ -37,8 +36,6 @@ void TPlanner::begin( this->turn_speeds.at(i) = this->dynamics.get_turn_speed(profile, static_cast(i)); } - this->load_turn_factors(); - this->number_of_usable_turns.fill(0); for (uint8_t i = 0; i < number_of_turns; i++) { @@ -322,44 +319,6 @@ float TPlanner::get_stop_distance(const LatticePose& node) const return (static_cast(cells) - 0.5F) * cell_size; } -template -float TPlanner::get_turn_time(TurnId turn, float length, float speed) const { - return this->turn_factors.at(std::to_underlying(turn)) * length / speed; -} - -template -void TPlanner::load_turn_factors() { - const RunProfile& profile = this->run_profile; - - const auto* const kept = std::ranges::find_if(this->kept_factors, [&profile](const TurnFactors& entry) { - return entry.valid and entry.profile.fan == profile.fan and entry.profile.risky == profile.risky and - entry.profile.utilization == profile.utilization and entry.profile.max_speed == profile.max_speed; - }); - - if (kept != this->kept_factors.end()) { - this->turn_factors = kept->factors; - return; - } - - const CurveLimits curve_limits = this->dynamics.get_curve_limits(profile); - - for (uint8_t i = 0; i < number_of_turns; i++) { - const auto turn = static_cast(i); - const TurnShape& shape = this->dynamics.get_turn(profile, turn); - const float speed = this->turn_speeds.at(i); - - this->turn_factors.at(i) = 1.0F; - - if (shape.valid and speed > 0.0F and shape.length() > 0.0F) { - this->turn_factors.at(i) = - CurveSpeed{shape, speed, speed, curve_limits}.duration() * speed / shape.length(); - } - } - - this->kept_factors.at(this->next_kept) = {.profile = profile, .factors = this->turn_factors, .valid = true}; - this->next_kept = static_cast((this->next_kept + 1) % kept_profiles); -} - template float TPlanner::get_speed(uint8_t arrival, uint8_t speed_ratio) const { if (arrival == rest) { @@ -399,10 +358,8 @@ TPlanner::Edge const float brakeable = SpeedProfile::get_brakeable_speed(distance, nominal_speed, this->limits); if (start_speed > brakeable) { - const TurnId previous = to_turn(arrival); - const float previous_length = this->dynamics.get_turn(this->run_profile, previous).length(); - penalty = this->get_turn_time(previous, previous_length, brakeable) - - this->get_turn_time(previous, previous_length, start_speed); + const float previous_length = this->dynamics.get_turn(this->run_profile, to_turn(arrival)).length(); + penalty = previous_length * (1.0F / brakeable - 1.0F / start_speed); start_speed = brakeable; } @@ -410,8 +367,8 @@ TPlanner::Edge std::min(nominal_speed, SpeedProfile::get_reachable_speed(distance, start_speed, this->limits)); const Edge edge{ - .cost = SpeedProfile{distance, start_speed, end_speed, this->limits}.duration() + - this->get_turn_time(turn, shape.length(), end_speed) + penalty, + .cost = SpeedProfile{distance, start_speed, end_speed, this->limits}.duration() + shape.length() / end_speed + + penalty, .speed_ratio = static_cast(std::lround(static_cast(full_speed) * end_speed / nominal_speed)), }; @@ -488,17 +445,12 @@ void TPlanner::compare_arrivals(uint8_t first, uint8_t second, ui const float kept_speed = this->get_speed(arrival, kept.speed_ratio); const float dropped_speed = this->get_speed(arrival, dropped.speed_ratio); - gap = std::max( - gap, (kept.cost - this->get_turn_time(turn, shape.length(), kept_speed)) - - (dropped.cost - this->get_turn_time(turn, shape.length(), dropped_speed)) - ); + gap = + std::max(gap, (kept.cost - shape.length() / kept_speed) - (dropped.cost - shape.length() / dropped_speed)); for (uint8_t j = 0; j < primitive.number_of_gates; j++) { const float remaining = shape.pre + shape.length() - std::max(shape.gates.at(j), shape.pre); - gap = std::max( - gap, (kept.cost - this->get_turn_time(turn, remaining, kept_speed)) - - (dropped.cost - this->get_turn_time(turn, remaining, dropped_speed)) - ); + gap = std::max(gap, (kept.cost - remaining / kept_speed) - (dropped.cost - remaining / dropped_speed)); } } @@ -533,11 +485,10 @@ bool TPlanner::dominates(const Label& first, const Label& second) return first.cost <= second.cost; } - const TurnId turn = to_turn(first.arrival); - const float length = this->dynamics.get_turn(this->run_profile, turn).length(); + const float length = this->dynamics.get_turn(this->run_profile, to_turn(first.arrival)).length(); - return first.cost - this->get_turn_time(turn, length, this->get_speed(first.arrival, first.speed_ratio)) <= - second.cost - this->get_turn_time(turn, length, this->get_speed(second.arrival, second.speed_ratio)); + return first.cost - length / this->get_speed(first.arrival, first.speed_ratio) <= + second.cost - length / this->get_speed(second.arrival, second.speed_ratio); } if (first.speed_ratio != full_speed) { @@ -547,11 +498,10 @@ bool TPlanner::dominates(const Label& first, const Label& second) float second_cost = second.cost; if (second.speed_ratio != full_speed) { - const TurnId turn = to_turn(second.arrival); - const float length = this->dynamics.get_turn(this->run_profile, turn).length(); + const float length = this->dynamics.get_turn(this->run_profile, to_turn(second.arrival)).length(); - second_cost -= this->get_turn_time(turn, length, this->get_speed(second.arrival, second.speed_ratio)) - - this->get_turn_time(turn, length, this->get_speed(second.arrival, full_speed)); + second_cost -= length / this->get_speed(second.arrival, second.speed_ratio) - + length / this->get_speed(second.arrival, full_speed); } return first.cost + this->gaps.at(first.arrival).at(second.arrival) <= second_cost; @@ -692,7 +642,7 @@ void TPlanner::relax_turn(uint16_t index, const LatticePose& entr const float stop_distance = this->get_stop_distance(exit); this->add_terminal({ - .cost = cost - this->get_turn_time(turn, remaining, speed), + .cost = cost - remaining / speed, .label = index, .run = run, .has_turn = true, diff --git a/micras_nav/include/micras/nav/planner.hpp b/micras_nav/include/micras/nav/planner.hpp index 624cdc6..b7e8f24 100644 --- a/micras_nav/include/micras/nav/planner.hpp +++ b/micras_nav/include/micras/nav/planner.hpp @@ -60,11 +60,7 @@ struct Route { * crosses passes the wall assumption. Its cost is the time of the run, from the evaluator the * executor plays back, plus the time of the turn at the speed the run profile gives it, so the * fastest path of the graph is the fastest route the robot can drive, to within what the velocity - * planner refines among the candidates. That speed is the one of the tightest point of the turn, - * and the robot drives the rest of it faster, braking into that point and accelerating out of it, - * so the time of each turn is its length at that speed times a factor, the time the executor - * takes over the length at that speed. Without it every turn is overpriced, and a route of many - * turns, such as a diagonal one, loses to one of few that is slower. Diagonals, the turns of 135 + * planner refines among the candidates. Diagonals, the turns of 135 * degrees and the two sizes of the turn of 90 degrees are part of the search rather than a rewrite * of its result. * @@ -383,27 +379,6 @@ class TPlanner { */ float get_stop_distance(const LatticePose& node) const; - /** - * @brief Get the time a part of a turn takes at a speed. - * - * @param turn The turn. - * @param length The length of the part of the turn, in meters. - * @param speed The speed the turn is priced at, in m/s. - * @return The time in seconds. - */ - float get_turn_time(TurnId turn, float length, float speed) const; - - /** - * @brief Compute the factor of the time of every turn for the run being planned, or take it - * from the ones computed before. - * - * @note Each factor times the whole motion of a turn, which is too much work for an iteration - * of the control loop, so the factors of the last few profiles are kept. The explorer goes - * through the same few profiles again and again, and only the first time is done with the robot - * stopped, at the goal. - */ - void load_turn_factors(); - /** * @brief Get the speed a label leaves its node with. * @@ -604,36 +579,6 @@ class TPlanner { */ std::array turn_speeds{}; - /** - * @brief Factors of the time of the turns of one run profile. - */ - struct TurnFactors { - RunProfile profile; - std::array factors; - bool valid; - }; - - /** - * @brief Number of profiles whose factors are kept, which covers every profile of the explorer - * and the fast run. - */ - static constexpr uint8_t kept_profiles{6}; - - /** - * @brief Factor of the time of each turn, for the run being planned. - */ - std::array turn_factors{}; - - /** - * @brief Factors of the profiles planned last. - */ - std::array kept_factors{}; - - /** - * @brief Slot of the kept factors to be replaced next. - */ - uint8_t next_kept{}; - /** * @brief Turns that can be driven on the run being planned, from a heading along the grid and * from a diagonal one. From 96a96a3d39ebface5ad46cb0e443a38ccf9457c9 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Sep 2026 13:18:12 -0300 Subject: [PATCH 521/561] =?UTF-8?q?=F0=9F=90=9B=20Price=20a=20turn=20enter?= =?UTF-8?q?ed=20slowly=20at=20a=20blend,=20and=20a=20slower=20next=20turn?= =?UTF-8?q?=20at=20nothing?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The planner priced every turn as if driven at one speed. Of the error of its cost against the exact time of the velocity planner, over 172,712 steps of the cheapest routes of 3522 mazes in five profiles, 97 % came from the 19 % of steps where that is not so: - A turn the straight before it is too short for is charged all at the entry speed, while the robot accelerates inside it. It is now charged at the entry speed plus 0.7 of the way to the turn's own speed, which leaves 0.4 ms of mean error per turn against 25 ms. The blend that zeroes the bias stays at 0.70 to 0.72 in every profile, and with a robot whose motors limit it from a standstill. - A turn that has to leave slower for the next one was charged all at the lower speed. The robot slows in its exit ramp at next to no cost, so there is no penalty any more: 0.7 ms of error left against 26 ms. On the 522 mazes of the public corpus, a route is priced within -5 +- 20 ms of its exact time, against +72 +- 76 ms, and the fast run's route is 11 ms faster on average, 222 ms at most: the old prices never ranked those routes among their best 32. The best route falls outside the explorer's window of 50 ms in 13 % of the runs instead of 46 %. In the simulator, fast runs from the same map changed by exactly what the exact times said, on 40 random corpus mazes. The planner does the same work as before. --- .../include/micras/nav/impl/planner.tpp | 51 +++++++++++-------- micras_nav/include/micras/nav/planner.hpp | 30 +++++++++-- 2 files changed, 56 insertions(+), 25 deletions(-) diff --git a/micras_nav/include/micras/nav/impl/planner.tpp b/micras_nav/include/micras/nav/impl/planner.tpp index e591f33..82a8f23 100644 --- a/micras_nav/include/micras/nav/impl/planner.tpp +++ b/micras_nav/include/micras/nav/impl/planner.tpp @@ -319,6 +319,13 @@ float TPlanner::get_stop_distance(const LatticePose& node) const return (static_cast(cells) - 0.5F) * cell_size; } +template +float TPlanner::get_turn_time(TurnId turn, float length, float speed) const { + const float nominal = this->turn_speeds.at(std::to_underlying(turn)); + + return length / (speed < nominal ? speed + turn_blend * (nominal - speed) : speed); +} + template float TPlanner::get_speed(uint8_t arrival, uint8_t speed_ratio) const { if (arrival == rest) { @@ -352,23 +359,16 @@ TPlanner::Edge const float distance = this->get_offset(arrival) + static_cast(run) * step + shape.pre; const float nominal_speed = this->turn_speeds.at(std::to_underlying(turn)); - float start_speed = this->get_speed(arrival, speed_ratio); - float penalty = 0.0F; - - const float brakeable = SpeedProfile::get_brakeable_speed(distance, nominal_speed, this->limits); - - if (start_speed > brakeable) { - const float previous_length = this->dynamics.get_turn(this->run_profile, to_turn(arrival)).length(); - penalty = previous_length * (1.0F / brakeable - 1.0F / start_speed); - start_speed = brakeable; - } + const float start_speed = std::min( + this->get_speed(arrival, speed_ratio), SpeedProfile::get_brakeable_speed(distance, nominal_speed, this->limits) + ); const float end_speed = std::min(nominal_speed, SpeedProfile::get_reachable_speed(distance, start_speed, this->limits)); const Edge edge{ - .cost = SpeedProfile{distance, start_speed, end_speed, this->limits}.duration() + shape.length() / end_speed + - penalty, + .cost = SpeedProfile{distance, start_speed, end_speed, this->limits}.duration() + + this->get_turn_time(turn, shape.length(), end_speed), .speed_ratio = static_cast(std::lround(static_cast(full_speed) * end_speed / nominal_speed)), }; @@ -445,12 +445,17 @@ void TPlanner::compare_arrivals(uint8_t first, uint8_t second, ui const float kept_speed = this->get_speed(arrival, kept.speed_ratio); const float dropped_speed = this->get_speed(arrival, dropped.speed_ratio); - gap = - std::max(gap, (kept.cost - shape.length() / kept_speed) - (dropped.cost - shape.length() / dropped_speed)); + gap = std::max( + gap, (kept.cost - this->get_turn_time(turn, shape.length(), kept_speed)) - + (dropped.cost - this->get_turn_time(turn, shape.length(), dropped_speed)) + ); for (uint8_t j = 0; j < primitive.number_of_gates; j++) { const float remaining = shape.pre + shape.length() - std::max(shape.gates.at(j), shape.pre); - gap = std::max(gap, (kept.cost - remaining / kept_speed) - (dropped.cost - remaining / dropped_speed)); + gap = std::max( + gap, (kept.cost - this->get_turn_time(turn, remaining, kept_speed)) - + (dropped.cost - this->get_turn_time(turn, remaining, dropped_speed)) + ); } } @@ -485,10 +490,11 @@ bool TPlanner::dominates(const Label& first, const Label& second) return first.cost <= second.cost; } - const float length = this->dynamics.get_turn(this->run_profile, to_turn(first.arrival)).length(); + const TurnId turn = to_turn(first.arrival); + const float length = this->dynamics.get_turn(this->run_profile, turn).length(); - return first.cost - length / this->get_speed(first.arrival, first.speed_ratio) <= - second.cost - length / this->get_speed(second.arrival, second.speed_ratio); + return first.cost - this->get_turn_time(turn, length, this->get_speed(first.arrival, first.speed_ratio)) <= + second.cost - this->get_turn_time(turn, length, this->get_speed(second.arrival, second.speed_ratio)); } if (first.speed_ratio != full_speed) { @@ -498,10 +504,11 @@ bool TPlanner::dominates(const Label& first, const Label& second) float second_cost = second.cost; if (second.speed_ratio != full_speed) { - const float length = this->dynamics.get_turn(this->run_profile, to_turn(second.arrival)).length(); + const TurnId turn = to_turn(second.arrival); + const float length = this->dynamics.get_turn(this->run_profile, turn).length(); - second_cost -= length / this->get_speed(second.arrival, second.speed_ratio) - - length / this->get_speed(second.arrival, full_speed); + second_cost -= this->get_turn_time(turn, length, this->get_speed(second.arrival, second.speed_ratio)) - + this->get_turn_time(turn, length, this->get_speed(second.arrival, full_speed)); } return first.cost + this->gaps.at(first.arrival).at(second.arrival) <= second_cost; @@ -642,7 +649,7 @@ void TPlanner::relax_turn(uint16_t index, const LatticePose& entr const float stop_distance = this->get_stop_distance(exit); this->add_terminal({ - .cost = cost - remaining / speed, + .cost = cost - this->get_turn_time(turn, remaining, speed), .label = index, .run = run, .has_turn = true, diff --git a/micras_nav/include/micras/nav/planner.hpp b/micras_nav/include/micras/nav/planner.hpp index b7e8f24..b569cd8 100644 --- a/micras_nav/include/micras/nav/planner.hpp +++ b/micras_nav/include/micras/nav/planner.hpp @@ -60,9 +60,12 @@ struct Route { * crosses passes the wall assumption. Its cost is the time of the run, from the evaluator the * executor plays back, plus the time of the turn at the speed the run profile gives it, so the * fastest path of the graph is the fastest route the robot can drive, to within what the velocity - * planner refines among the candidates. Diagonals, the turns of 135 - * degrees and the two sizes of the turn of 90 degrees are part of the search rather than a rewrite - * of its result. + * planner refines among the candidates. A turn the run cannot reach its speed for is priced at a + * blend of the two speeds, since the robot accelerates inside it, and a turn that has to leave + * slower for the next one costs nothing more, since the robot slows in its exit ramp. Over thousands + * of mazes that prices a route to within about 20 ms of what the velocity planner makes of it. + * Diagonals, the turns of 135 degrees and the two sizes of the turn of 90 degrees are part of the + * search rather than a rewrite of its result. * * What the search holds are labels: a way of reaching a node, with its cost, the turn that reached * it and how fast that turn is driven, since those fix the speed the next straight starts at and @@ -246,6 +249,12 @@ class TPlanner { */ static constexpr uint8_t full_speed{255}; + /** + * @brief Share of the way from the speed a turn is entered at to its own speed that the time of + * the turn is priced at. + */ + static constexpr float turn_blend{0.7F}; + /** * @brief Number of headings a wall can be crossed with: two along the grid and four diagonal. */ @@ -388,6 +397,21 @@ class TPlanner { */ float get_speed(uint8_t arrival, uint8_t speed_ratio) const; + /** + * @brief Get the time a part of a turn takes, entered at a speed. + * + * @note Entered below its speed, a turn is not driven at the speed it was entered at: the + * robot accelerates inside it, where its curvature leaves grip to spare. The time is the length + * at a speed that blends the two, by the turn blend, which matches the motion the executor plays + * within a few milliseconds per turn over thousands of mazes, whatever the traction. + * + * @param turn The turn. + * @param length The length of the part of the turn, in meters. + * @param speed The speed the turn is entered at, in m/s. + * @return The time in seconds. + */ + float get_turn_time(TurnId turn, float length, float speed) const; + /** * @brief Get the straight left between the curve that reached a label and its node. * From db65a19361d07d0491aca704a027f984df040183 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Sep 2026 15:15:46 -0300 Subject: [PATCH 522/561] =?UTF-8?q?=F0=9F=90=9B=20Answer=20the=20second=20?= =?UTF-8?q?review=20of=20the=20navigation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The second review of #54 found a racing line that grazes, a time scale that lets a weaker motor end a run in ERROR, a recovery that never starts, and a handful of smaller faults. What changed: - The racing line keeps the margin of the careful turns whatever the switch, 13 mm in the final check. With risky it accepted 8 mm, and on japan2017ef the line through the risky route grazed the walls at 1.4 m/s on every seed, where the same risky turns without the line never touched. It is now clean there and 75 ms slower; the other nine mazes of the simulator run as before. - The time scale changes no faster than gives the faster wheel 10 m/s^2 of acceleration, instead of 2 per second, so it falls quickly at low speed, and it only rises as fast as the voltage left over lets the acceleration of the rise through. With the supply at 8 V in both models of the simulator, 19 of 20 fast runs ended in ERROR before. With the new scale and a reserve of 30 % none does, and with the old scale 15 still do: a motor that weak also needs a larger reserve, which waits for the identified model. - A reading is dropped for its range only when both the range it reads and the range the pose predicts are past 120 mm, so a pose that drifted from a wall gets its readings rejected and counted towards a recovery. - A straight into a cell with the wall ahead unknown is held to the search speed only up to the last point the robot can stop at the center from, and is free after it. The search of the ten mazes takes 867 s instead of 911 s, none slower. - Each comparison of the planner's preparation is charged to the budget for the edges it times instead of as one, so it no longer makes the longest iteration of the explorer. - A status of the IMU with a bit set that always reads zero brings no sample, so a data line held high trips the IMU timeout instead of reading -1 LSB. - A wall sensor whose dark reading is above 80 % of the full scale is blind, its receiver saturated by ambient light, and the wall observer takes no vote from it. - The pool keeps which fault stopped the robot until the next run starts. - The default references of the wall sensors are marked as placeholders, the IMU timeout says why it counts iterations, and the planner and the explorer say how far the price of a route can be off. --- micras_nav/include/micras/nav/controller.hpp | 43 ++++++++++- micras_nav/include/micras/nav/explorer.hpp | 6 +- .../include/micras/nav/impl/localizer.tpp | 12 ++- .../include/micras/nav/impl/mission.tpp | 49 ++++++++---- .../include/micras/nav/impl/planner.tpp | 16 ++-- .../include/micras/nav/impl/racing_line.tpp | 3 +- .../include/micras/nav/impl/wall_observer.tpp | 2 +- micras_nav/include/micras/nav/localizer.hpp | 15 ++-- .../include/micras/nav/measurements.hpp | 3 + micras_nav/include/micras/nav/mission.hpp | 19 ++++- micras_nav/include/micras/nav/planner.hpp | 17 ++-- micras_nav/include/micras/nav/racing_line.hpp | 28 +++---- .../include/micras/nav/wall_observer.hpp | 4 +- micras_nav/src/controller.cpp | 77 ++++++++++++++----- .../micras/proxy/impl/wall_sensors.tpp | 8 ++ micras_proxy/include/micras/proxy/imu.hpp | 11 +++ .../include/micras/proxy/wall_sensors.hpp | 25 +++++- micras_proxy/src/imu.cpp | 4 + 18 files changed, 254 insertions(+), 88 deletions(-) diff --git a/micras_nav/include/micras/nav/controller.hpp b/micras_nav/include/micras/nav/controller.hpp index d23c66d..0f7d5c0 100644 --- a/micras_nav/include/micras/nav/controller.hpp +++ b/micras_nav/include/micras/nav/controller.hpp @@ -5,6 +5,8 @@ #ifndef MICRAS_NAV_CONTROLLER_HPP #define MICRAS_NAV_CONTROLLER_HPP +#include + #include "micras/nav/robot_model.hpp" #include "micras/nav/segment.hpp" #include "micras/nav/state.hpp" @@ -57,9 +59,9 @@ class Controller { * @note The steering gain is in radians of offset per meter of error and the offset is limited * to the largest steering. Below the blend speed the steering fades out, since turning in place * cannot reduce an error across the path. The friction speed is the wheel speed over which the - * static friction compensation goes from nothing to all of it. The time scale may change by at - * most the largest time scale rate per second, since a change of the time scale is itself an - * acceleration of the reference, of that rate times the speed. + * static friction compensation goes from nothing to all of it. A change of the time scale is + * itself an acceleration of the reference, of its rate times the speed, so the rate of the + * scale times the speed of the faster wheel is kept within the largest time scale acceleration. */ struct Config { RobotModel model; @@ -70,7 +72,7 @@ class Controller { float steering_blend_speed; float friction_speed; float voltage_reserve; - float max_time_scale_rate; + float max_time_scale_acceleration; }; /** @@ -156,6 +158,19 @@ class Controller { */ static Gains compute_gains(const Axis& axis, float speed_constant, float acceleration_constant); + /** + * @brief Find the time scale of this iteration, moving towards the one the reference fits at. + * + * @note The scale falls as fast as the time scale acceleration allows. It rises no faster than + * that either, and only as fast as the voltage left over at the new scale lets through the + * acceleration the rise adds, so that rising never saturates the motors by itself. + * + * @param reference What the robot should be doing, at the full pace. + * @param elapsed_time Time since the last iteration. + * @return The time scale, in (0, 1]. + */ + float find_next_time_scale(const Reference& reference, float elapsed_time) const; + /** * @brief Find the largest time scale at which the feed forward fits in the voltage available. * @@ -168,6 +183,26 @@ class Controller { */ float find_time_scale(const Reference& reference) const; + /** + * @brief Get the terms of the feed forward of each wheel, left then right. + * + * @note Each wheel has four terms, in volts: the static friction, which does not scale, the + * speed term, which scales with the time scale, the acceleration term, which scales with its + * square, and the term of the rate of the time scale, which is the acceleration a change of the + * scale adds, per unit of that rate. + * + * @param reference What the robot should be doing, at the full pace. + * @return The terms of the feed forward of each wheel. + */ + std::array, 2> get_wheel_terms(const Reference& reference) const; + + /** + * @brief Get the voltage the feed forward may use, with the reserve for the feedback left out. + * + * @return The voltage available. + */ + float get_available_voltage() const; + /** * @brief Parameters of the controller, with the physical description of the robot. */ diff --git a/micras_nav/include/micras/nav/explorer.hpp b/micras_nav/include/micras/nav/explorer.hpp index e97098f..4ab04e9 100644 --- a/micras_nav/include/micras/nav/explorer.hpp +++ b/micras_nav/include/micras/nav/explorer.hpp @@ -22,8 +22,10 @@ namespace micras::nav { * never seen is taken as absent. If that route crosses no unknown wall, the map already proves that * nothing faster exists and the search is over. Otherwise the cells next to the unknown walls it * crosses are what is worth visiting, and the question is asked again once the map has changed. The - * test uses the cost of the planner, whose candidates the fast run is chosen among, so a faster - * route cannot stay hidden, and only cells on some best conceivable route are ever targeted. + * test uses the cost of the planner, whose candidates the fast run is chosen among, so a route + * faster by more than the error of that cost cannot stay hidden, and only cells on some best + * conceivable route are ever targeted. A route priced within that error of a better one may be + * dropped by the planner even when it would drive faster. * * The fastest route depends on the profile of the run, so the test is repeated for every profile the * map has to be good for and the targets are put together. diff --git a/micras_nav/include/micras/nav/impl/localizer.tpp b/micras_nav/include/micras/nav/impl/localizer.tpp index f498b6f..f05e2ae 100644 --- a/micras_nav/include/micras/nav/impl/localizer.tpp +++ b/micras_nav/include/micras/nav/impl/localizer.tpp @@ -5,6 +5,7 @@ #ifndef MICRAS_NAV_LOCALIZER_TPP #define MICRAS_NAV_LOCALIZER_TPP +#include #include #include #include @@ -49,7 +50,13 @@ void Localizer::correct( this->track_edge(sensor, reading, sampled, hit, wall_model, maze); } - if (not reading.valid or not hit.valid or hit.state != WallState::WALL or hit.range > this->config.max_range or + if (not reading.valid or not hit.valid or hit.state != WallState::WALL) { + continue; + } + + const float range = wall_model.get_range(reading.distance, sensor, hit); + + if (std::min(range, hit.range) > this->config.max_range or not wall_model.is_footprint_clear( hit, sensor, this->get_position_deviation(), this->get_orientation_deviation() )) { @@ -59,8 +66,7 @@ void Localizer::correct( const float deviation = wall_model.get_range_deviation(hit.range); const bool accepted = this->update( - wall_model.get_range(reading.distance, sensor, hit) - hit.range, - {hit.jacobian.at(0), hit.jacobian.at(1), hit.jacobian.at(2), 0.0F}, + range - hit.range, {hit.jacobian.at(0), hit.jacobian.at(1), hit.jacobian.at(2), 0.0F}, deviation * deviation * this->config.range_correlation, gate, this->config.max_position_correction ); diff --git a/micras_nav/include/micras/nav/impl/mission.tpp b/micras_nav/include/micras/nav/impl/mission.tpp index e77836f..1fbbf7e 100644 --- a/micras_nav/include/micras/nav/impl/mission.tpp +++ b/micras_nav/include/micras/nav/impl/mission.tpp @@ -274,21 +274,20 @@ TMission::Status TMission::update( const Segment* watched = this->executor.get_current(); + const float commit_distance = this->get_commit_distance(); + if (this->watching_front and watched != nullptr and watched->kind == SegmentKind::STRAIGHT and - watched->start.position.distance(this->get_entry_pose(this->watched_cell).position) < watch_tolerance) { - const float cell_size = this->dynamics.get_model().maze.cell_size; - const float braking = this->search_speed * this->search_speed / - (2.0F * this->dynamics.get_linear_limits(this->config.search_profile).deceleration); + watched->start.position.distance(this->get_entry_pose(this->watched_cell).position) < + std::max(commit_distance, 0.0F) + watch_tolerance) { + const float travelled = watched->start.position.distance(this->get_entry_pose(this->watched_cell).position) + + this->executor.get_reference().distance; const WallState wall = this->maze.get_wall(this->watched_cell); if (wall == WallState::NO_WALL) { this->watching_front = false; - } else if ( - wall == WallState::WALL or - this->executor.get_reference().distance >= cell_size / 2.0F - braking - this->config.commit_margin - ) { - this->divert_to_center(); + } else if (wall == WallState::WALL or travelled >= commit_distance) { + this->divert_to_center(travelled); } } @@ -409,15 +408,23 @@ void TMission::decide_at_entry() { Move move{}; if (next->orientation == this->cell.orientation) { - Segment straight = make_segment(SegmentKind::STRAIGHT, cell_size, entry); + const float commit_distance = this->get_commit_distance(); this->watching_front = this->maze.get_wall(this->cell) == WallState::UNKNOWN; - if (this->watching_front) { - straight.max_speed = this->search_speed; + if (this->watching_front and commit_distance > 0.0F) { + Segment watched = make_segment(SegmentKind::STRAIGHT, commit_distance, entry); + watched.max_speed = this->search_speed; + + move.add(watched); + move.add(make_segment( + SegmentKind::STRAIGHT, cell_size - commit_distance, + entry.compose({.position = {.x = commit_distance, .y = 0.0F}, .orientation = 0.0F}) + )); + } else { + move.add(make_segment(SegmentKind::STRAIGHT, cell_size, entry)); } - move.add(straight); this->execute(move, this->search_speed, this->search_speed); this->watched_cell = this->cell; @@ -573,10 +580,10 @@ bool TMission::finish_at_entry() { } template -void TMission::divert_to_center() { +void TMission::divert_to_center(float travelled) { const float cell_size = this->dynamics.get_model().maze.cell_size; const Reference& current = this->executor.get_reference(); - const float remaining = cell_size / 2.0F - current.distance; + const float remaining = cell_size / 2.0F - travelled; this->watching_front = false; @@ -589,8 +596,7 @@ void TMission::divert_to_center() { Move move{}; move.add(make_segment( SegmentKind::STRAIGHT, remaining, - this->get_entry_pose(this->watched_cell) - .compose({.position = {.x = current.distance, .y = 0.0F}, .orientation = 0.0F}) + this->get_entry_pose(this->watched_cell).compose({.position = {.x = travelled, .y = 0.0F}, .orientation = 0.0F}) )); if (this->maze.get_wall(this->watched_cell) == WallState::WALL) { @@ -608,6 +614,15 @@ void TMission::divert_to_center() { this->at_center = true; } +template +float TMission::get_commit_distance() const { + const float cell_size = this->dynamics.get_model().maze.cell_size; + const float braking = this->search_speed * this->search_speed / + (2.0F * this->dynamics.get_linear_limits(this->config.search_profile).deceleration); + + return cell_size / 2.0F - braking - this->config.commit_margin; +} + template Segment TMission::make_segment(SegmentKind kind, float length, const Pose& start) { return { diff --git a/micras_nav/include/micras/nav/impl/planner.tpp b/micras_nav/include/micras/nav/impl/planner.tpp index 82a8f23..1072365 100644 --- a/micras_nav/include/micras/nav/impl/planner.tpp +++ b/micras_nav/include/micras/nav/impl/planner.tpp @@ -121,9 +121,9 @@ void TPlanner::begin( template bool TPlanner::step(uint32_t max_edges) { - for (; max_edges > 0; max_edges--) { + while (max_edges > 0) { if (this->preparing) { - this->prepare(); + max_edges -= std::min(this->prepare(), max_edges); continue; } @@ -147,6 +147,7 @@ bool TPlanner::step(uint32_t max_edges) { } this->advance_expansion(); + max_edges--; } return this->is_finished(); @@ -389,7 +390,7 @@ bool TPlanner::is_comparable(uint8_t first, uint8_t second) const } template -void TPlanner::prepare() { +uint32_t TPlanner::prepare() { while (this->prepare_first < number_of_arrivals and not this->is_comparable(this->prepare_first, this->prepare_second)) { this->prepare_second++; @@ -402,13 +403,16 @@ void TPlanner::prepare() { if (this->prepare_first == number_of_arrivals) { this->preparing = false; - return; + return 0; } + const bool diagonal = ends_diagonal(this->prepare_first); + const uint32_t cost = 2U * this->number_of_usable_turns.at(diagonal ? 1 : 0) + (diagonal ? 0U : this->goal_length); + this->compare_arrivals(this->prepare_first, this->prepare_second, this->prepare_run); this->prepare_run++; - const uint8_t longest = ends_diagonal(this->prepare_first) ? max_run : max_straight_run; + const uint8_t longest = diagonal ? max_run : max_straight_run; if (this->prepare_run > longest or std::isinf(this->gaps.at(this->prepare_first).at(this->prepare_second))) { this->prepare_run = 0; @@ -419,6 +423,8 @@ void TPlanner::prepare() { this->prepare_first++; } } + + return std::max(cost, 1); } template diff --git a/micras_nav/include/micras/nav/impl/racing_line.tpp b/micras_nav/include/micras/nav/impl/racing_line.tpp index 9531f86..d685a53 100644 --- a/micras_nav/include/micras/nav/impl/racing_line.tpp +++ b/micras_nav/include/micras/nav/impl/racing_line.tpp @@ -73,7 +73,6 @@ void TRacingLine::begin( this->maze = &maze; this->segments = segments; this->run_profile = profile; - this->least_margin = profile.risky ? this->config.risky_least_margin : this->config.least_margin; this->openings.fill(0); this->sweeps = 0; @@ -409,7 +408,7 @@ void TRacingLine::check(uint32_t& budget) { if (not this->is_clear( {.x = this->line.xs.at(this->cursor), .y = this->line.ys.at(this->cursor)}, std::cos(heading), - std::sin(heading), this->least_margin + std::sin(heading), this->config.least_margin )) { this->finish(false); return; diff --git a/micras_nav/include/micras/nav/impl/wall_observer.tpp b/micras_nav/include/micras/nav/impl/wall_observer.tpp index 3abfdf7..6b61f77 100644 --- a/micras_nav/include/micras/nav/impl/wall_observer.tpp +++ b/micras_nav/include/micras/nav/impl/wall_observer.tpp @@ -45,7 +45,7 @@ bool TWallObserver::update( for (uint8_t sensor = 0; sensor < number_of_wall_sensors; sensor++) { const WallReading& reading = measurements.walls.at(sensor); - if (not reading.is_new) { + if (not reading.is_new or reading.blind) { continue; } diff --git a/micras_nav/include/micras/nav/localizer.hpp b/micras_nav/include/micras/nav/localizer.hpp index 292f75b..e91dbd6 100644 --- a/micras_nav/include/micras/nav/localizer.hpp +++ b/micras_nav/include/micras/nav/localizer.hpp @@ -57,12 +57,15 @@ class Localizer { * the fast ranges went through. That filter is slower than the ranges are sampled, so the noise * of consecutive ranges is mostly the same noise, and the range correlation is how many of them * share it: the variance of each is multiplied by it, so that what a second of ranges is worth - * does not depend on how many there are in it. The speed window is the number of iterations the linear - * speed is measured over, which trades noise for delay. The rest window is the time over which - * the gyroscope is compared with the encoders to measure its bias. The maximum range is the - * longest range a reading corrects the pose from: past it the beam of the emitter grows wide - * enough to land partly on the floor, the reading loses light the model of the sensor does not - * know about, and the range comes out long by several percent. + * does not depend on how many there are in it. The speed window is the number of iterations the + * linear speed is measured over, which trades noise for delay. The rest window is the time over + * which the gyroscope is compared with the encoders to measure its bias. The maximum range is + * the longest range a reading corrects the pose from: past it the beam of the emitter grows + * wide enough to land partly on the floor, the reading loses light the model of the sensor does + * not know about, and the range comes out long by several percent. A reading is used when + * either the range it reads or the range the pose predicts is within it, so that a pose that + * drifted away from a wall has its readings rejected, and counted towards a recovery, instead + * of dropped. * * The ends of the side walls are used as references along the path. An end is accepted while * the robot moves forward faster than the edge speed, since the only time it reverses is to diff --git a/micras_nav/include/micras/nav/measurements.hpp b/micras_nav/include/micras/nav/measurements.hpp index 6c1bf68..3384300 100644 --- a/micras_nav/include/micras/nav/measurements.hpp +++ b/micras_nav/include/micras/nav/measurements.hpp @@ -18,6 +18,8 @@ namespace micras::nav { * sees anything above its noise, so a reading that is not valid means that nothing is within range. A receiver that * saturates reports the shortest distance it can measure. * + * A blind reading is one whose receiver ambient light saturates, which says nothing about a wall. + * * @note The new flag is set for a single iteration after the sensor produces a value. An estimator * that is fed the same sample twice counts it as two independent observations and becomes * overconfident, so that is who needs it. @@ -25,6 +27,7 @@ namespace micras::nav { struct WallReading { float distance; bool valid; + bool blind; bool is_new; }; diff --git a/micras_nav/include/micras/nav/mission.hpp b/micras_nav/include/micras/nav/mission.hpp index 847c6da..053cbf8 100644 --- a/micras_nav/include/micras/nav/mission.hpp +++ b/micras_nav/include/micras/nav/mission.hpp @@ -42,7 +42,9 @@ namespace micras::nav { * straight may start with the wall ahead still unknown, since that wall only comes into range * inside the cell, but then it is watched: if it turns out to be there, or is still unknown at the * last point from which the robot can stop at the center of the cell, the robot stops there and - * decides again. A wall seen late costs a stop instead of a crash. + * decides again. A wall seen late costs a stop instead of a crash. Up to that point the straight is + * held to the search speed, which the stop is computed from, and past it the straight is free, + * since by then the wall is either known to be absent or the robot is already stopping. * * The mission keeps its own account of which cell the robot is in, from the moves it made. Whenever * the robot stands still at the center of a cell, that account is checked against the pose estimate, @@ -236,7 +238,8 @@ class TMission { static constexpr uint8_t max_move_segments{6}; /** - * @brief Distance within which a segment is taken to start where the watched cell is entered. + * @brief Distance within which a segment is taken to start on the part of the watched cell that + * is watched, which is where the cell is entered or the point the straight is split at. */ static constexpr float watch_tolerance{0.001F}; @@ -328,8 +331,18 @@ class TMission { /** * @brief Stop at the center of the cell being crossed instead of going through it. + * + * @param travelled The distance the reference covered since the robot crossed into the cell. + */ + void divert_to_center(float travelled); + + /** + * @brief Get the distance into a cell of the last point from which the robot, crossing it at + * the search speed, can still stop at its center. + * + * @return The distance from the edge of the cell, less the commit margin. */ - void divert_to_center(); + float get_commit_distance() const; /** * @brief Make a segment. diff --git a/micras_nav/include/micras/nav/planner.hpp b/micras_nav/include/micras/nav/planner.hpp index b569cd8..3a0e0e4 100644 --- a/micras_nav/include/micras/nav/planner.hpp +++ b/micras_nav/include/micras/nav/planner.hpp @@ -62,10 +62,11 @@ struct Route { * fastest path of the graph is the fastest route the robot can drive, to within what the velocity * planner refines among the candidates. A turn the run cannot reach its speed for is priced at a * blend of the two speeds, since the robot accelerates inside it, and a turn that has to leave - * slower for the next one costs nothing more, since the robot slows in its exit ramp. Over thousands - * of mazes that prices a route to within about 20 ms of what the velocity planner makes of it. - * Diagonals, the turns of 135 degrees and the two sizes of the turn of 90 degrees are part of the - * search rather than a rewrite of its result. + * slower for the next one costs nothing more, since the robot slows in its exit ramp. Over + * thousands of mazes the price of a route differs from what the velocity planner makes of it by + * 20 ms at one standard deviation, and by a few tens of milliseconds at worst. Diagonals, the turns + * of 135 degrees and the two sizes of the turn of 90 degrees are part of the search rather than a + * rewrite of its result. * * What the search holds are labels: a way of reaching a node, with its cost, the turn that reached * it and how fast that turn is driven, since those fix the speed the next straight starts at and @@ -136,7 +137,8 @@ class TPlanner { * @brief Advance the search. * * @note Before the search itself, every pair of arrivals is compared for the dominance, one run - * of one pair for each edge of the budget. + * of one pair at a time, each charged to the budget for the edges it times, so that no call + * takes longer than the budget allows. * * @param max_edges The largest number of edges to try in this call. * @return True if the search has finished. @@ -449,8 +451,11 @@ class TPlanner { /** * @brief Compare the next pair of arrivals over the next run, moving on to the next pair after. + * + * @return The number of edges the comparison may time, at least one, or zero once every pair + * was compared. */ - void prepare(); + uint32_t prepare(); /** * @brief Bound how much slower one arrival at full speed can be than another, over one run. diff --git a/micras_nav/include/micras/nav/racing_line.hpp b/micras_nav/include/micras/nav/racing_line.hpp index fc48e96..e913c18 100644 --- a/micras_nav/include/micras/nav/racing_line.hpp +++ b/micras_nav/include/micras/nav/racing_line.hpp @@ -57,24 +57,23 @@ class TRacingLine { * @brief Configuration struct for the racing line. * * @note The margin is the distance the line keeps between the outline of the robot and any - * obstacle wherever the route leaves room for it, with the risky switch too. The least margins - * are what the final check requires everywhere, without and with the risky switch: the margins - * the turns were designed with, less what the resampling and the headings of the final line use - * up. Where the route itself comes closer than the margin, which the risky turns do, the line - * does not move. The trust is how far a sample may slide in one sweep, the scan step the - * resolution of the bounds, and the length weight the weight of the first differences, in units - * of the spacing. A sweep that slides no sample farther than the convergence tolerance ends the - * optimization. The lateral share is the part of the lateral grip of the run the curves of the - * line are planned with: the tires slide sideways in proportion to what they are asked for, - * which the pose estimate only predicts, and the line holds its curves far longer than a turn - * does. + * obstacle wherever the route leaves room for it, with the risky switch too. The least margin + * is what the final check requires everywhere, whatever the switch: the margin the turns were + * designed with, less what the resampling and the headings of the final line use up. Where the + * route itself comes closer than the margin, which the risky turns do, the line does not move, + * and so it fails the check there, since a line that close to the walls grazed them. The trust + * is how far a sample may slide in one sweep, the scan step the resolution of the bounds, and + * the length weight the weight of the first differences, in units of the spacing. A sweep that + * slides no sample farther than the convergence tolerance ends the optimization. The lateral + * share is the part of the lateral grip of the run the curves of the line are planned with: the + * tires slide sideways in proportion to what they are asked for, which the pose estimate only + * predicts, and the line holds its curves far longer than a turn does. */ // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) no defaults, so that a missing field is a warning struct Config { float spacing; float margin; float least_margin; - float risky_least_margin; float trust; float scan_step; float length_weight; @@ -366,11 +365,6 @@ class TRacingLine { */ RunProfile run_profile{}; - /** - * @brief Margin the line must keep everywhere, for the run. - */ - float least_margin{}; - /** * @brief Phase of the optimization. */ diff --git a/micras_nav/include/micras/nav/wall_observer.hpp b/micras_nav/include/micras/nav/wall_observer.hpp index e88be75..0c5eb71 100644 --- a/micras_nav/include/micras/nav/wall_observer.hpp +++ b/micras_nav/include/micras/nav/wall_observer.hpp @@ -22,7 +22,9 @@ namespace micras::nav { * would lie on the face of that wall, allowing for how uncertain the pose is, which the wall model * answers from the geometry of the sensors. Every reading taken in those conditions is a vote: * for the wall when something is seen at the range the wall would be at, against it when nothing is - * seen up to beyond that range. A wall is decided when enough votes agree, and it is decided once. + * seen up to beyond that range. A blind reading does not vote, since a receiver that ambient light + * saturates sees nothing whether the wall is there or not. A wall is decided when enough votes + * agree, and it is decided once. * A wall that never gets enough votes stays unknown rather than being guessed. * * @note Every sensor votes on whatever wall it happens to point at, so the wall ahead is decided by diff --git a/micras_nav/src/controller.cpp b/micras_nav/src/controller.cpp index e36a3b4..2cce4fa 100644 --- a/micras_nav/src/controller.cpp +++ b/micras_nav/src/controller.cpp @@ -49,8 +49,7 @@ Controller::Controller(const Config& config) : Controller::Command Controller::update(const Reference& unscaled, const State& estimate, float elapsed_time) { const RobotModel& model = this->config.model; - const float step = this->config.max_time_scale_rate * elapsed_time; - const float scale = std::clamp(this->find_time_scale(unscaled), this->time_scale - step, this->time_scale + step); + const float scale = this->find_next_time_scale(unscaled, elapsed_time); const float scale_rate = (scale - this->time_scale) / elapsed_time; this->time_scale = scale; @@ -117,29 +116,40 @@ Controller::Command Controller::update(const Reference& unscaled, const State& e }; } -float Controller::find_time_scale(const Reference& reference) const { - const RobotModel& model = this->config.model; +float Controller::find_next_time_scale(const Reference& reference, float elapsed_time) const { + const float half_track = this->config.model.chassis.track_width / 2.0F; + const float fastest_wheel = std::abs(reference.twist.linear) + std::abs(reference.twist.angular) * half_track; + const float largest_change = this->config.max_time_scale_acceleration * elapsed_time; + const float step = largest_change / std::max(fastest_wheel, largest_change); + const float target = this->find_time_scale(reference); - const float half_track = model.chassis.track_width / 2.0F; - const float available = model.drive.supply_voltage * (1.0F - this->config.voltage_reserve); + if (target <= this->time_scale) { + return std::max(target, this->time_scale - step); + } - std::array, 2> wheels{}; + const float highest = std::min(target, this->time_scale + step); + const float available = this->get_available_voltage(); - for (uint8_t i = 0; i < 2; i++) { - const float side = i == 0 ? -1.0F : 1.0F; - const float speed = reference.twist.linear + side * reference.twist.angular * half_track; + float rate = (highest - this->time_scale) / elapsed_time; - wheels.at(i) = { - model.drive.static_friction_voltage * std::clamp(speed / this->config.friction_speed, -1.0F, 1.0F), - model.speed_constant() * reference.twist.linear + - side * model.angular_speed_constant() * reference.twist.angular, - model.acceleration_constant() * reference.acceleration.linear + - side * model.angular_acceleration_constant() * reference.acceleration.angular, - }; + for (const std::array& wheel : this->get_wheel_terms(reference)) { + const float voltage = wheel.at(0) + highest * (wheel.at(1) + highest * wheel.at(2)); + const float rate_term = std::abs(wheel.at(3)); + + if (rate_term > 0.0F) { + rate = std::min(rate, std::max((available - std::copysign(voltage, wheel.at(3))) / rate_term, 0.0F)); + } } + return this->time_scale + rate * elapsed_time; +} + +float Controller::find_time_scale(const Reference& reference) const { + const std::array, 2> wheels = this->get_wheel_terms(reference); + const float available = this->get_available_voltage(); + const auto fits = [&wheels, available](float scale) { - return std::ranges::all_of(wheels, [scale, available](const std::array& wheel) { + return std::ranges::all_of(wheels, [scale, available](const std::array& wheel) { return std::abs(wheel.at(0) + scale * (wheel.at(1) + scale * wheel.at(2))) <= available * 1.0001F; }); }; @@ -150,7 +160,7 @@ float Controller::find_time_scale(const Reference& reference) const { float best = min_time_scale; - for (const std::array& wheel : wheels) { + for (const std::array& wheel : wheels) { for (const float limit : {available, -available}) { for (const float root : solve_quadratic(wheel.at(2), wheel.at(1), wheel.at(0) - limit)) { if (root > best and root < 1.0F and fits(root)) { @@ -163,6 +173,35 @@ float Controller::find_time_scale(const Reference& reference) const { return best; } +std::array, 2> Controller::get_wheel_terms(const Reference& reference) const { + const RobotModel& model = this->config.model; + + const float half_track = model.chassis.track_width / 2.0F; + + std::array, 2> wheels{}; + + for (uint8_t i = 0; i < 2; i++) { + const float side = i == 0 ? -1.0F : 1.0F; + const float speed = reference.twist.linear + side * reference.twist.angular * half_track; + + wheels.at(i) = { + model.drive.static_friction_voltage * std::clamp(speed / this->config.friction_speed, -1.0F, 1.0F), + model.speed_constant() * reference.twist.linear + + side * model.angular_speed_constant() * reference.twist.angular, + model.acceleration_constant() * reference.acceleration.linear + + side * model.angular_acceleration_constant() * reference.acceleration.angular, + model.acceleration_constant() * reference.twist.linear + + side * model.angular_acceleration_constant() * reference.twist.angular, + }; + } + + return wheels; +} + +float Controller::get_available_voltage() const { + return this->config.model.drive.supply_voltage * (1.0F - this->config.voltage_reserve); +} + float Controller::get_time_scale() const { return this->time_scale; } diff --git a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp index 77d3298..98a05c7 100644 --- a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp +++ b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp @@ -30,6 +30,7 @@ TWallSensors::TWallSensors(const Config& config) : noise_floor{config.noise_floor}, max_distance{config.max_distance}, max_reading{config.max_reading}, + blind_reading{config.blind_reading}, wall_distance{config.wall_distance}, wall_hysteresis{config.wall_hysteresis}, calibration_samples{config.calibration_samples}, @@ -137,6 +138,7 @@ void TWallSensors::update() { this->max_distance; reading.valid = distance < this->max_distance; + reading.blind = this->get_dark_reading(i) >= this->blind_reading; reading.distance = this->fast_filters.at(i).update(std::min(distance, this->max_distance)); reading.slow_distance = this->slow_filters.at(i).update(std::min(distance, this->max_distance)); @@ -184,6 +186,12 @@ float TWallSensors::get_intensity(uint8_t sensor_index) const { this->adc.get_max_reading(); } +template +float TWallSensors::get_dark_reading(uint8_t sensor_index) const { + return static_cast(std::min(this->scans.at(sensor_index), this->scans.at(sensor_index + num_of_sensors))) / + this->adc.get_max_reading(); +} + template void TWallSensors::calibrate_sensor(uint8_t sensor_index) { this->calibrations.at(sensor_index) = { diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 34eea1b..669210b 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -117,6 +117,8 @@ class Imu { /** * @brief Convert the registers that the last transfer brought. + * + * @note A status that the sensor cannot have sent brings no sample. */ void read_response(); @@ -174,6 +176,15 @@ class Imu { static constexpr uint8_t gyroscope_ready{0x02}; ///@} + /** + * @brief Bits of the status register that always read zero. + * + * @note A status with any of them set did not come from the sensor: a data line held high, as + * when the sensor does not answer, reads all ones, which would otherwise look like a new + * sample. + */ + static constexpr uint8_t unused_status_bits{0x48}; + /** * @brief Conversion constants. */ diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index 2456c40..e352c6e 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -46,7 +46,9 @@ class TWallSensors { * the distance of the maximum reading. A wall is considered present when the slow distance is * below the wall distance, and absent again when it goes above it by the hysteresis. The * receiver offset is the distance from the optical axis of the emitter to the receiver, and the - * receiver half angle the angle off its own axis at which the receiver's sensitivity halves. + * receiver half angle the angle off its own axis at which the receiver's sensitivity halves. A + * dark reading, taken with the emitter off, at or above the blind reading means that ambient + * light saturates the receiver, which then cannot see a wall with the emitter on either. */ struct Config { hal::AdcDma::Config adc; @@ -61,6 +63,7 @@ class TWallSensors { float noise_floor; float max_reading; float max_distance; + float blind_reading; float wall_distance; float wall_hysteresis; uint16_t calibration_samples; @@ -71,12 +74,14 @@ class TWallSensors { * * @note The fast distance is for everything that is a position and the slow one for deciding * whether there is a wall. The reading is valid when the sensor sees anything above its noise, - * and it is new for a single update after the sensor produces a value. + * and it is new for a single update after the sensor produces a value. It is blind when ambient + * light saturates the receiver, and then says nothing about a wall. */ struct Reading { float distance; float slow_distance; bool valid; + bool blind; bool is_new; }; @@ -130,6 +135,17 @@ class TWallSensors { */ float get_intensity(uint8_t sensor_index) const; + /** + * @brief Get the reading of a sensor with its emitter off, as a fraction of the full scale. + * + * @note Ambient light raises the output of the receiver, and the emitter raises it further, so + * the dark reading is the lower of the pair. + * + * @param sensor_index Index of the sensor. + * @return The dark reading. + */ + float get_dark_reading(uint8_t sensor_index) const; + /** * @brief Start calibrating a sensor, with the robot placed at the reference distance. * @@ -320,6 +336,11 @@ class TWallSensors { */ float max_reading; + /** + * @brief Dark reading at and above which the receiver is blinded by ambient light. + */ + float blind_reading; + /** * @brief Slow distance below which a wall is present. */ diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 430045f..9d6654f 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -93,6 +93,10 @@ void Imu::read_response() { const uint8_t status = std::get<1>(this->response); + if ((status & unused_status_bits) != 0) { + return; + } + if ((status & accelerometer_ready) != 0) { for (uint8_t axis = 0; axis < 3; axis++) { this->linear_acceleration.at(axis) = get_axis(acceleration_offset, axis) * this->xl_factor; From fb1d9ef8496fdff1ec5fef5894b75a920a58e5b4 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Sep 2026 17:45:21 -0300 Subject: [PATCH 523/561] =?UTF-8?q?=F0=9F=90=9B=20Answer=20the=20third=20r?= =?UTF-8?q?eview=20of=20the=20navigation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The third review of #54 found a sign error in the rise of the time scale, a rise that ignores the feedback, a preparation of the planner charged far above its cost, and a racing line pass that never wins. What changed: - The rise of the time scale keeps the voltage of each wheel within the supply whichever way the wheel turns: the limit took the magnitude of the voltage where it needed its sign, and let a wheel turning backwards saturate. - The time scale does not rise while the last command saturated the motors, since the feedback then needs the voltage the rise would take. The controller forgets its scale and its saturation when a run starts. With the supply at 8 V in both models of the simulator, 8 of 20 fast runs end in ERROR instead of 20, and with a reserve of 30 % none does, as before. - Each comparison of the preparation is charged only for the edges it times that were not in the cache, about a tenth of what it was charged, so the explorer's rounds are as short as before db65a19 and the cap per iteration stays. - With risky, the racing line goes only through the route planned without the risky turns. A line keeps the margin of the careful turns, so the line through the risky route never passed, and planning it took almost half of the planning. - A watched straight that is not split keeps the search speed, should the point to commit at ever come before the cell. In the simulator the contest stays clean with the fan and with every switch, with the same fast runs, the search at 859 s over the ten mazes instead of 867 s, and the planning with every switch in half the time. --- micras_nav/include/micras/nav/controller.hpp | 14 ++++- .../include/micras/nav/impl/mission.tpp | 52 +++++++++---------- .../include/micras/nav/impl/planner.tpp | 6 ++- micras_nav/include/micras/nav/mission.hpp | 35 +++++-------- micras_nav/include/micras/nav/planner.hpp | 14 +++-- micras_nav/src/controller.cpp | 17 +++++- 6 files changed, 79 insertions(+), 59 deletions(-) diff --git a/micras_nav/include/micras/nav/controller.hpp b/micras_nav/include/micras/nav/controller.hpp index 0f7d5c0..efc4ef6 100644 --- a/micras_nav/include/micras/nav/controller.hpp +++ b/micras_nav/include/micras/nav/controller.hpp @@ -103,6 +103,11 @@ class Controller { */ explicit Controller(const Config& config); + /** + * @brief Forget the time scale and the saturation of the last run, before the next one starts. + */ + void reset(); + /** * @brief Compute the command for this instant. * @@ -163,7 +168,9 @@ class Controller { * * @note The scale falls as fast as the time scale acceleration allows. It rises no faster than * that either, and only as fast as the voltage left over at the new scale lets through the - * acceleration the rise adds, so that rising never saturates the motors by itself. + * acceleration the rise adds, so that rising never saturates the motors by itself. It does not + * rise at all while the last command saturated the motors, since the feedback is then + * catching up with the reference and needs the voltage the rise would take. * * @param reference What the robot should be doing, at the full pace. * @param elapsed_time Time since the last iteration. @@ -229,6 +236,11 @@ class Controller { */ static constexpr float min_time_scale{0.1F}; + /** + * @brief Whether the last command asked more of a wheel than the supply gives. + */ + bool saturated{}; + /** * @brief Errors and terms of the last update. */ diff --git a/micras_nav/include/micras/nav/impl/mission.tpp b/micras_nav/include/micras/nav/impl/mission.tpp index 1fbbf7e..78b4da2 100644 --- a/micras_nav/include/micras/nav/impl/mission.tpp +++ b/micras_nav/include/micras/nav/impl/mission.tpp @@ -77,8 +77,7 @@ void TMission::begin_plan(const RunProfile& profile) { this->planner.begin(this->maze, WallAssumption::PESSIMISTIC, profile); this->racing_line.reset(); this->stage = PlanStage::SEARCH; - this->pass = LinePass::REQUESTED; - this->risky_line_time = std::numeric_limits::infinity(); + this->careful_search = false; } template @@ -92,7 +91,7 @@ bool TMission::update_plan(uint32_t max_edges) { return false; } - if (this->pass == LinePass::REQUESTED) { + if (not this->careful_search) { this->route_time = this->choose_route(this->solve_profile, this->solve_route, this->solve_segments); if (not this->solve_profile.racing_line or this->solve_segments.empty()) { @@ -100,6 +99,15 @@ bool TMission::update_plan(uint32_t max_edges) { return true; } + if (this->solve_profile.risky) { + RunProfile careful = this->solve_profile; + careful.risky = false; + + this->careful_search = true; + this->planner.begin(this->maze, WallAssumption::PESSIMISTIC, careful); + return false; + } + this->racing_line.begin(this->solve_route, this->solve_segments, this->maze, this->solve_profile); } else { RunProfile careful = this->solve_profile; @@ -107,11 +115,11 @@ bool TMission::update_plan(uint32_t max_edges) { this->choose_route(careful, this->careful_route, this->careful_segments); if (this->careful_segments.empty()) { - this->pass = LinePass::AGAIN; - this->racing_line.begin(this->solve_route, this->solve_segments, this->maze, this->solve_profile); - } else { - this->racing_line.begin(this->careful_route, this->careful_segments, this->maze, careful); + this->stage = PlanStage::IDLE; + return true; } + + this->racing_line.begin(this->careful_route, this->careful_segments, this->maze, careful); } this->stage = PlanStage::LINE; @@ -126,29 +134,11 @@ bool TMission::update_plan(uint32_t max_edges) { } const Line& line = this->racing_line.get_line(); - const float line_time = line.is_ready() ? line.get_finish_time() : std::numeric_limits::infinity(); - - if (this->pass == LinePass::REQUESTED and this->solve_profile.risky) { - RunProfile careful = this->solve_profile; - careful.risky = false; - - this->risky_line_time = line_time; - this->pass = LinePass::CAREFUL; - this->planner.begin(this->maze, WallAssumption::PESSIMISTIC, careful); - this->stage = PlanStage::SEARCH; - return false; - } - - if (this->pass == LinePass::CAREFUL and this->risky_line_time < line_time) { - this->pass = LinePass::AGAIN; - this->racing_line.begin(this->solve_route, this->solve_segments, this->maze, this->solve_profile); - return false; - } this->stage = PlanStage::IDLE; - if (line.is_ready() and line_time < this->route_time) { - this->route_time = line_time; + if (line.is_ready() and line.get_finish_time() < this->route_time) { + this->route_time = line.get_finish_time(); this->solve_segments.clear(); this->solve_segments.push_back(make_segment(SegmentKind::LINE, line.length(), line.get_start())); } @@ -422,7 +412,13 @@ void TMission::decide_at_entry() { entry.compose({.position = {.x = commit_distance, .y = 0.0F}, .orientation = 0.0F}) )); } else { - move.add(make_segment(SegmentKind::STRAIGHT, cell_size, entry)); + Segment straight = make_segment(SegmentKind::STRAIGHT, cell_size, entry); + + if (this->watching_front) { + straight.max_speed = this->search_speed; + } + + move.add(straight); } this->execute(move, this->search_speed, this->search_speed); diff --git a/micras_nav/include/micras/nav/impl/planner.tpp b/micras_nav/include/micras/nav/impl/planner.tpp index 1072365..417c522 100644 --- a/micras_nav/include/micras/nav/impl/planner.tpp +++ b/micras_nav/include/micras/nav/impl/planner.tpp @@ -353,6 +353,8 @@ TPlanner::Edge return {.cost = cached_cost, .speed_ratio = cached_speed_ratio}; } + this->computed_edges++; + const float cell_size = this->dynamics.get_model().maze.cell_size; const float step = ends_diagonal(arrival) ? cell_size / std::numbers::sqrt2_v : cell_size; @@ -407,11 +409,13 @@ uint32_t TPlanner::prepare() { } const bool diagonal = ends_diagonal(this->prepare_first); - const uint32_t cost = 2U * this->number_of_usable_turns.at(diagonal ? 1 : 0) + (diagonal ? 0U : this->goal_length); + const uint32_t computed_before = this->computed_edges; this->compare_arrivals(this->prepare_first, this->prepare_second, this->prepare_run); this->prepare_run++; + const uint32_t cost = this->computed_edges - computed_before + (diagonal ? 0U : this->goal_length); + const uint8_t longest = diagonal ? max_run : max_straight_run; if (this->prepare_run > longest or std::isinf(this->gaps.at(this->prepare_first).at(this->prepare_second))) { diff --git a/micras_nav/include/micras/nav/mission.hpp b/micras_nav/include/micras/nav/mission.hpp index 053cbf8..ab0391c 100644 --- a/micras_nav/include/micras/nav/mission.hpp +++ b/micras_nav/include/micras/nav/mission.hpp @@ -142,9 +142,10 @@ class TMission { * @note Only to be called with the robot stopped: when the search ends, the best candidates * are compiled and timed, which is not bounded to fit in an iteration. When the profile asks * for the racing line, it is optimized next, through the cells of the route chosen, and it - * replaces the route if it is found and faster. With the risky switch too, a second line goes - * through the route planned without the risky turns, and the risky line is only driven if it - * is the faster of the two; it is then optimized again, since only one line is kept. + * replaces the route if it is found and faster. With the risky switch too, the line goes + * through the route planned without the risky turns instead, from a second search: a line + * keeps the margin of the careful turns, which a line through the risky ones never does, and + * it replaces the risky route only if it is faster. * * @param max_edges The largest number of edges the planner may try in this call. * @return True if the planning has finished. @@ -222,16 +223,6 @@ class TMission { LINE = 2, }; - /** - * @brief Which racing line is being optimized: through the route of the profile asked for, - * through the route planned without the risky turns, or the first one again. - */ - enum class LinePass : uint8_t { - REQUESTED = 0, - CAREFUL = 1, - AGAIN = 2, - }; - /** * @brief Largest number of segments a move through one cell takes. */ @@ -477,30 +468,28 @@ class TMission { Route solve_route; /** - * @brief Route planned without the risky turns, and its segments, for the second racing line. + * @brief Route planned without the risky turns, and its segments, for the racing line of a run + * with the risky switch. */ ///@{ Route careful_route; std::vector careful_segments; ///@} - /** - * @brief Time of the racing line through the risky route, to compare the second one with. - */ - float risky_line_time{}; - /** * @brief Time the planned fast run is expected to take. */ float route_time{}; /** - * @brief What the planning of a fast run is doing, and which racing line it is at. + * @brief What the planning of a fast run is doing. */ - ///@{ PlanStage stage{PlanStage::IDLE}; - LinePass pass{LinePass::REQUESTED}; - ///@} + + /** + * @brief Whether the search running is the one for the route without the risky turns. + */ + bool careful_search{}; /** * @brief Last reference produced. diff --git a/micras_nav/include/micras/nav/planner.hpp b/micras_nav/include/micras/nav/planner.hpp index 3a0e0e4..1c58297 100644 --- a/micras_nav/include/micras/nav/planner.hpp +++ b/micras_nav/include/micras/nav/planner.hpp @@ -137,8 +137,8 @@ class TPlanner { * @brief Advance the search. * * @note Before the search itself, every pair of arrivals is compared for the dominance, one run - * of one pair at a time, each charged to the budget for the edges it times, so that no call - * takes longer than the budget allows. + * of one pair at a time, each charged to the budget for the edges it times that were not + * timed before, so that no call takes longer than the budget allows. * * @param max_edges The largest number of edges to try in this call. * @return True if the search has finished. @@ -452,8 +452,8 @@ class TPlanner { /** * @brief Compare the next pair of arrivals over the next run, moving on to the next pair after. * - * @return The number of edges the comparison may time, at least one, or zero once every pair - * was compared. + * @return The number of edges the comparison timed, counting each way into the goal as one, + * at least one, or zero once every pair was compared. */ uint32_t prepare(); @@ -624,6 +624,12 @@ class TPlanner { */ uint8_t goal_length{}; + /** + * @brief Number of edges timed rather than read from the cache, of which only differences + * count. + */ + uint32_t computed_edges{}; + /** * @brief Edge of every pair of an arrival at full speed and a turn, for every run, filled in when * first needed. diff --git a/micras_nav/src/controller.cpp b/micras_nav/src/controller.cpp index 2cce4fa..01c3150 100644 --- a/micras_nav/src/controller.cpp +++ b/micras_nav/src/controller.cpp @@ -46,6 +46,11 @@ Controller::Controller(const Config& config) : config.angular, config.model.angular_speed_constant(), config.model.angular_acceleration_constant() )} { } +void Controller::reset() { + this->time_scale = 1.0F; + this->saturated = false; +} + Controller::Command Controller::update(const Reference& unscaled, const State& estimate, float elapsed_time) { const RobotModel& model = this->config.model; @@ -110,10 +115,14 @@ Controller::Command Controller::update(const Reference& unscaled, const State& e const float to_percent = 100.0F / model.drive.supply_voltage; - return { + const Command command{ .forward = to_percent * (forward_feed_forward + forward_feedback), .rotation = to_percent * (rotation_feed_forward + rotation_feedback), }; + + this->saturated = std::abs(command.forward) + std::abs(command.rotation) > 100.0F; + + return command; } float Controller::find_next_time_scale(const Reference& reference, float elapsed_time) const { @@ -127,6 +136,10 @@ float Controller::find_next_time_scale(const Reference& reference, float elapsed return std::max(target, this->time_scale - step); } + if (this->saturated) { + return this->time_scale; + } + const float highest = std::min(target, this->time_scale + step); const float available = this->get_available_voltage(); @@ -137,7 +150,7 @@ float Controller::find_next_time_scale(const Reference& reference, float elapsed const float rate_term = std::abs(wheel.at(3)); if (rate_term > 0.0F) { - rate = std::min(rate, std::max((available - std::copysign(voltage, wheel.at(3))) / rate_term, 0.0F)); + rate = std::min(rate, std::max((available - std::copysign(1.0F, wheel.at(3)) * voltage) / rate_term, 0.0F)); } } From 788a44ed7e349e06f423ca369c6c4fd2e026609a Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Sep 2026 18:06:32 -0300 Subject: [PATCH 524/561] =?UTF-8?q?=E2=9C=A8=20Brake=20no=20harder=20than?= =?UTF-8?q?=20the=20motors=20can?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The speed profile accelerated along the motor curve but braked at the grip of the tires alone, on the grounds that the back EMF helps braking. It does, but only on top of what the winding lets through: a motor brakes from rest as hard as it accelerates from rest, and the harder the faster it turns. So a motor that cannot accelerate the robot from rest as hard as the tires allow cannot brake it that hard at low speed either, and a plan that brakes at the grip there leaves the robot ahead of its reference with the motors saturated. Braking is now bounded by the tires down to the speed where the motors fall below them, and by the motors under it, the deceleration growing with the speed as rate * (motor speed + speed). That is one more phase of the profile, of the same exponential form as the motor limited acceleration. The distance to brake, the speed that can be braked from, the point a watched straight commits at and the braking along a curve all follow it. The motors of today's robot brake harder than the tires from rest, so nothing changes for it: every run of the contest in the simulator is byte identical. With the winding resistance tripled in both models of the simulator and a reserve of 30 %, 2 of 20 fast runs end in ERROR instead of 7, both crashes where the robot cannot turn as fast as a curve asks, since the turns are not limited by the motors yet. --- .../include/micras/nav/impl/mission.tpp | 5 +- .../include/micras/nav/motion_limits.hpp | 25 ++++++- .../include/micras/nav/speed_profile.hpp | 19 ++++- micras_nav/src/motion_limits.cpp | 12 +++- micras_nav/src/speed_profile.cpp | 71 +++++++++++++++++-- 5 files changed, 116 insertions(+), 16 deletions(-) diff --git a/micras_nav/include/micras/nav/impl/mission.tpp b/micras_nav/include/micras/nav/impl/mission.tpp index 78b4da2..70aee4a 100644 --- a/micras_nav/include/micras/nav/impl/mission.tpp +++ b/micras_nav/include/micras/nav/impl/mission.tpp @@ -613,8 +613,9 @@ void TMission::divert_to_center(float travelled) { template float TMission::get_commit_distance() const { const float cell_size = this->dynamics.get_model().maze.cell_size; - const float braking = this->search_speed * this->search_speed / - (2.0F * this->dynamics.get_linear_limits(this->config.search_profile).deceleration); + const float braking = SpeedProfile::get_braking_distance( + this->search_speed, 0.0F, this->dynamics.get_linear_limits(this->config.search_profile) + ); return cell_size / 2.0F - braking - this->config.commit_margin; } diff --git a/micras_nav/include/micras/nav/motion_limits.hpp b/micras_nav/include/micras/nav/motion_limits.hpp index 4c40036..2f25bab 100644 --- a/micras_nav/include/micras/nav/motion_limits.hpp +++ b/micras_nav/include/micras/nav/motion_limits.hpp @@ -31,8 +31,11 @@ struct RunProfile { * * @details The acceleration is bounded by the tires up to the speed where the motors take over, and * from there it falls linearly with the speed, reaching zero at the free running speed, which is - * how a DC motor fed from a fixed voltage behaves. Braking is not limited by the motors, since the - * back EMF helps it. + * how a DC motor fed from a fixed voltage behaves. The back EMF helps braking instead, so the + * braking the motors can give grows with the speed, from what they give accelerating from rest. + * Braking is bounded by the tires down to the speed where that falls below them, and by the motors + * under it, which only happens when the motors cannot accelerate the robot from rest as hard as the + * tires allow. */ struct MotionLimits { /** @@ -50,6 +53,21 @@ struct MotionLimits { */ float crossover_speed() const; + /** + * @brief Get the largest deceleration available at a speed. + * + * @param speed The current speed. + * @return The deceleration limit at that speed, as a positive number. + */ + float deceleration_at(float speed) const; + + /** + * @brief Get the speed under which the motors, rather than the tires, limit the braking. + * + * @return The braking crossover speed, which is negative if the motors never limit it. + */ + float braking_crossover_speed() const; + /** * @brief Get the same limits with a lower top speed. * @@ -74,7 +92,8 @@ struct MotionLimits { * left of the grip there can change the speed: the lateral part takes its share as on a friction * circle, and the angular part, which the two tires make by pushing in opposite directions, takes * it from the push each tire has left. A change of speed on a curve also changes the angular speed, - * and that angular acceleration is counted too. + * and that angular acceleration is counted too. The motors bound the change of speed along the path + * as they do on a straight. * * @note On a straight nothing is taken and the limits are those of the linear motion. */ diff --git a/micras_nav/include/micras/nav/speed_profile.hpp b/micras_nav/include/micras/nav/speed_profile.hpp index 37e8998..90a3c81 100644 --- a/micras_nav/include/micras/nav/speed_profile.hpp +++ b/micras_nav/include/micras/nav/speed_profile.hpp @@ -103,12 +103,24 @@ class SpeedProfile { */ static float get_brakeable_speed(float distance, float end_speed, const MotionLimits& limits); + /** + * @brief Get the distance needed to brake between two speeds. + * + * @param start_speed The speed at the start. + * @param end_speed The speed at the end, not negative. + * @param limits The limits of the motion. + * @return The distance covered while braking as hard as the limits allow, or zero if the end + * speed is not lower than the start. + */ + static float get_braking_distance(float start_speed, float end_speed, const MotionLimits& limits); + private: /** * @brief Stretch of the motion with one law for the acceleration. * * @note The rate is zero for a constant acceleration. Otherwise the acceleration decays as the - * speed approaches the target, as `rate * (target - speed)`, which is the motor limited stretch. + * speed approaches the target, as `rate * (target - speed)`, which is a motor limited stretch: + * accelerating towards the free speed, or braking towards the free speed backwards. */ struct Phase { float duration{}; @@ -120,9 +132,10 @@ class SpeedProfile { }; /** - * @brief Number of phases: traction limited, motor limited, cruise and braking. + * @brief Number of phases: traction and motor limited acceleration, cruise, and traction and + * motor limited braking. */ - static constexpr uint8_t number_of_phases{4}; + static constexpr uint8_t number_of_phases{5}; /** * @brief Get the distance needed to accelerate between two speeds. diff --git a/micras_nav/src/motion_limits.cpp b/micras_nav/src/motion_limits.cpp index 9799273..16ec3d2 100644 --- a/micras_nav/src/motion_limits.cpp +++ b/micras_nav/src/motion_limits.cpp @@ -18,6 +18,14 @@ float MotionLimits::crossover_speed() const { return this->motor_speed * (1.0F - this->acceleration / this->motor_acceleration); } +float MotionLimits::deceleration_at(float speed) const { + return std::min(this->deceleration, this->motor_acceleration * (1.0F + speed / this->motor_speed)); +} + +float MotionLimits::braking_crossover_speed() const { + return this->motor_speed * (this->deceleration / this->motor_acceleration - 1.0F); +} + MotionLimits MotionLimits::capped(float speed) const { MotionLimits limits = *this; limits.max_speed = std::min(limits.max_speed, speed); @@ -60,7 +68,9 @@ float CurveLimits::get_deceleration(float speed, const Bending& bending) const { return 0.0F; } - return spare / (1.0F / this->linear.deceleration + std::abs(bending.curvature) / this->angular); + const float grip = spare / (1.0F / this->linear.deceleration + std::abs(bending.curvature) / this->angular); + + return std::min(grip, this->linear.deceleration_at(speed)); } float CurveLimits::get_spare(float speed, const Bending& bending) const { diff --git a/micras_nav/src/speed_profile.cpp b/micras_nav/src/speed_profile.cpp index 0b2a71b..c3291ff 100644 --- a/micras_nav/src/speed_profile.cpp +++ b/micras_nav/src/speed_profile.cpp @@ -16,7 +16,7 @@ SpeedProfile::SpeedProfile(float distance, float start_speed, float end_speed, c return; } - const float braking_distance = (start_speed * start_speed - end_speed * end_speed) / (2.0F * limits.deceleration); + const float braking_distance = get_braking_distance(start_speed, end_speed, limits); if (braking_distance >= this->total_distance) { const float deceleration = (start_speed * start_speed - end_speed * end_speed) / (2.0F * this->total_distance); @@ -30,8 +30,8 @@ SpeedProfile::SpeedProfile(float distance, float start_speed, float end_speed, c float peak = cap; const auto excess = [&](float speed) { - return get_acceleration_distance(start_speed, speed, limits) + - (speed * speed - end_speed * end_speed) / (2.0F * limits.deceleration) - this->total_distance; + return get_acceleration_distance(start_speed, speed, limits) + get_braking_distance(speed, end_speed, limits) - + this->total_distance; }; if (excess(low) >= 0.0F) { @@ -72,7 +72,7 @@ SpeedProfile::SpeedProfile(float distance, float start_speed, float end_speed, c } const float final_speed = std::min(peak, end_speed); - const float braking = (peak * peak - final_speed * final_speed) / (2.0F * limits.deceleration); + const float braking = get_braking_distance(peak, final_speed, limits); float accelerated = 0.0F; @@ -87,8 +87,20 @@ SpeedProfile::SpeedProfile(float distance, float start_speed, float end_speed, c this->append({.duration = cruise / peak}); } - if (peak > final_speed) { - this->append({.duration = (peak - final_speed) / limits.deceleration, .acceleration = -limits.deceleration}); + const float braking_end = std::clamp(limits.braking_crossover_speed(), final_speed, peak); + + if (peak > braking_end) { + this->append({.duration = (peak - braking_end) / limits.deceleration, .acceleration = -limits.deceleration}); + } + + if (braking_end > final_speed) { + const float rate = limits.motor_acceleration / limits.motor_speed; + + this->append({ + .duration = std::log((limits.motor_speed + braking_end) / (limits.motor_speed + final_speed)) / rate, + .rate = rate, + .target = -limits.motor_speed, + }); } } @@ -191,7 +203,52 @@ float SpeedProfile::get_brakeable_speed(float distance, float end_speed, const M return end_speed; } - return std::min(limits.max_speed, std::sqrt(end_speed * end_speed + 2.0F * limits.deceleration * distance)); + const float unlimited = + std::min(limits.max_speed, std::sqrt(end_speed * end_speed + 2.0F * limits.deceleration * distance)); + + if (limits.braking_crossover_speed() <= end_speed or end_speed >= limits.max_speed) { + return unlimited; + } + + if (get_braking_distance(limits.max_speed, end_speed, limits) <= distance) { + return limits.max_speed; + } + + float low = end_speed; + float high = limits.max_speed; + + for (uint8_t i = 0; i < 32; i++) { + const float middle = (low + high) / 2.0F; + + if (get_braking_distance(middle, end_speed, limits) > distance) { + high = middle; + } else { + low = middle; + } + } + + return low; +} + +float SpeedProfile::get_braking_distance(float start_speed, float end_speed, const MotionLimits& limits) { + if (start_speed <= end_speed) { + return 0.0F; + } + + const float braking_end = std::clamp(limits.braking_crossover_speed(), end_speed, start_speed); + + float distance = (start_speed * start_speed - braking_end * braking_end) / (2.0F * limits.deceleration); + + if (braking_end > end_speed) { + const float rate = limits.motor_acceleration / limits.motor_speed; + + distance += + ((braking_end - end_speed) - + limits.motor_speed * std::log((limits.motor_speed + braking_end) / (limits.motor_speed + end_speed))) / + rate; + } + + return distance; } float SpeedProfile::get_acceleration_distance(float start_speed, float end_speed, const MotionLimits& limits) { From 22446595daa6777e02fff32d4a737539b42c5eb8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 27 Sep 2026 19:46:15 -0300 Subject: [PATCH 525/561] =?UTF-8?q?=F0=9F=90=9B=20Answer=20the=20fourth=20?= =?UTF-8?q?review=20of=20the=20navigation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The fourth review of #54 left three notes on the motor limited braking of 788a44e, all in the regime of motors weaker than the tires, and asked to see the ambient light of the wall sensors before deciding how blind they get: - A braking that does not fit its distance scales the limits of the tires and of the motors by the same factor, instead of one constant deceleration, which asked up to 18 % more than the motors give at the end of it. - The speed that can be braked from has a closed form above the braking crossover, and takes eight Newton steps under it instead of a bisection of 32, so an edge no longer costs 32 logarithms. It stays within a micron of the distance, since the distance grows faster the higher the speed and the steps close in from above. - The motor limited braking distance never comes out negative. - The dark reading of each wall sensor, the ambient light its receiver sees, is in the pool, for the bench to decide the blind reading from. Today's robot brakes harder with its motors than its tires allow, so it runs as before: every run of the contest in the simulator is byte identical but for the new columns and the link credit the larger stream uses. With the winding resistance tripled, the fast runs end as with 788a44e. --- .../include/micras/nav/speed_profile.hpp | 24 +++++- micras_nav/src/speed_profile.cpp | 86 +++++++++++-------- .../micras/proxy/impl/wall_sensors.tpp | 3 +- .../include/micras/proxy/wall_sensors.hpp | 7 +- 4 files changed, 77 insertions(+), 43 deletions(-) diff --git a/micras_nav/include/micras/nav/speed_profile.hpp b/micras_nav/include/micras/nav/speed_profile.hpp index 90a3c81..096b551 100644 --- a/micras_nav/include/micras/nav/speed_profile.hpp +++ b/micras_nav/include/micras/nav/speed_profile.hpp @@ -42,9 +42,10 @@ class SpeedProfile { * @brief Construct a new Speed Profile object. * * @note Speeds that the distance does not allow are not an error. A start speed too high to - * brake from is braked harder than the limit, and an end speed that cannot be reached leaves - * the motion accelerating all the way, so the profile always covers exactly the distance. It is - * the velocity planner that keeps the speeds it asks for feasible. + * brake from is braked harder than the limit, the tires and the motors alike by the same + * factor, and an end speed that cannot be reached leaves the motion accelerating all the way, + * so the profile always covers exactly the distance. It is the velocity planner that keeps the + * speeds it asks for feasible. * * @param distance The distance to cover, which must not be negative. * @param start_speed The speed at the start. @@ -137,6 +138,14 @@ class SpeedProfile { */ static constexpr uint8_t number_of_phases{5}; + /** + * @brief Newton steps that find the speed that can be braked from where the motors limit it. + * + * @note The braking distance grows faster the higher the speed there, so the steps, started + * from the crossover above the answer, close in on it from above without overshooting. + */ + static constexpr uint8_t newton_steps{8}; + /** * @brief Get the distance needed to accelerate between two speeds. * @@ -147,6 +156,15 @@ class SpeedProfile { */ static float get_acceleration_distance(float start_speed, float end_speed, const MotionLimits& limits); + /** + * @brief Append the phases that brake between two speeds as hard as the limits allow. + * + * @param start_speed The speed at the start of the braking. + * @param end_speed The speed at its end, not higher than the start. + * @param limits The limits of the motion. + */ + void append_braking(float start_speed, float end_speed, const MotionLimits& limits); + /** * @brief Append a phase, advancing the state of the motion to its end. * diff --git a/micras_nav/src/speed_profile.cpp b/micras_nav/src/speed_profile.cpp index c3291ff..fa9f2e0 100644 --- a/micras_nav/src/speed_profile.cpp +++ b/micras_nav/src/speed_profile.cpp @@ -19,9 +19,19 @@ SpeedProfile::SpeedProfile(float distance, float start_speed, float end_speed, c const float braking_distance = get_braking_distance(start_speed, end_speed, limits); if (braking_distance >= this->total_distance) { - const float deceleration = (start_speed * start_speed - end_speed * end_speed) / (2.0F * this->total_distance); + if (limits.braking_crossover_speed() <= end_speed) { + const float deceleration = + (start_speed * start_speed - end_speed * end_speed) / (2.0F * this->total_distance); - this->append({.duration = (start_speed - end_speed) / deceleration, .acceleration = -deceleration}); + this->append({.duration = (start_speed - end_speed) / deceleration, .acceleration = -deceleration}); + return; + } + + MotionLimits harder = limits; + harder.deceleration *= braking_distance / this->total_distance; + harder.motor_acceleration *= braking_distance / this->total_distance; + + this->append_braking(start_speed, end_speed, harder); return; } @@ -87,21 +97,7 @@ SpeedProfile::SpeedProfile(float distance, float start_speed, float end_speed, c this->append({.duration = cruise / peak}); } - const float braking_end = std::clamp(limits.braking_crossover_speed(), final_speed, peak); - - if (peak > braking_end) { - this->append({.duration = (peak - braking_end) / limits.deceleration, .acceleration = -limits.deceleration}); - } - - if (braking_end > final_speed) { - const float rate = limits.motor_acceleration / limits.motor_speed; - - this->append({ - .duration = std::log((limits.motor_speed + braking_end) / (limits.motor_speed + final_speed)) / rate, - .rate = rate, - .target = -limits.motor_speed, - }); - } + this->append_braking(peak, final_speed, limits); } float SpeedProfile::duration() const { @@ -203,31 +199,28 @@ float SpeedProfile::get_brakeable_speed(float distance, float end_speed, const M return end_speed; } - const float unlimited = - std::min(limits.max_speed, std::sqrt(end_speed * end_speed + 2.0F * limits.deceleration * distance)); + const float crossover = limits.braking_crossover_speed(); - if (limits.braking_crossover_speed() <= end_speed or end_speed >= limits.max_speed) { - return unlimited; + if (crossover <= end_speed or end_speed >= limits.max_speed) { + return std::min(limits.max_speed, std::sqrt(end_speed * end_speed + 2.0F * limits.deceleration * distance)); } - if (get_braking_distance(limits.max_speed, end_speed, limits) <= distance) { - return limits.max_speed; - } + const float motor_distance = get_braking_distance(crossover, end_speed, limits); - float low = end_speed; - float high = limits.max_speed; + if (distance >= motor_distance) { + return std::min( + limits.max_speed, + std::sqrt(crossover * crossover + 2.0F * limits.deceleration * (distance - motor_distance)) + ); + } - for (uint8_t i = 0; i < 32; i++) { - const float middle = (low + high) / 2.0F; + float speed = crossover; - if (get_braking_distance(middle, end_speed, limits) > distance) { - high = middle; - } else { - low = middle; - } + for (uint8_t i = 0; i < newton_steps; i++) { + speed -= (get_braking_distance(speed, end_speed, limits) - distance) * limits.deceleration_at(speed) / speed; } - return low; + return std::clamp(speed, end_speed, limits.max_speed); } float SpeedProfile::get_braking_distance(float start_speed, float end_speed, const MotionLimits& limits) { @@ -241,16 +234,35 @@ float SpeedProfile::get_braking_distance(float start_speed, float end_speed, con if (braking_end > end_speed) { const float rate = limits.motor_acceleration / limits.motor_speed; + const float span = braking_end - end_speed; distance += - ((braking_end - end_speed) - - limits.motor_speed * std::log((limits.motor_speed + braking_end) / (limits.motor_speed + end_speed))) / - rate; + std::max(span - limits.motor_speed * std::log1p(span / (limits.motor_speed + end_speed)), 0.0F) / rate; } return distance; } +void SpeedProfile::append_braking(float start_speed, float end_speed, const MotionLimits& limits) { + const float braking_end = std::clamp(limits.braking_crossover_speed(), end_speed, start_speed); + + if (start_speed > braking_end) { + this->append( + {.duration = (start_speed - braking_end) / limits.deceleration, .acceleration = -limits.deceleration} + ); + } + + if (braking_end > end_speed) { + const float rate = limits.motor_acceleration / limits.motor_speed; + + this->append({ + .duration = std::log1p((braking_end - end_speed) / (limits.motor_speed + end_speed)) / rate, + .rate = rate, + .target = -limits.motor_speed, + }); + } +} + float SpeedProfile::get_acceleration_distance(float start_speed, float end_speed, const MotionLimits& limits) { if (end_speed <= start_speed) { return 0.0F; diff --git a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp index 98a05c7..03101df 100644 --- a/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp +++ b/micras_proxy/include/micras/proxy/impl/wall_sensors.tpp @@ -138,7 +138,8 @@ void TWallSensors::update() { this->max_distance; reading.valid = distance < this->max_distance; - reading.blind = this->get_dark_reading(i) >= this->blind_reading; + reading.dark = this->get_dark_reading(i); + reading.blind = reading.dark >= this->blind_reading; reading.distance = this->fast_filters.at(i).update(std::min(distance, this->max_distance)); reading.slow_distance = this->slow_filters.at(i).update(std::min(distance, this->max_distance)); diff --git a/micras_proxy/include/micras/proxy/wall_sensors.hpp b/micras_proxy/include/micras/proxy/wall_sensors.hpp index e352c6e..87b4352 100644 --- a/micras_proxy/include/micras/proxy/wall_sensors.hpp +++ b/micras_proxy/include/micras/proxy/wall_sensors.hpp @@ -74,12 +74,15 @@ class TWallSensors { * * @note The fast distance is for everything that is a position and the slow one for deciding * whether there is a wall. The reading is valid when the sensor sees anything above its noise, - * and it is new for a single update after the sensor produces a value. It is blind when ambient - * light saturates the receiver, and then says nothing about a wall. + * and it is new for a single update after the sensor produces a value. The dark reading is the + * one taken with the emitter off, as a fraction of the full scale, which is the ambient light + * the receiver sees. The reading is blind when that light saturates the receiver, and then says + * nothing about a wall. */ struct Reading { float distance; float slow_distance; + float dark; bool valid; bool blind; bool is_new; From a8fdbe7158126502ebb79c3de74b0172c3a06104 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 03:30:37 -0300 Subject: [PATCH 526/561] =?UTF-8?q?=E2=9C=A8=20Give=20the=20host=20SPI=20a?= =?UTF-8?q?=20device=20slot=20keyed=20by=20bus=20and=20chip=20select?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A chip behind an SPI chip select is a SpiDevice: select, exchange and deselect, and the SPI mode it answers in. Board::spi_device attaches one by the bus handle and the chip select, since hspi3 carries the IMU and both encoders. The host Spi routes every transfer between select_device and unselect_device to the selected device, so a register access split over several HAL calls is one transaction. start_transfer completes when the host clock passes the time its bytes take at the bus's bit rate, which the fake Cube layer now gives SPI3 (125 MHz kernel clock, prescaler 32), and ends through on_transfer_end as the DMA interrupt does. A device addressed in another mode, or a chip select with no device, answers all ones. --- .../host/include/micras/hal/host/board.hpp | 29 +++ .../host/include/micras/hal/host/ports.hpp | 20 ++ .../include/micras/hal/host/spi_device.hpp | 92 ++++++++ micras_hal/host/include/stm32_host.h | 29 +++ micras_hal/host/src/board.cpp | 39 +++- micras_hal/host/src/spi.cpp | 205 +++++++++++++++++- 6 files changed, 397 insertions(+), 17 deletions(-) create mode 100644 micras_hal/host/include/micras/hal/host/spi_device.hpp diff --git a/micras_hal/host/include/micras/hal/host/board.hpp b/micras_hal/host/include/micras/hal/host/board.hpp index 86c1e9d..193413e 100644 --- a/micras_hal/host/include/micras/hal/host/board.hpp +++ b/micras_hal/host/include/micras/hal/host/board.hpp @@ -13,6 +13,10 @@ #include #include "micras/hal/host/ports.hpp" +#include "micras/hal/host/spi_device.hpp" + +struct GPIO_TypeDef; +struct SPI_HandleTypeDef; namespace micras::hal::host { /** @@ -91,6 +95,31 @@ class Board { */ static SamplePort& samples(std::string_view name); + /** + * @brief Get the port of one chip select of an SPI bus. + * + * @param spi SPI handle. + * @param cs_port GPIO port of the chip select. + * @param cs_pin Pin mask of the chip select. + * @return The port. + */ + static SpiPort& spi(const void* spi, const void* cs_port, uint16_t cs_pin); + + /** + * @brief Attach a device to one chip select of an SPI bus. + * + * @note Keyed by the bus and the chip select, since several devices share + * a bus. Binds the SPI port and the chip select's GPIO port. The device + * is held by reference and must outlive the run. + * + * @param spi SPI handle. + * @param cs_port GPIO port of the chip select. + * @param cs_pin Pin mask of the chip select. + * @param device The device. + */ + static void + spi_device(const SPI_HandleTypeDef* spi, const GPIO_TypeDef* cs_port, uint16_t cs_pin, SpiDevice& device); + /** * @brief Get the flash. * diff --git a/micras_hal/host/include/micras/hal/host/ports.hpp b/micras_hal/host/include/micras/hal/host/ports.hpp index c7ae30b..11ef9c1 100644 --- a/micras_hal/host/include/micras/hal/host/ports.hpp +++ b/micras_hal/host/include/micras/hal/host/ports.hpp @@ -17,6 +17,8 @@ #include #include +#include "micras/hal/host/spi_device.hpp" + namespace micras::hal::host { /** * @brief What every port has: a name and who has used it. @@ -247,6 +249,24 @@ struct SamplePort : Port { */ uint32_t sequence{0}; }; + +/** + * @brief One chip select of an SPI bus, and the device behind it. + * + * @note The firmware touches the port when it selects the device; a binding + * binds it by attaching a device with Board::spi_device(). + */ +struct SpiPort : Port { + /** + * @brief Device attached to the chip select, none before a binding attaches one. + */ + SpiDevice* device{nullptr}; + + /** + * @brief Whether the chip select is low, between the firmware's select and unselect. + */ + bool selected{false}; +}; } // namespace micras::hal::host #endif // MICRAS_HAL_HOST_PORTS_HPP diff --git a/micras_hal/host/include/micras/hal/host/spi_device.hpp b/micras_hal/host/include/micras/hal/host/spi_device.hpp new file mode 100644 index 0000000..20b86ea --- /dev/null +++ b/micras_hal/host/include/micras/hal/host/spi_device.hpp @@ -0,0 +1,92 @@ +/** + * @file + * + * @brief A chip on an SPI bus, as the host backend sees it: a chip select and a stream of bytes. + */ + +#ifndef MICRAS_HAL_HOST_SPI_DEVICE_HPP +#define MICRAS_HAL_HOST_SPI_DEVICE_HPP + +#include +#include + +namespace micras::hal::host { +/** + * @brief A device behind one chip select of an SPI bus. + * + * @note The host Spi calls select() when the firmware pulls the device's chip + * select low, exchange() for every transfer while it is low, however many + * HAL calls the firmware splits the transaction into, and deselect() when + * the chip select goes high again. A transaction is therefore everything + * between select() and deselect(), which is what a chip sees on the bus. + * + * @note A device only answers in the SPI mode it expects. When the mode the bus + * runs in differs, the host Spi does not call exchange() and the firmware + * reads all ones, as a chip clocked on the wrong edge answers on the robot. + */ +class SpiDevice { +public: + /** + * @brief SPI mode: the clock polarity in the high bit, the clock phase in the low one. + */ + enum class Mode : uint8_t { + MODE_0 = 0, + MODE_1 = 1, + MODE_2 = 2, + MODE_3 = 3, + }; + + /** + * @brief Special member functions: a device is attached by reference, so it cannot be copied or moved. + */ + ///@{ + virtual ~SpiDevice() = default; + SpiDevice(const SpiDevice&) = delete; + SpiDevice(SpiDevice&&) = delete; + SpiDevice& operator=(const SpiDevice&) = delete; + SpiDevice& operator=(SpiDevice&&) = delete; + + ///@} + + /** + * @brief Get the SPI mode the device answers in. + * + * @return The mode. + */ + Mode mode() const { return this->spi_mode; } + + /** + * @brief Start a transaction: the chip select went low. + */ + virtual void select() = 0; + + /** + * @brief Clock bytes through the device while it is selected. + * + * @param transmitted Bytes the controller sends, one per received byte. + * @param received Bytes the device answers with, as many as were transmitted. + */ + virtual void exchange(std::span transmitted, std::span received) = 0; + + /** + * @brief End the transaction: the chip select went high. + */ + virtual void deselect() = 0; + +protected: + /** + * @brief Construct a device that answers in one SPI mode. + * + * @param mode The mode. + */ + explicit SpiDevice(Mode mode) : spi_mode{mode} { } + +private: + /** + * @brief SPI mode the device answers in. + */ + Mode spi_mode; +}; +} // namespace micras::hal::host + +#endif // MICRAS_HAL_HOST_SPI_DEVICE_HPP diff --git a/micras_hal/host/include/stm32_host.h b/micras_hal/host/include/stm32_host.h index 244bb50..96a1a95 100644 --- a/micras_hal/host/include/stm32_host.h +++ b/micras_hal/host/include/stm32_host.h @@ -150,12 +150,40 @@ inline constexpr uint32_t SPI_PHASE_2EDGE{0x1U << 24U}; ///@} +/** + * @brief Dividers of the SPI kernel clock, from 2 to 256, in the bits the vendor puts them. + */ +///@{ +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_2{0x00000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_4{0x10000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_8{0x20000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_16{0x30000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_32{0x40000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_64{0x50000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_128{0x60000000U}; +inline constexpr uint32_t SPI_BAUDRATEPRESCALER_256{0x70000000U}; + +///@} + +/** + * @brief The registers of an SPI bus the host backend models. + * + * @note kernel_clock is not a register: it is the frequency the bus's kernel + * clock runs at, which on the chip comes from the clock tree the init + * functions configure. The fake init function writes it, and the bit + * rate is that divided by the prescaler. + */ +struct SPI_TypeDef { + uint32_t kernel_clock; +}; + /** * @brief Configuration of an SPI bus, as the init function fills it. */ struct SPI_InitTypeDef { uint32_t CLKPolarity; uint32_t CLKPhase; + uint32_t BaudRatePrescaler; }; /** @@ -170,6 +198,7 @@ enum HAL_SPI_StateTypeDef : uint8_t { * @brief An SPI handle. */ struct SPI_HandleTypeDef { + SPI_TypeDef* Instance; SPI_InitTypeDef Init; HAL_SPI_StateTypeDef State; }; diff --git a/micras_hal/host/src/board.cpp b/micras_hal/host/src/board.cpp index e5709f3..5d53e31 100644 --- a/micras_hal/host/src/board.cpp +++ b/micras_hal/host/src/board.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include "micras/hal/host/board.hpp" @@ -17,6 +18,11 @@ namespace { */ using Key = std::pair; +/** + * @brief Key of an SPI port: the handle, and the GPIO port and pin of the chip select. + */ +using SpiKey = std::tuple; + /** * @brief Every port and name, created on first use. */ @@ -28,6 +34,7 @@ struct Registry { std::map uarts; std::map encoders; std::map> samples; + std::map spis; std::map gpio_names; std::map handle_names; FlashPort flash{{.name = "flash"}}; @@ -56,6 +63,18 @@ std::string handle_name(const void* handle) { std::format("handle@{:#x}", std::bit_cast(handle)); } +/** + * @brief Name a GPIO pin, falling back to its port and mask. + * + * @param key GPIO port and pin mask. + * @return Its name. + */ +std::string gpio_name(const Key& key) { + const auto found = registry().gpio_names.find(key); + return found != registry().gpio_names.end() ? found->second : + std::format("{} pin {:#06x}", handle_name(key.first), key.second); +} + /** * @brief Find a port, creating it with a name on first use. * @@ -80,11 +99,7 @@ typename Map::mapped_type& find_or_create(Map& ports, const typename Map::key_ty GpioPort& Board::gpio(const void* port, uint16_t pin) { const Key key{port, pin}; - return find_or_create(registry().gpios, key, [&key] { - const auto found = registry().gpio_names.find(key); - return found != registry().gpio_names.end() ? found->second : - std::format("{} pin {:#06x}", handle_name(key.first), key.second); - }); + return find_or_create(registry().gpios, key, [&key] { return gpio_name(key); }); } PwmPort& Board::pwm(const void* timer, uint32_t channel) { @@ -122,6 +137,19 @@ SamplePort& Board::samples(std::string_view name) { return found->second; } +SpiPort& Board::spi(const void* spi, const void* cs_port, uint16_t cs_pin) { + return find_or_create(registry().spis, SpiKey{spi, cs_port, cs_pin}, [spi, cs_port, cs_pin] { + return std::format("{} {}", handle_name(spi), gpio_name(Key{cs_port, cs_pin})); + }); +} + +void Board::spi_device(const SPI_HandleTypeDef* spi, const GPIO_TypeDef* cs_port, uint16_t cs_pin, SpiDevice& device) { + SpiPort& port = Board::spi(spi, cs_port, cs_pin); + port.device = &device; + port.bound = true; + Board::gpio(cs_port, cs_pin).bound = true; +} + FlashPort& Board::flash() { return registry().flash; } @@ -163,6 +191,7 @@ std::vector Board::unbound() { collect(registry().uarts); collect(registry().encoders); collect(registry().samples); + collect(registry().spis); return names; } diff --git a/micras_hal/host/src/spi.cpp b/micras_hal/host/src/spi.cpp index fc7f1f0..009b308 100644 --- a/micras_hal/host/src/spi.cpp +++ b/micras_hal/host/src/spi.cpp @@ -5,11 +5,107 @@ #include #include #include +#include #include +#include +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/host/spi_device.hpp" #include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" namespace micras::hal { +namespace { +/** + * @brief What the host keeps for each Spi that the firmware's class has no member for. + */ +struct HostState { + /** + * @brief Chip select of the device, which keys its port together with the handle. + */ + Gpio::Config cs{}; + + /** + * @brief Host clock cycle the running transfer ends at. + */ + uint64_t transfer_end{0}; +}; + +/** + * @brief Get the host state of every Spi alive. + * + * @return The states, keyed by the object. + */ +std::map& host_states() { + static std::map states; + return states; +} + +/** + * @brief Get the port of the chip select an Spi drives. + * + * @param spi The Spi. + * @param handle Its handle. + * @return The port. + */ +host::SpiPort& port_of(const Spi* spi, const SPI_HandleTypeDef* handle) { + const Gpio::Config& cs = host_states().at(spi).cs; + return host::Board::spi(handle, cs.port, cs.pin); +} + +/** + * @brief Get the SPI mode the bus runs in, from the polarity and phase its handle holds. + * + * @param handle Handle of the bus. + * @return The mode. + */ +host::SpiDevice::Mode mode_of(const SPI_HandleTypeDef& handle) { + const bool polarity = handle.Init.CLKPolarity == SPI_POLARITY_HIGH; + const bool phase = handle.Init.CLKPhase == SPI_PHASE_2EDGE; + return static_cast((polarity ? 2 : 0) + (phase ? 1 : 0)); +} + +/** + * @brief Clock bytes through the device a port selects. + * + * @param handle Handle of the bus. + * @param port Port of the chip select. + * @param transmitted Bytes sent. + * @param received Bytes answered, as many as were sent; all ones when no device drives the bus in its mode. + */ +void exchange( + const SPI_HandleTypeDef& handle, host::SpiPort& port, std::span transmitted, + std::span received +) { + if (not port.selected or port.device == nullptr or port.device->mode() != mode_of(handle)) { + std::ranges::fill(received, 0xFF); + return; + } + + port.device->exchange(transmitted, received); +} + +/** + * @brief Get how long a transfer takes on a bus. + * + * @param handle Handle of the bus. + * @param bytes Bytes transferred. + * @return Host clock cycles, zero when the bus has no clock to time it by. + */ +uint64_t transfer_cycles(const SPI_HandleTypeDef& handle, std::size_t bytes) { + if (handle.Instance == nullptr or handle.Instance->kernel_clock == 0) { + return 0; + } + + const uint64_t kernel_clock = handle.Instance->kernel_clock; + const uint64_t divider = 2ULL << (handle.Init.BaudRatePrescaler >> 28U); + const uint64_t cycles_per_second = host::Clock::instance().cycles_per_microsecond() * 1000000ULL; + + return ((bytes * 8 * divider * cycles_per_second) + kernel_clock - 1) / kernel_clock; +} +} // namespace + std::array Spi::transferring{}; Spi::Spi(const Config& config) : @@ -18,6 +114,7 @@ Spi::Spi(const Config& config) : timeout{config.timeout}, clock_polarity{config.clock_polarity}, clock_phase{config.clock_phase} { + host_states()[this] = HostState{.cs = config.cs_gpio}; this->unselect_device(); if (this->handle->State == HAL_SPI_STATE_RESET) { @@ -28,45 +125,129 @@ Spi::Spi(const Config& config) : } Spi::~Spi() { + if (this->transfer == Transfer::RUNNING) { + this->unselect_device(); + } + std::ranges::replace(transferring, this, static_cast(nullptr)); + host_states().erase(this); } bool Spi::select_device() { + const auto bus_busy = [this] { + return std::ranges::any_of(transferring, [this](const Spi* device) { + return device != nullptr and device->handle == this->handle and device->get_transfer() == Transfer::RUNNING; + }); + }; + + if (bus_busy()) { + const uint32_t start = Timer::get_counter(); + const uint32_t limit = Timer::to_cycles(1000 * this->timeout); + + while (bus_busy()) { + if (Timer::get_counter() - start > limit) { + return false; + } + } + } + this->handle->Init.CLKPolarity = this->clock_polarity; this->handle->Init.CLKPhase = this->clock_phase; this->cs_gpio.write(false); + + host::SpiPort& port = port_of(this, this->handle); + port.touched = true; + + if (not port.selected) { + port.selected = true; + + if (port.device != nullptr) { + port.device->select(); + } + } + return true; } void Spi::unselect_device() { this->cs_gpio.write(true); + + host::SpiPort& port = port_of(this, this->handle); + + if (port.selected) { + port.selected = false; + + if (port.device != nullptr) { + port.device->deselect(); + } + } } -// NOLINTBEGIN(readability-convert-member-functions-to-static): hal::Spi's interface, declared by the firmware. -bool Spi::transmit(std::span /*data*/) { - return false; +bool Spi::transmit(std::span data) { + if (this->handle->State != HAL_SPI_STATE_READY) { + return false; + } + + std::vector ignored(data.size()); + exchange(*this->handle, port_of(this, this->handle), data, ignored); + return true; } -bool Spi::receive(std::span /*data*/) { - return false; +bool Spi::receive(std::span data) { + if (this->handle->State != HAL_SPI_STATE_READY) { + return false; + } + + const std::vector zeros(data.size()); + exchange(*this->handle, port_of(this, this->handle), zeros, data); + return true; } -bool Spi::transmit_receive(std::span /*transmitted*/, std::span /*received*/) { - return false; +bool Spi::transmit_receive(std::span transmitted, std::span received) { + if (received.size() < transmitted.size() or this->handle->State != HAL_SPI_STATE_READY) { + return false; + } + + exchange(*this->handle, port_of(this, this->handle), transmitted, received.first(transmitted.size())); + return true; } -// NOLINTEND(readability-convert-member-functions-to-static) +bool Spi::start_transfer(std::span transmitted, std::span received) { + auto* const slot = std::ranges::find(transferring, nullptr); + + if (received.size() < transmitted.size() or slot == transferring.end() or + this->handle->State != HAL_SPI_STATE_READY or not this->select_device()) { + this->transfer = Transfer::FAILED; + return false; + } + + *slot = this; + this->transfer = Transfer::RUNNING; + exchange(*this->handle, port_of(this, this->handle), transmitted, received.first(transmitted.size())); + host_states().at(this).transfer_end = + host::Clock::instance().now() + transfer_cycles(*this->handle, transmitted.size()); -bool Spi::start_transfer(std::span /*transmitted*/, std::span /*received*/) { - this->transfer = Transfer::FAILED; - return false; + return true; } Spi::Transfer Spi::get_transfer() const { + if (this->transfer == Transfer::RUNNING and host::Clock::instance().now() >= host_states().at(this).transfer_end) { + on_transfer_end(this->handle, true); + } + return this->transfer; } -void Spi::on_transfer_end(const SPI_HandleTypeDef* /*handle*/, bool /*succeeded*/) { } +void Spi::on_transfer_end(const SPI_HandleTypeDef* handle, bool succeeded) { + for (Spi*& device : transferring) { + if (device != nullptr and device->handle == handle) { + device->unselect_device(); + device->transfer = succeeded ? Transfer::COMPLETE : Transfer::FAILED; + device = nullptr; + return; + } + } +} bool Spi::was_initialized() const { return this->initialized; From 7aed279bd495594a2dca645310480ad7bf774e9b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 03:30:56 -0300 Subject: [PATCH 527/561] =?UTF-8?q?=E2=9C=A8=20Model=20the=20LSM6DSV=20and?= =?UTF-8?q?=20AS5047U=20as=20SPI=20devices?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit micras_hal_host_models (micras::hal_host_models, namespace micras::models) holds the two SPI chips of Micras as devices of the host SPI slot, knowing nothing but the slot. Lsm6dsvModel is the 128 registers of the main page with the datasheet defaults, software power-on reset, auto-increment, data ready bits set by a new sample and cleared by the read of its output, samples encoded with the full scale in CTRL6 and CTRL8, and the output data rates of CTRL1, CTRL2 and HAODR_CFG, in SPI mode 3. As5047uModel is the 24-bit frames with their CRC-8 checked on every frame, answers pipelined by one frame, ERRFL with the CRC and framing error bits, and the volatile registers the firmware writes, in SPI mode 1. Their unit tests address the LSM6DSV's registers by ST's driver header, so a register the model puts elsewhere than the driver fails. --- micras_proxy/models/CMakeLists.txt | 34 ++ .../include/micras/models/as5047u_model.hpp | 182 ++++++++++ .../include/micras/models/lsm6dsv_model.hpp | 200 +++++++++++ micras_proxy/models/src/as5047u_model.cpp | 187 ++++++++++ micras_proxy/models/src/lsm6dsv_model.cpp | 275 +++++++++++++++ .../models/tests/as5047u_model_test.cpp | 219 ++++++++++++ .../models/tests/lsm6dsv_model_test.cpp | 327 ++++++++++++++++++ 7 files changed, 1424 insertions(+) create mode 100644 micras_proxy/models/CMakeLists.txt create mode 100644 micras_proxy/models/include/micras/models/as5047u_model.hpp create mode 100644 micras_proxy/models/include/micras/models/lsm6dsv_model.hpp create mode 100644 micras_proxy/models/src/as5047u_model.cpp create mode 100644 micras_proxy/models/src/lsm6dsv_model.cpp create mode 100644 micras_proxy/models/tests/as5047u_model_test.cpp create mode 100644 micras_proxy/models/tests/lsm6dsv_model_test.cpp diff --git a/micras_proxy/models/CMakeLists.txt b/micras_proxy/models/CMakeLists.txt new file mode 100644 index 0000000..d37754e --- /dev/null +++ b/micras_proxy/models/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################### +## Chip models: SPI devices for the host micras_hal +############################################################################### + +# They know the SPI device slot of the host HAL and nothing else: no Cube layer, no +# firmware, no engine. A robot's bindings attach them to its buses and feed them. +add_library(micras_hal_host_models STATIC + src/as5047u_model.cpp + src/lsm6dsv_model.cpp +) +add_library(micras::hal_host_models ALIAS micras_hal_host_models) +target_include_directories(micras_hal_host_models PUBLIC + "${CMAKE_CURRENT_SOURCE_DIR}/include" + "${MICRAS_SIM_HAL_HOST_INCLUDE}" +) +target_compile_features(micras_hal_host_models PUBLIC cxx_std_23) +target_compile_options(micras_hal_host_models PRIVATE -Wall -Wextra) + +if(MICRAS_TESTS) + include(GoogleTest) + + # ST's register driver header names the registers the tests address, so a model + # register at another address than the driver's fails here. + add_executable(micras_hal_host_models_tests + tests/as5047u_model_test.cpp + tests/lsm6dsv_model_test.cpp + ) + target_link_libraries(micras_hal_host_models_tests PRIVATE + micras_hal_host_models lsm6dsv_headers GTest::gtest_main + ) + target_compile_options(micras_hal_host_models_tests PRIVATE -Wall -Wextra) + + gtest_discover_tests(micras_hal_host_models_tests) +endif() diff --git a/micras_proxy/models/include/micras/models/as5047u_model.hpp b/micras_proxy/models/include/micras/models/as5047u_model.hpp new file mode 100644 index 0000000..c24ecdb --- /dev/null +++ b/micras_proxy/models/include/micras/models/as5047u_model.hpp @@ -0,0 +1,182 @@ +/** + * @file + * + * @brief The AS5047U magnetic position sensor, as its SPI interface shows it. + */ + +#ifndef MICRAS_MODELS_AS5047U_MODEL_HPP +#define MICRAS_MODELS_AS5047U_MODEL_HPP + +#include +#include +#include +#include +#include + +#include "micras/hal/host/spi_device.hpp" + +namespace micras::models { +/** + * @brief The registers of an AS5047U behind an SPI chip select, in its 24-bit frame mode. + * + * @details Every frame is three bytes. A command frame carries a don't care + * bit, the read flag, a 14-bit address and a CRC; a data frame carries + * the warning and error flags, 14 bits of data and a CRC. The CRC is a + * CRC-8 of the first two bytes, polynomial 0x1D, initial value 0xC4, + * final XOR 0xFF. Answers are pipelined by one frame: what the sensor + * sends during a frame answers the frame before it, whatever the chip + * select did in between. + * + * @details A read command is answered with the register's content. A write + * command is answered with the register's old content and is followed + * by a data frame holding the new one, which is answered with the new + * content. + * + * @note Every frame's CRC is checked. A frame with a wrong CRC is ignored, and + * a write whose data frame it was is dropped; it sets the CRC error bit of + * ERRFL. A chip select raised in the middle of a frame drops it and sets + * the framing error bit. The error flag of every answer is set while + * ERRFL is not zero, and reading ERRFL clears it. + * + * @note DISABLE, ZPOSM, ZPOSL, SETTINGS1 to SETTINGS3 and ECC are held, zero + * after power-on, as volatile registers. Writes to any other address are + * ignored and every other register reads zero: the angle, the magnitude + * and the diagnostics are not modeled. So is the position: on the robot it + * reaches the firmware through the quadrature output and a timer in + * encoder mode, and so it does in a simulation. + */ +class As5047uModel : public hal::host::SpiDevice { +public: + /** + * @brief Bytes in a frame. + */ + static constexpr std::size_t frame_size{3}; + + /** + * @brief Addresses of the registers the model holds. + */ + ///@{ + static constexpr uint16_t nop_address{0x0000}; + static constexpr uint16_t errfl_address{0x0001}; + static constexpr uint16_t disable_address{0x0015}; + static constexpr uint16_t zposm_address{0x0016}; + static constexpr uint16_t zposl_address{0x0017}; + static constexpr uint16_t settings1_address{0x0018}; + static constexpr uint16_t settings2_address{0x0019}; + static constexpr uint16_t settings3_address{0x001A}; + static constexpr uint16_t ecc_address{0x001B}; + ///@} + + /** + * @brief Bits of ERRFL the model sets. + */ + ///@{ + static constexpr uint16_t framing_error{1U << 4U}; + static constexpr uint16_t crc_error{1U << 6U}; + ///@} + + /** + * @brief Power the sensor on, in SPI mode 1. + */ + As5047uModel(); + + /** + * @brief Start a transaction at the start of a frame. + */ + void select() override; + + /** + * @brief Shift the pending answer out and the frame in, acting on every frame completed. + * + * @param transmitted Bytes the controller sends. + * @param received Bytes of the answer to the previous frame. + */ + void exchange(std::span transmitted, std::span received) override; + + /** + * @brief End the transaction, dropping an incomplete frame. + */ + void deselect() override; + + /** + * @brief Get a register without the side effects of reading it over SPI. + * + * @param address Register address. + * @return The register's content, zero for one not modeled. + */ + uint16_t peek(uint16_t address) const; + + /** + * @brief Compute the CRC of a frame. + * + * @param high First byte of the frame. + * @param low Second byte of the frame. + * @return The third byte of a valid frame. + */ + static uint8_t crc(uint8_t high, uint8_t low); + +private: + /** + * @brief Act on one complete frame and prepare the answer to it. + * + * @param frame The frame, most significant byte first. + */ + void process(const std::array& frame); + + /** + * @brief Build a data frame answering with a value, the error flag set while ERRFL is not zero. + * + * @param data The value. + * @return The frame, in its low 24 bits. + */ + uint32_t answer_with(uint16_t data) const; + + /** + * @brief Get the register a write to an address changes. + * + * @param address Register address. + * @return The register, or null when the address is not writable. + */ + uint8_t* writable(uint16_t address); + + /** + * @brief Volatile registers, each 8 bits wide. + */ + ///@{ + uint8_t disable{0}; + uint8_t zposm{0}; + uint8_t zposl{0}; + uint8_t settings1{0}; + uint8_t settings2{0}; + uint8_t settings3{0}; + uint8_t ecc{0}; + ///@} + + /** + * @brief Error flags, in ERRFL's layout. + */ + uint16_t errfl{0}; + + /** + * @brief Frame shifted out next: the answer to the last complete frame. + */ + uint32_t answer; + + /** + * @brief Bytes of the frame being received. + */ + std::array frame{}; + + /** + * @brief Number of bytes of the frame received so far. + */ + std::size_t received_bytes{0}; + + /** + * @brief Address whose write command was received, so the next frame holds its data. + */ + std::optional pending_write; +}; +} // namespace micras::models + +#endif // MICRAS_MODELS_AS5047U_MODEL_HPP diff --git a/micras_proxy/models/include/micras/models/lsm6dsv_model.hpp b/micras_proxy/models/include/micras/models/lsm6dsv_model.hpp new file mode 100644 index 0000000..6f53b4a --- /dev/null +++ b/micras_proxy/models/include/micras/models/lsm6dsv_model.hpp @@ -0,0 +1,200 @@ +/** + * @file + * + * @brief The LSM6DSV inertial measurement unit, as its SPI interface shows it. + */ + +#ifndef MICRAS_MODELS_LSM6DSV_MODEL_HPP +#define MICRAS_MODELS_LSM6DSV_MODEL_HPP + +#include +#include +#include +#include + +#include "micras/hal/host/spi_device.hpp" + +namespace micras::models { +/** + * @brief The registers of an LSM6DSV behind an SPI chip select, fed with samples of the motion it measures. + * + * @details The first byte of a transaction is the command: the read flag in the + * top bit and the register address below it. Every following byte + * reads or writes one register, and the address advances after each + * one while CTRL3 IF_INC is set, as it is after reset. + * + * @note The register file is the 128 registers of the main page, with the + * datasheet's defaults: WHO_AM_I 0x70, PIN_CTRL 0x23, CTRL3 0x44 + * (IF_INC and BDU) and zero everywhere else. Writes to read-only and + * reserved registers are ignored. While FUNC_CFG_ACCESS selects the + * embedded functions or the sensor hub page, which are not modeled, every + * other register reads zero and ignores writes. + * + * @note Writing SW_POR in FUNC_CFG_ACCESS or SW_RESET in CTRL3 restores the + * defaults, and the rest of that transaction is ignored. + * + * @note A sample updates the outputs of each sensor whose output data rate is + * not off (ODR_G in CTRL2, ODR_XL in CTRL1), encoded with the full scale + * its register holds (FS_G in CTRL6, FS_XL in CTRL8), and sets that + * sensor's data ready bit in STATUS_REG (GDA, XLDA). Reading the high + * byte of one of the sensor's axes clears the bit, so the burst that + * reads a sample clears it. A transaction happens between two samples, + * which is what block data update guarantees on the chip. + * + * @note The rates in CTRL1, CTRL2 and HAODR_CFG only say whether a sensor is on + * and are decoded for the one feeding the samples: the samples arrive at + * whatever rate push_sample is called at. + */ +class Lsm6dsvModel : public hal::host::SpiDevice { +public: + /** + * @brief Registers of the main page. + */ + static constexpr std::size_t register_count{128}; + + /** + * @brief Value of WHO_AM_I. + */ + static constexpr uint8_t device_id{0x70}; + + /** + * @brief Power the chip on, in SPI mode 3. + */ + Lsm6dsvModel(); + + /** + * @brief Start a transaction. + */ + void select() override; + + /** + * @brief Take the command, then read or write one register per byte. + * + * @param transmitted Bytes the controller sends. + * @param received Register values read, zero while the command is being sent and during writes. + */ + void exchange(std::span transmitted, std::span received) override; + + /** + * @brief End the transaction. + */ + void deselect() override; + + /** + * @brief Deliver a new sample of the motion the chip measures. + * + * @param angular_velocity Angular velocity about the chip's x, y and z axes, in rad/s. + * @param linear_acceleration Specific force along the chip's x, y and z axes, in m/s^2. + */ + void push_sample(const std::array& angular_velocity, const std::array& linear_acceleration); + + /** + * @brief Get a register without the side effects of reading it over SPI. + * + * @param address Register address; the top bit is ignored. + * @return The register's value. + */ + uint8_t peek(uint8_t address) const; + + /** + * @brief Get the output data rate of the gyroscope, from CTRL2 and HAODR_CFG. + * + * @return The rate, in hertz, zero when the gyroscope is off. + */ + double gyroscope_data_rate() const; + + /** + * @brief Get the output data rate of the accelerometer, from CTRL1 and HAODR_CFG. + * + * @return The rate, in hertz, zero when the accelerometer is off. + */ + double accelerometer_data_rate() const; + + /** + * @brief Get the sensitivity of the gyroscope at the full scale in CTRL6. + * + * @return Angular velocity of one count, in rad/s, zero for a reserved full scale. + */ + double gyroscope_sensitivity() const; + + /** + * @brief Get the sensitivity of the accelerometer at the full scale in CTRL8. + * + * @return Specific force of one count, in m/s^2. + */ + double accelerometer_sensitivity() const; + +private: + /** + * @brief Restore every register to its default. + */ + void reset(); + + /** + * @brief Read a register over SPI, clearing the data ready bit it completes. + * + * @param address Register address. + * @return The register's value. + */ + uint8_t read(uint8_t address); + + /** + * @brief Write a register over SPI, applying what the write triggers. + * + * @param address Register address. + * @param value Value written. + */ + void write(uint8_t address, uint8_t value); + + /** + * @brief Check whether the main page is the one the SPI interface addresses. + * + * @return True unless FUNC_CFG_ACCESS selects another page. + */ + bool main_page() const; + + /** + * @brief Write the three axes of one sensor into its output registers. + * + * @param first Address of the low byte of the x axis. + * @param values Values of the three axes. + * @param sensitivity Value of one count; zero writes zero. + */ + void encode(uint8_t first, const std::array& values, double sensitivity); + + /** + * @brief Decode an output data rate. + * + * @param odr ODR field of CTRL1 or CTRL2. + * @return The rate, in hertz, with the high accuracy set HAODR_CFG selects. + */ + double data_rate(uint8_t odr) const; + + /** + * @brief The register file. + */ + std::array registers{}; + + /** + * @brief Register the next byte of the transaction reads or writes. + */ + uint8_t address{0}; + + /** + * @brief Whether the command byte of the transaction was received. + */ + bool addressed{false}; + + /** + * @brief Whether the transaction reads. + */ + bool reading{false}; + + /** + * @brief Whether the rest of the transaction is ignored, after a reset inside it. + */ + bool ignoring{false}; +}; +} // namespace micras::models + +#endif // MICRAS_MODELS_LSM6DSV_MODEL_HPP diff --git a/micras_proxy/models/src/as5047u_model.cpp b/micras_proxy/models/src/as5047u_model.cpp new file mode 100644 index 0000000..3f649e1 --- /dev/null +++ b/micras_proxy/models/src/as5047u_model.cpp @@ -0,0 +1,187 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/models/as5047u_model.hpp" + +namespace micras::models { +namespace { +/** + * @brief Generator polynomial of the frame CRC, without its top bit. + */ +constexpr uint8_t crc_polynomial{0x1D}; + +/** + * @brief Initial value of the frame CRC. + */ +constexpr uint8_t crc_initial_value{0xC4}; + +/** + * @brief Final XOR of the frame CRC. + */ +constexpr uint8_t crc_final_xor{0xFF}; + +/** + * @brief Bits of the 14-bit address or data field. + */ +constexpr uint16_t field_mask{0x3FFF}; + +/** + * @brief Position of the address or data field in a frame. + */ +constexpr uint32_t field_shift{8}; + +/** + * @brief Read flag of a command frame, and error flag of a data frame. + */ +constexpr uint32_t flag_bit{1U << 22U}; +} // namespace + +As5047uModel::As5047uModel() : SpiDevice{Mode::MODE_1}, answer{this->answer_with(0)} { } + +void As5047uModel::select() { + this->received_bytes = 0; +} + +void As5047uModel::exchange(std::span transmitted, std::span received) { + const std::size_t size = std::min(transmitted.size(), received.size()); + + for (std::size_t index = 0; index < size; index++) { + const uint32_t shift = 8U * static_cast(frame_size - 1 - this->received_bytes); + received[index] = static_cast(this->answer >> shift); + this->frame.at(this->received_bytes) = transmitted[index]; + this->received_bytes++; + + if (this->received_bytes == frame_size) { + this->received_bytes = 0; + this->process(this->frame); + } + } +} + +void As5047uModel::deselect() { + if (this->received_bytes != 0) { + this->errfl |= framing_error; + this->pending_write.reset(); + } + + this->received_bytes = 0; +} + +uint16_t As5047uModel::peek(uint16_t address) const { + switch (address) { + case errfl_address: + return this->errfl; + + case disable_address: + return this->disable; + + case zposm_address: + return this->zposm; + + case zposl_address: + return this->zposl; + + case settings1_address: + return this->settings1; + + case settings2_address: + return this->settings2; + + case settings3_address: + return this->settings3; + + case ecc_address: + return this->ecc; + + default: + return 0; + } +} + +uint8_t As5047uModel::crc(uint8_t high, uint8_t low) { + uint8_t remainder = crc_initial_value; + + for (const uint8_t byte : {high, low}) { + remainder ^= byte; + + for (uint8_t bit = 0; bit < 8; bit++) { + remainder = (remainder & 0x80U) != 0 ? static_cast((remainder << 1U) ^ crc_polynomial) : + static_cast(remainder << 1U); + } + } + + return remainder ^ crc_final_xor; +} + +void As5047uModel::process(const std::array& frame) { + const uint32_t raw = static_cast(std::get<0>(frame)) << 16U | + static_cast(std::get<1>(frame)) << 8U | std::get<2>(frame); + const auto field = static_cast((raw >> field_shift) & field_mask); + + if (crc(std::get<0>(frame), std::get<1>(frame)) != std::get<2>(frame)) { + this->errfl |= crc_error; + this->pending_write.reset(); + this->answer = this->answer_with(0); + return; + } + + if (this->pending_write.has_value()) { + const uint16_t address = this->pending_write.value(); + this->pending_write.reset(); + + if (uint8_t* const target = this->writable(address); target != nullptr) { + *target = static_cast(field); + } + + this->answer = this->answer_with(this->peek(address)); + return; + } + + this->answer = this->answer_with(this->peek(field)); + + if ((raw & flag_bit) == 0) { + this->pending_write = field; + } else if (field == errfl_address) { + this->errfl = 0; + } +} + +uint32_t As5047uModel::answer_with(uint16_t data) const { + const uint32_t upper = (this->errfl != 0 ? flag_bit : 0U) | static_cast(data & field_mask) << field_shift; + return upper | crc(static_cast(upper >> 16U), static_cast(upper >> 8U)); +} + +uint8_t* As5047uModel::writable(uint16_t address) { + switch (address) { + case disable_address: + return &this->disable; + + case zposm_address: + return &this->zposm; + + case zposl_address: + return &this->zposl; + + case settings1_address: + return &this->settings1; + + case settings2_address: + return &this->settings2; + + case settings3_address: + return &this->settings3; + + case ecc_address: + return &this->ecc; + + default: + return nullptr; + } +} +} // namespace micras::models diff --git a/micras_proxy/models/src/lsm6dsv_model.cpp b/micras_proxy/models/src/lsm6dsv_model.cpp new file mode 100644 index 0000000..f077bc4 --- /dev/null +++ b/micras_proxy/models/src/lsm6dsv_model.cpp @@ -0,0 +1,275 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/models/lsm6dsv_model.hpp" + +namespace micras::models { +namespace { +/** + * @brief Top bit of the command byte: set to read, clear to write. + */ +constexpr uint8_t read_flag{0x80}; + +/** + * @brief Bits of the command byte and of an address that name a register. + */ +constexpr uint8_t address_mask{0x7F}; + +/** + * @brief Addresses of the registers the model gives a behavior to. + */ +///@{ +constexpr uint8_t func_cfg_access{0x01}; +constexpr uint8_t pin_ctrl{0x02}; +constexpr uint8_t who_am_i{0x0F}; +constexpr uint8_t ctrl1{0x10}; +constexpr uint8_t ctrl2{0x11}; +constexpr uint8_t ctrl3{0x12}; +constexpr uint8_t ctrl6{0x15}; +constexpr uint8_t ctrl8{0x17}; +constexpr uint8_t status_reg{0x1E}; +constexpr uint8_t outx_l_g{0x22}; +constexpr uint8_t outx_l_a{0x28}; +constexpr uint8_t haodr_cfg{0x62}; +///@} + +/** + * @brief Bits of FUNC_CFG_ACCESS: the software power-on reset, and the two that select another page. + */ +///@{ +constexpr uint8_t sw_por{0x04}; +constexpr uint8_t other_pages{0xC0}; +///@} + +/** + * @brief Bits of CTRL3: the software reset, the self-clearing reboot, and the address auto-increment. + */ +///@{ +constexpr uint8_t sw_reset{0x01}; +constexpr uint8_t boot{0x80}; +constexpr uint8_t if_inc{0x04}; +///@} + +/** + * @brief Data ready bits of STATUS_REG. + */ +///@{ +constexpr uint8_t xlda{0x01}; +constexpr uint8_t gda{0x02}; +///@} + +/** + * @brief Defaults of the registers that do not reset to zero. + */ +constexpr std::array, 3> nonzero_defaults{{ + {pin_ctrl, 0x23}, + {who_am_i, Lsm6dsvModel::device_id}, + {ctrl3, 0x44}, +}}; + +/** + * @brief First and last address of every range of registers the SPI interface can write. + */ +constexpr std::array, 7> writable_ranges{{ + {0x01, 0x03}, + {0x06, 0x0E}, + {0x10, 0x19}, + {0x50, 0x51}, + {0x54, 0x5F}, + {0x62, 0x6B}, + {0x6F, 0x75}, +}}; + +/** + * @brief Sensitivity of the gyroscope for each FS_G code, in mdps per count; zero for the reserved codes. + */ +constexpr std::array gyroscope_mdps_per_count{ + 4.375, 8.75, 17.5, 35.0, 70.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 140.0, 0.0, 0.0, 0.0, +}; + +/** + * @brief Sensitivity of the accelerometer for each FS_XL code, in mg per count. + */ +constexpr std::array accelerometer_mg_per_count{0.061, 0.122, 0.244, 0.488}; + +/** + * @brief Conversions from the datasheet's units. + */ +///@{ +constexpr double mdps_to_radps{std::numbers::pi / 180000.0}; +constexpr double mg_to_mps2{0.00980665}; +///@} + +/** + * @brief Rate of ODR code 3 for each high accuracy set of HAODR_CFG, in hertz; each code above doubles it. + */ +constexpr std::array base_rates{15.0, 15.625, 12.5, 15.0}; + +/** + * @brief Check whether the SPI interface can write a register. + * + * @param address Register address. + * @return True for a read-write register. + */ +bool writable(uint8_t address) { + return std::ranges::any_of(writable_ranges, [address](const auto& range) { + return address >= range.first and address <= range.second; + }); +} +} // namespace + +Lsm6dsvModel::Lsm6dsvModel() : SpiDevice{Mode::MODE_3} { + this->reset(); +} + +void Lsm6dsvModel::select() { + this->addressed = false; + this->ignoring = false; +} + +void Lsm6dsvModel::exchange(std::span transmitted, std::span received) { + const std::size_t size = std::min(transmitted.size(), received.size()); + + for (std::size_t index = 0; index < size; index++) { + received[index] = 0; + + if (this->ignoring) { + continue; + } + + if (not this->addressed) { + this->reading = (transmitted[index] & read_flag) != 0; + this->address = transmitted[index] & address_mask; + this->addressed = true; + continue; + } + + if (this->reading) { + received[index] = this->read(this->address); + } else { + this->write(this->address, transmitted[index]); + } + + if ((this->registers.at(ctrl3) & if_inc) != 0) { + this->address = (this->address + 1) & address_mask; + } + } +} + +void Lsm6dsvModel::deselect() { + this->addressed = false; + this->ignoring = false; +} + +void Lsm6dsvModel::push_sample( + const std::array& angular_velocity, const std::array& linear_acceleration +) { + if ((this->registers.at(ctrl2) & 0x0F) != 0) { + this->encode(outx_l_g, angular_velocity, this->gyroscope_sensitivity()); + this->registers.at(status_reg) |= gda; + } + + if ((this->registers.at(ctrl1) & 0x0F) != 0) { + this->encode(outx_l_a, linear_acceleration, this->accelerometer_sensitivity()); + this->registers.at(status_reg) |= xlda; + } +} + +uint8_t Lsm6dsvModel::peek(uint8_t address) const { + return this->registers.at(address & address_mask); +} + +double Lsm6dsvModel::gyroscope_data_rate() const { + return this->data_rate(this->registers.at(ctrl2) & 0x0F); +} + +double Lsm6dsvModel::accelerometer_data_rate() const { + return this->data_rate(this->registers.at(ctrl1) & 0x0F); +} + +double Lsm6dsvModel::gyroscope_sensitivity() const { + return gyroscope_mdps_per_count.at(this->registers.at(ctrl6) & 0x0F) * mdps_to_radps; +} + +double Lsm6dsvModel::accelerometer_sensitivity() const { + return accelerometer_mg_per_count.at(this->registers.at(ctrl8) & 0x03) * mg_to_mps2; +} + +void Lsm6dsvModel::reset() { + this->registers.fill(0); + + for (const auto& [address, value] : nonzero_defaults) { + this->registers.at(address) = value; + } +} + +uint8_t Lsm6dsvModel::read(uint8_t address) { + if (not this->main_page() and address != func_cfg_access) { + return 0; + } + + const uint8_t value = this->registers.at(address); + + if (address >= outx_l_g and address < outx_l_a and (address & 1) != 0) { + this->registers.at(status_reg) &= ~gda; + } else if (address >= outx_l_a and address < outx_l_a + 6 and (address & 1) != 0) { + this->registers.at(status_reg) &= ~xlda; + } + + return value; +} + +void Lsm6dsvModel::write(uint8_t address, uint8_t value) { + if ((not this->main_page() and address != func_cfg_access) or not writable(address)) { + return; + } + + if ((address == func_cfg_access and (value & sw_por) != 0) or (address == ctrl3 and (value & sw_reset) != 0)) { + this->reset(); + this->ignoring = true; + return; + } + + this->registers.at(address) = address == ctrl3 ? value & ~boot : value; +} + +bool Lsm6dsvModel::main_page() const { + return (this->registers.at(func_cfg_access) & other_pages) == 0; +} + +void Lsm6dsvModel::encode(uint8_t first, const std::array& values, double sensitivity) { + for (std::size_t axis = 0; axis < values.size(); axis++) { + const double counts = sensitivity > 0.0 ? std::round(values.at(axis) / sensitivity) : 0.0; + const auto word = static_cast(static_cast(std::clamp(counts, -32768.0, 32767.0))); + const auto low = static_cast(first + (2 * axis)); + + this->registers.at(low) = static_cast(word); + this->registers.at(low + 1) = static_cast(word >> 8U); + } +} + +double Lsm6dsvModel::data_rate(uint8_t odr) const { + if (odr == 0 or odr > 12) { + return 0.0; + } + + if (odr == 1) { + return 1.875; + } + + if (odr == 2) { + return 7.5; + } + + return base_rates.at(this->registers.at(haodr_cfg) & 0x03) * static_cast(1U << (odr - 3U)); +} +} // namespace micras::models diff --git a/micras_proxy/models/tests/as5047u_model_test.cpp b/micras_proxy/models/tests/as5047u_model_test.cpp new file mode 100644 index 0000000..17415a7 --- /dev/null +++ b/micras_proxy/models/tests/as5047u_model_test.cpp @@ -0,0 +1,219 @@ +/** + * @file + */ + +#include +#include + +#include + +#include "micras/models/as5047u_model.hpp" + +namespace micras::models { +namespace { +using Frame = std::array; + +constexpr uint32_t read_flag{1U << 22U}; +constexpr uint32_t error_flag{1U << 22U}; + +Frame frame_of(uint32_t upper) { + const auto high = static_cast(upper >> 16U); + const auto low = static_cast(upper >> 8U); + return {high, low, As5047uModel::crc(high, low)}; +} + +Frame read_command(uint16_t address) { + return frame_of(read_flag | static_cast(address) << 8U); +} + +Frame write_command(uint16_t address) { + return frame_of(static_cast(address) << 8U); +} + +Frame data_frame(uint16_t data) { + return frame_of(static_cast(data) << 8U); +} + +uint32_t transfer(As5047uModel& chip, const Frame& frame) { + Frame received{}; + chip.select(); + chip.exchange(frame, received); + chip.deselect(); + return static_cast(std::get<0>(received)) << 16U | static_cast(std::get<1>(received)) << 8U | + std::get<2>(received); +} + +uint16_t data_of(uint32_t answer) { + return static_cast((answer >> 8U) & 0x3FFFU); +} + +bool valid(uint32_t answer) { + return As5047uModel::crc(static_cast(answer >> 16U), static_cast(answer >> 8U)) == + static_cast(answer); +} + +uint16_t read_register(As5047uModel& chip, uint16_t address) { + transfer(chip, read_command(address)); + return data_of(transfer(chip, read_command(As5047uModel::nop_address))); +} + +void write_register(As5047uModel& chip, uint16_t address, uint16_t data) { + transfer(chip, write_command(address)); + transfer(chip, data_frame(data)); +} + +TEST(As5047uModel, AnswersInSpiMode1) { + const As5047uModel chip; + + EXPECT_EQ(chip.mode(), hal::host::SpiDevice::Mode::MODE_1); +} + +TEST(As5047uModel, ComputesTheFrameCrc) { + EXPECT_EQ(As5047uModel::crc(0x00, 0x00), 0xF1); + EXPECT_EQ(As5047uModel::crc(0x40, 0x01), 0x06); + EXPECT_EQ(As5047uModel::crc(0x40, 0x1A), 0x04); + EXPECT_EQ(As5047uModel::crc(0x00, 0x80), 0xD7); +} + +TEST(As5047uModel, PowersOnWithZeroedRegistersAndAnEmptyAnswer) { + As5047uModel chip; + + for (const uint16_t address : + {As5047uModel::errfl_address, As5047uModel::disable_address, As5047uModel::zposm_address, + As5047uModel::zposl_address, As5047uModel::settings1_address, As5047uModel::settings2_address, + As5047uModel::settings3_address, As5047uModel::ecc_address}) { + EXPECT_EQ(chip.peek(address), 0) << "register " << address; + } + + EXPECT_EQ(transfer(chip, read_command(As5047uModel::nop_address)), 0x0000F1U); +} + +TEST(As5047uModel, AnswersEachFrameInTheNextOne) { + As5047uModel chip; + write_register(chip, As5047uModel::settings3_address, 0x80); + write_register(chip, As5047uModel::zposm_address, 0x12); + + transfer(chip, read_command(As5047uModel::settings3_address)); + const uint32_t settings3 = transfer(chip, read_command(As5047uModel::zposm_address)); + const uint32_t zposm = transfer(chip, read_command(As5047uModel::nop_address)); + + EXPECT_EQ(data_of(settings3), 0x80); + EXPECT_EQ(data_of(zposm), 0x12); + EXPECT_TRUE(valid(settings3)); + EXPECT_TRUE(valid(zposm)); +} + +TEST(As5047uModel, WritesWithACommandAndADataFrame) { + As5047uModel chip; + write_register(chip, As5047uModel::settings2_address, 0x04); + + transfer(chip, write_command(As5047uModel::settings2_address)); + const uint32_t old_content = transfer(chip, data_frame(0x21)); + const uint32_t new_content = transfer(chip, read_command(As5047uModel::nop_address)); + + EXPECT_EQ(data_of(old_content), 0x04); + EXPECT_EQ(data_of(new_content), 0x21); + EXPECT_EQ(chip.peek(As5047uModel::settings2_address), 0x21); +} + +TEST(As5047uModel, HoldsEveryVolatileRegister) { + As5047uModel chip; + uint16_t value = 0x11; + + for (const uint16_t address : + {As5047uModel::disable_address, As5047uModel::zposm_address, As5047uModel::zposl_address, + As5047uModel::settings1_address, As5047uModel::settings2_address, As5047uModel::settings3_address, + As5047uModel::ecc_address}) { + write_register(chip, address, value); + EXPECT_EQ(read_register(chip, address), value) << "register " << address; + value += 0x11; + } +} + +TEST(As5047uModel, IgnoresWritesToOtherAddresses) { + As5047uModel chip; + + write_register(chip, As5047uModel::errfl_address, 0x7F); + write_register(chip, 0x3FFF, 0x7F); + + EXPECT_EQ(chip.peek(As5047uModel::errfl_address), 0); + EXPECT_EQ(read_register(chip, 0x3FFF), 0); +} + +TEST(As5047uModel, RejectsAFrameWithAWrongCrc) { + As5047uModel chip; + Frame command = write_command(As5047uModel::settings3_address); + std::get<2>(command) ^= 0x01; + + transfer(chip, command); + const uint32_t after_error = transfer(chip, data_frame(0x80)); + + EXPECT_EQ(chip.peek(As5047uModel::settings3_address), 0); + EXPECT_EQ(chip.peek(As5047uModel::errfl_address), As5047uModel::crc_error); + EXPECT_NE(after_error & error_flag, 0U); + EXPECT_TRUE(valid(after_error)); +} + +TEST(As5047uModel, DropsAWriteWhoseDataFrameHasAWrongCrc) { + As5047uModel chip; + Frame data = data_frame(0x80); + std::get<2>(data) ^= 0x80; + + transfer(chip, write_command(As5047uModel::settings3_address)); + transfer(chip, data); + transfer(chip, data_frame(0x80)); + + EXPECT_EQ(chip.peek(As5047uModel::settings3_address), 0); + EXPECT_EQ(chip.peek(As5047uModel::errfl_address), As5047uModel::crc_error); +} + +TEST(As5047uModel, ClearsTheErrorFlagsWhenErrflIsRead) { + As5047uModel chip; + Frame command = read_command(As5047uModel::settings3_address); + std::get<0>(command) ^= 0x01; + transfer(chip, command); + + transfer(chip, read_command(As5047uModel::errfl_address)); + const uint32_t errfl = transfer(chip, read_command(As5047uModel::nop_address)); + const uint32_t after = transfer(chip, read_command(As5047uModel::nop_address)); + + EXPECT_EQ(data_of(errfl), As5047uModel::crc_error); + EXPECT_NE(errfl & error_flag, 0U); + EXPECT_EQ(after & error_flag, 0U); + EXPECT_EQ(chip.peek(As5047uModel::errfl_address), 0); +} + +TEST(As5047uModel, FlagsAFrameCutShortByTheChipSelect) { + As5047uModel chip; + const Frame command = write_command(As5047uModel::settings3_address); + const std::array partial{std::get<0>(command), std::get<1>(command)}; + std::array received{}; + + chip.select(); + chip.exchange(partial, received); + chip.deselect(); + transfer(chip, data_frame(0x80)); + + EXPECT_EQ(chip.peek(As5047uModel::settings3_address), 0); + EXPECT_EQ(chip.peek(As5047uModel::errfl_address), As5047uModel::framing_error); +} + +TEST(As5047uModel, TakesAFrameSplitAcrossExchanges) { + As5047uModel chip; + const Frame command = write_command(As5047uModel::zposl_address); + const std::array first{std::get<0>(command)}; + const std::array rest{std::get<1>(command), std::get<2>(command)}; + std::array first_received{}; + std::array rest_received{}; + + chip.select(); + chip.exchange(first, first_received); + chip.exchange(rest, rest_received); + chip.deselect(); + transfer(chip, data_frame(0x3F)); + + EXPECT_EQ(chip.peek(As5047uModel::zposl_address), 0x3F); + EXPECT_EQ(chip.peek(As5047uModel::errfl_address), 0); +} +} // namespace +} // namespace micras::models diff --git a/micras_proxy/models/tests/lsm6dsv_model_test.cpp b/micras_proxy/models/tests/lsm6dsv_model_test.cpp new file mode 100644 index 0000000..2ca6fad --- /dev/null +++ b/micras_proxy/models/tests/lsm6dsv_model_test.cpp @@ -0,0 +1,327 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "lsm6dsv_reg.h" +#include "micras/models/lsm6dsv_model.hpp" + +namespace micras::models { +namespace { +constexpr uint8_t read_flag{0x80}; + +std::vector transact(Lsm6dsvModel& chip, std::vector transmitted) { + std::vector received(transmitted.size()); + chip.select(); + chip.exchange(transmitted, received); + chip.deselect(); + return received; +} + +void write(Lsm6dsvModel& chip, uint8_t address, uint8_t value) { + transact(chip, {address, value}); +} + +uint8_t read(Lsm6dsvModel& chip, uint8_t address) { + return transact(chip, {static_cast(address | read_flag), 0}).at(1); +} + +int16_t word(const std::vector& bytes, std::size_t low) { + return static_cast(bytes.at(low) | (bytes.at(low + 1) << 8U)); +} + +void configure(Lsm6dsvModel& chip, uint8_t gyroscope_scale, uint8_t accelerometer_scale) { + write(chip, LSM6DSV_HAODR_CFG, 0x01); + write(chip, LSM6DSV_CTRL1, 0x1C); + write(chip, LSM6DSV_CTRL2, 0x1C); + write(chip, LSM6DSV_CTRL6, gyroscope_scale); + write(chip, LSM6DSV_CTRL8, accelerometer_scale); +} + +std::vector read_burst(Lsm6dsvModel& chip) { + std::vector command(17); + command.at(0) = LSM6DSV_STATUS_REG | read_flag; + return transact(chip, command); +} + +TEST(Lsm6dsvModel, AnswersInSpiMode3) { + const Lsm6dsvModel chip; + + EXPECT_EQ(chip.mode(), hal::host::SpiDevice::Mode::MODE_3); +} + +TEST(Lsm6dsvModel, PowersOnWithTheDatasheetDefaults) { + const Lsm6dsvModel chip; + + for (uint8_t address = 0; address < Lsm6dsvModel::register_count; address++) { + uint8_t expected = 0x00; + + if (address == LSM6DSV_WHO_AM_I) { + expected = LSM6DSV_ID; + } else if (address == LSM6DSV_PIN_CTRL) { + expected = 0x23; + } else if (address == LSM6DSV_CTRL3) { + expected = 0x44; + } + + EXPECT_EQ(chip.peek(address), expected) << "register " << static_cast(address); + } +} + +TEST(Lsm6dsvModel, AnswersWhoAmIWithTheDeviceId) { + Lsm6dsvModel chip; + + const std::vector received = transact(chip, {LSM6DSV_WHO_AM_I | read_flag, 0xFF}); + + EXPECT_EQ(received.at(0), 0x00); + EXPECT_EQ(received.at(1), LSM6DSV_ID); +} + +TEST(Lsm6dsvModel, AdvancesTheAddressAfterEveryByte) { + Lsm6dsvModel chip; + + transact(chip, {LSM6DSV_CTRL1, 0x16, 0x1C}); + const std::vector received = transact(chip, {LSM6DSV_CTRL1 | read_flag, 0, 0}); + + EXPECT_EQ(received.at(1), 0x16); + EXPECT_EQ(received.at(2), 0x1C); +} + +TEST(Lsm6dsvModel, KeepsOneTransactionAcrossSeveralExchanges) { + Lsm6dsvModel chip; + write(chip, LSM6DSV_CTRL2, 0x1C); + + const std::array command{LSM6DSV_CTRL1 | read_flag}; + std::array ignored{}; + const std::array dummy{}; + std::array received{}; + + chip.select(); + chip.exchange(command, ignored); + chip.exchange(dummy, received); + chip.deselect(); + + EXPECT_EQ(std::get<0>(received), 0x00); + EXPECT_EQ(std::get<1>(received), 0x1C); +} + +TEST(Lsm6dsvModel, StartsEveryTransactionWithACommand) { + Lsm6dsvModel chip; + + transact(chip, {LSM6DSV_CTRL1, 0x16}); + transact(chip, {LSM6DSV_CTRL2, 0x1C}); + + EXPECT_EQ(chip.peek(LSM6DSV_CTRL1), 0x16); + EXPECT_EQ(chip.peek(LSM6DSV_CTRL2), 0x1C); +} + +TEST(Lsm6dsvModel, StaysOnOneRegisterWithoutAutoIncrement) { + Lsm6dsvModel chip; + write(chip, LSM6DSV_CTRL3, 0x40); + + transact(chip, {LSM6DSV_CTRL1, 0x16, 0x1C}); + const std::vector received = transact(chip, {LSM6DSV_CTRL1 | read_flag, 0, 0}); + + EXPECT_EQ(received.at(1), 0x1C); + EXPECT_EQ(received.at(2), 0x1C); + EXPECT_EQ(chip.peek(LSM6DSV_CTRL2), 0x00); +} + +TEST(Lsm6dsvModel, IgnoresWritesToReadOnlyAndReservedRegisters) { + Lsm6dsvModel chip; + + write(chip, LSM6DSV_WHO_AM_I, 0x00); + write(chip, LSM6DSV_STATUS_REG, 0xFF); + write(chip, LSM6DSV_OUTX_L_G, 0x12); + write(chip, 0x04, 0x34); + + EXPECT_EQ(chip.peek(LSM6DSV_WHO_AM_I), LSM6DSV_ID); + EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_OUTX_L_G), 0x00); + EXPECT_EQ(chip.peek(0x04), 0x00); +} + +TEST(Lsm6dsvModel, RestoresTheDefaultsOnASoftwarePowerOnReset) { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + transact(chip, {LSM6DSV_FUNC_CFG_ACCESS, 0x04, 0x55}); + + EXPECT_EQ(chip.peek(LSM6DSV_FUNC_CFG_ACCESS), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_PIN_CTRL), 0x23); + EXPECT_EQ(chip.peek(LSM6DSV_CTRL1), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_CTRL2), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_CTRL3), 0x44); + EXPECT_EQ(chip.peek(LSM6DSV_CTRL6), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_OUTX_L_G), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_HAODR_CFG), 0x00); + EXPECT_EQ(read(chip, LSM6DSV_WHO_AM_I), LSM6DSV_ID); +} + +TEST(Lsm6dsvModel, RestoresTheDefaultsOnASoftwareReset) { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + + write(chip, LSM6DSV_CTRL3, 0x45); + + EXPECT_EQ(chip.peek(LSM6DSV_CTRL3), 0x44); + EXPECT_EQ(chip.peek(LSM6DSV_CTRL2), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_CTRL6), 0x00); +} + +TEST(Lsm6dsvModel, HidesTheMainPageWhileAnotherIsSelected) { + Lsm6dsvModel chip; + + write(chip, LSM6DSV_FUNC_CFG_ACCESS, 0x80); + EXPECT_EQ(read(chip, LSM6DSV_WHO_AM_I), 0x00); + write(chip, LSM6DSV_CTRL1, 0x1C); + + write(chip, LSM6DSV_FUNC_CFG_ACCESS, 0x00); + EXPECT_EQ(read(chip, LSM6DSV_WHO_AM_I), LSM6DSV_ID); + EXPECT_EQ(chip.peek(LSM6DSV_CTRL1), 0x00); +} + +TEST(Lsm6dsvModel, IgnoresSamplesWhileTheSensorsAreOff) { + Lsm6dsvModel chip; + + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_OUTX_L_G), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_OUTX_L_A), 0x00); +} + +TEST(Lsm6dsvModel, SetsTheDataReadyBitOfEachSensorThatIsOn) { + Lsm6dsvModel chip; + write(chip, LSM6DSV_CTRL2, 0x1C); + + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x02); + EXPECT_EQ(chip.peek(LSM6DSV_OUTX_L_A), 0x00); + + write(chip, LSM6DSV_CTRL1, 0x1C); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x03); +} + +TEST(Lsm6dsvModel, ClearsTheDataReadyBitsWithTheBurstThatReadsTheSample) { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + const std::vector first = read_burst(chip); + const std::vector second = read_burst(chip); + + EXPECT_EQ(first.at(1), 0x03); + EXPECT_EQ(second.at(1), 0x00); + EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x00); + EXPECT_EQ(word(second, 5), word(first, 5)); +} + +TEST(Lsm6dsvModel, ClearsEachDataReadyBitWithAHighByteOfItsSensor) { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + read(chip, LSM6DSV_OUTX_L_G); + EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x03); + + read(chip, LSM6DSV_OUTY_H_G); + EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x01); + + read(chip, LSM6DSV_OUTZ_H_A); + EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x00); +} + +TEST(Lsm6dsvModel, EncodesTheSampleWithTheFullScaleInTheRegisters) { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, -1.0, 0.0}, {9.80665, -9.80665, 0.0}); + + const std::vector wide = read_burst(chip); + + EXPECT_EQ(word(wide, 5), 409); + EXPECT_EQ(word(wide, 7), -409); + EXPECT_EQ(word(wide, 9), 0); + EXPECT_EQ(word(wide, 11), 4098); + EXPECT_EQ(word(wide, 13), -4098); + EXPECT_EQ(word(wide, 15), 0); + + write(chip, LSM6DSV_CTRL6, LSM6DSV_125dps); + write(chip, LSM6DSV_CTRL8, LSM6DSV_2g); + chip.push_sample({1.0, 100.0, -100.0}, {9.80665, 100.0, -100.0}); + + const std::vector narrow = read_burst(chip); + + EXPECT_EQ(word(narrow, 5), 13096); + EXPECT_EQ(word(narrow, 7), 32767); + EXPECT_EQ(word(narrow, 9), -32768); + EXPECT_EQ(word(narrow, 11), 16393); + EXPECT_EQ(word(narrow, 13), 32767); + EXPECT_EQ(word(narrow, 15), -32768); +} + +TEST(Lsm6dsvModel, GivesTheSensitivityOfEveryFullScale) { + Lsm6dsvModel chip; + constexpr double mdps{std::numbers::pi / 180000.0}; + constexpr double mg{0.00980665}; + + const std::array, 6> gyroscope{{ + {LSM6DSV_125dps, 4.375}, + {LSM6DSV_250dps, 8.75}, + {LSM6DSV_500dps, 17.5}, + {LSM6DSV_1000dps, 35.0}, + {LSM6DSV_2000dps, 70.0}, + {LSM6DSV_4000dps, 140.0}, + }}; + const std::array, 4> accelerometer{{ + {LSM6DSV_2g, 0.061}, + {LSM6DSV_4g, 0.122}, + {LSM6DSV_8g, 0.244}, + {LSM6DSV_16g, 0.488}, + }}; + + for (const auto& [code, sensitivity] : gyroscope) { + write(chip, LSM6DSV_CTRL6, code); + EXPECT_DOUBLE_EQ(chip.gyroscope_sensitivity(), sensitivity * mdps); + } + + for (const auto& [code, sensitivity] : accelerometer) { + write(chip, LSM6DSV_CTRL8, code); + EXPECT_DOUBLE_EQ(chip.accelerometer_sensitivity(), sensitivity * mg); + } +} + +TEST(Lsm6dsvModel, DecodesTheOutputDataRates) { + Lsm6dsvModel chip; + + EXPECT_EQ(chip.gyroscope_data_rate(), 0.0); + EXPECT_EQ(chip.accelerometer_data_rate(), 0.0); + + write(chip, LSM6DSV_CTRL2, 0x10 | (LSM6DSV_ODR_HA01_AT_8000Hz & 0x0F)); + write(chip, LSM6DSV_CTRL1, 0x10 | (LSM6DSV_ODR_HA01_AT_125Hz & 0x0F)); + + EXPECT_EQ(chip.gyroscope_data_rate(), 7680.0); + + write(chip, LSM6DSV_HAODR_CFG, 0x01); + EXPECT_EQ(chip.gyroscope_data_rate(), 8000.0); + EXPECT_EQ(chip.accelerometer_data_rate(), 125.0); + + write(chip, LSM6DSV_HAODR_CFG, 0x02); + EXPECT_EQ(chip.gyroscope_data_rate(), 6400.0); + + write(chip, LSM6DSV_CTRL1, LSM6DSV_ODR_AT_1Hz875); + EXPECT_EQ(chip.accelerometer_data_rate(), 1.875); +} +} // namespace +} // namespace micras::models From 9f6f2972aef64a6c648015c57b90be28e961b5ee Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 03:45:18 -0300 Subject: [PATCH 528/561] =?UTF-8?q?=E2=9C=A8=20Run=20the=20firmware's=20ow?= =?UTF-8?q?n=20SPI=20chip=20proxies=20over=20the=20chip=20models?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The Micras target now compiles the firmware's imu.cpp and rotary_sensor.cpp and ST's lsm6dsv_reg.c, and its own replacements are gone. The bindings attach an Lsm6dsvModel and two As5047uModels to hspi3 by the chip selects target.hpp names, and turn each sample of the IMU device back into rad/s and m/s^2 for Lsm6dsvModel::push_sample; the encoders' position stays on the timer encoders. The sample port had no other user and is deleted. The firmware's objects outlive the target, so unwire forgets the ports and with them the chips. Recorded as baselines/v2, the gate's baseline now, because the behavior moved: - the real Imu reads the burst it started one update() earlier, so a sample reaches the firmware one loop later; - its constructor waits 10 ms, then 30 ms after the software power-on reset, so the robot exists 40 ms into the run: INIT and IDLE move from 0.000125 s and 0.00025 s to 0.040125 s and 0.04025 s, and the CSV starts at tick 320; - the scale of every IMU reading comes from the full scale the firmware wrote into CTRL6 and CTRL8. Every value stays within v1's tolerances: the explore reaches the goal at 26.891 s instead of 26.880 s, with a maximum pose error of 12.90 mm instead of 12.78 mm. --- .../host/include/micras/hal/host/board.hpp | 8 ----- .../host/include/micras/hal/host/ports.hpp | 21 ----------- micras_hal/host/src/board.cpp | 35 ++++++------------- 3 files changed, 11 insertions(+), 53 deletions(-) diff --git a/micras_hal/host/include/micras/hal/host/board.hpp b/micras_hal/host/include/micras/hal/host/board.hpp index 193413e..9e90397 100644 --- a/micras_hal/host/include/micras/hal/host/board.hpp +++ b/micras_hal/host/include/micras/hal/host/board.hpp @@ -87,14 +87,6 @@ class Board { */ static EncoderPort& encoder(const void* timer); - /** - * @brief Get a named sample port, for a swapped chip driver. - * - * @param name Name both sides agree on. - * @return The port. - */ - static SamplePort& samples(std::string_view name); - /** * @brief Get the port of one chip select of an SPI bus. * diff --git a/micras_hal/host/include/micras/hal/host/ports.hpp b/micras_hal/host/include/micras/hal/host/ports.hpp index 11ef9c1..c9ec81c 100644 --- a/micras_hal/host/include/micras/hal/host/ports.hpp +++ b/micras_hal/host/include/micras/hal/host/ports.hpp @@ -7,7 +7,6 @@ #ifndef MICRAS_HAL_HOST_PORTS_HPP #define MICRAS_HAL_HOST_PORTS_HPP -#include #include #include #include @@ -230,26 +229,6 @@ struct EncoderPort : Port { int32_t count{0}; }; -/** - * @brief Samples a swapped chip driver reads, such as an inertial measurement unit's. - */ -struct SamplePort : Port { - /** - * @brief Largest number of values one sample holds. - */ - static constexpr std::size_t max_values{16}; - - /** - * @brief Latest sample. - */ - std::array values{}; - - /** - * @brief Incremented with every new sample. - */ - uint32_t sequence{0}; -}; - /** * @brief One chip select of an SPI bus, and the device behind it. * diff --git a/micras_hal/host/src/board.cpp b/micras_hal/host/src/board.cpp index 5d53e31..f309a54 100644 --- a/micras_hal/host/src/board.cpp +++ b/micras_hal/host/src/board.cpp @@ -27,18 +27,17 @@ using SpiKey = std::tuple; * @brief Every port and name, created on first use. */ struct Registry { - std::map gpios; - std::map pwms; - std::map pwm_dmas; - std::map adcs; - std::map uarts; - std::map encoders; - std::map> samples; - std::map spis; - std::map gpio_names; - std::map handle_names; - FlashPort flash{{.name = "flash"}}; - McuPort mcu{{.name = "mcu"}}; + std::map gpios; + std::map pwms; + std::map pwm_dmas; + std::map adcs; + std::map uarts; + std::map encoders; + std::map spis; + std::map gpio_names; + std::map handle_names; + FlashPort flash{{.name = "flash"}}; + McuPort mcu{{.name = "mcu"}}; }; /** @@ -126,17 +125,6 @@ EncoderPort& Board::encoder(const void* timer) { return find_or_create(registry().encoders, timer, [timer] { return handle_name(timer) + " encoder"; }); } -SamplePort& Board::samples(std::string_view name) { - auto found = registry().samples.find(name); - - if (found == registry().samples.end()) { - found = registry().samples.emplace(std::string{name}, SamplePort{}).first; - found->second.name = std::string{name}; - } - - return found->second; -} - SpiPort& Board::spi(const void* spi, const void* cs_port, uint16_t cs_pin) { return find_or_create(registry().spis, SpiKey{spi, cs_port, cs_pin}, [spi, cs_port, cs_pin] { return std::format("{} {}", handle_name(spi), gpio_name(Key{cs_port, cs_pin})); @@ -190,7 +178,6 @@ std::vector Board::unbound() { collect(registry().adcs); collect(registry().uarts); collect(registry().encoders); - collect(registry().samples); collect(registry().spis); return names; From c14c6a7b55fbfda3396f7c02a98e62648d0ee751 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 05:03:14 -0300 Subject: [PATCH 529/561] =?UTF-8?q?=F0=9F=94=A7=20Make=20the=20shared=20bu?= =?UTF-8?q?ild=20and=20style=20files=20canonical?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit micras-lib holds the copy of every file the three repositories share. The warnings take their -Werror switch from the caller, so each repository keeps its own, and the GCC-only flags are guarded so that a clang build accepts them. The lint script runs the clang tools found by their versioned names, with the extra arguments each compiler needs. The tidy header filter reaches the .tpp files and leaves dependencies out, and the tests drop the cognitive complexity check that the test macros inflate. The host stage of the Dockerfile is the image every host job runs in. --- .clang-tidy | 3 ++- .docker/Dockerfile | 39 ++++++++++++++++++++++++++++ cmake/micras_warnings.cmake | 37 +++++++++++++++++--------- cmake/templates/run_clang_tidy.sh.in | 38 +++++++++++++++++++-------- tests/.clang-tidy | 7 +++++ 5 files changed, 100 insertions(+), 24 deletions(-) create mode 100644 .docker/Dockerfile create mode 100644 tests/.clang-tidy diff --git a/.clang-tidy b/.clang-tidy index 8714925..33139c4 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -58,7 +58,8 @@ ImplementationFileExtensions: - cc - cpp - cxx -HeaderFilterRegex: '(include|config)/.*\.hpp$' +HeaderFilterRegex: '(include|config)/.*\.(hpp|tpp)$' +ExcludeHeaderFilterRegex: '(_deps|external)/' FormatStyle: file CheckOptions: # The vendor HAL module headers are not standalone; they require stm32xx_hal.h to have diff --git a/.docker/Dockerfile b/.docker/Dockerfile new file mode 100644 index 0000000..37af454 --- /dev/null +++ b/.docker/Dockerfile @@ -0,0 +1,39 @@ +# syntax=docker/dockerfile:1.27 + +# >>> host stage >>> +############################################################################### +# Host tools: shared byte for byte by micras-lib, micras-simulation and # +# MicrasFirmware, whose CI compares the lines between the two markers # +############################################################################### +FROM ubuntu:26.04 AS host + +# The compilers and the clang tooling are pinned to a major version, so that a distribution update +# can move neither the generated code nor the formatting rules and the enabled check set +ARG HOST_GCC_VERSION=15 +ARG HOST_CLANG_VERSION=22 + +# The apt caches are kept in BuildKit cache mounts instead of the image layers, so the hook that +# deletes the downloaded packages after every install has to go +RUN rm -f /etc/apt/apt.conf.d/docker-clean + +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + apt-get update && \ + apt-get install -y --no-install-recommends \ + ca-certificates \ + cmake \ + doxygen \ + git \ + graphviz \ + ninja-build \ + python3 \ + python3-matplotlib \ + python3-numpy \ + "gcc-${HOST_GCC_VERSION}" \ + "g++-${HOST_GCC_VERSION}" \ + "clang-${HOST_CLANG_VERSION}" \ + "clang-format-${HOST_CLANG_VERSION}" \ + "clang-tidy-${HOST_CLANG_VERSION}" + +WORKDIR /project +# <<< host stage <<< diff --git a/cmake/micras_warnings.cmake b/cmake/micras_warnings.cmake index fef905f..eb201b2 100644 --- a/cmake/micras_warnings.cmake +++ b/cmake/micras_warnings.cmake @@ -2,12 +2,12 @@ ## Project Warning Flags ############################################################################### -# These flags deliberately live here and not in cube/cmake/gcc-arm-none-eabi.cmake, -# because that file is regenerated by STM32CubeMX and any edit to it is lost. +# The canonical copy lives in micras-lib; micras-simulation and MicrasFirmware carry the same file +# byte for byte, and the firmware's CI compares them. The -Werror switch belongs to each repository +# (MICRAS_LIB_WERROR, MICRAS_SIM_WERROR, MICRAS_WERROR) and is passed to micras_apply_warnings. -option(MICRAS_WERROR "Treat compiler warnings as errors" OFF) - -# The -Wduplicated-* and -Wuseless-cast flags are GCC only +# The -Wduplicated-* and -Wuseless-cast flags are GCC only. -fno-common is a code generation flag: +# GCC's default for C and a no-op for C++, kept so that the robot's image stays the same. set(MICRAS_WARNING_OPTIONS -Wall -Wextra @@ -15,8 +15,8 @@ set(MICRAS_WARNING_OPTIONS -Wcast-align -Wcast-qual -Wdouble-promotion - -Wduplicated-branches - -Wduplicated-cond + $<$:-Wduplicated-branches> + $<$:-Wduplicated-cond> -Wfloat-conversion -Wformat=2 -Wimplicit-fallthrough @@ -26,19 +26,32 @@ set(MICRAS_WARNING_OPTIONS -Wundef -Wunused-parameter -fno-common - $<$:-Werror> # C++ only, they make the C compiler warn about the command line itself $<$:-Wnon-virtual-dtor> $<$:-Wold-style-cast> $<$:-Woverloaded-virtual> - $<$:-Wuseless-cast> + $<$,$>:-Wuseless-cast> ) +# Kept globally as well, so that the function works in any directory, whichever of the three +# repositories included this file first +set_property(GLOBAL PROPERTY MICRAS_WARNING_OPTIONS ${MICRAS_WARNING_OPTIONS}) + +# micras_apply_warnings(... [WERROR ]) +# # Applied per target, so that the generated sources are never compiled with them, and as options -# rather than through a target, which a static library would carry in its interface +# rather than through a target, which a static library would carry in its interface. The targets +# are real names, not aliases. function(micras_apply_warnings) - foreach(TARGET_NAME ${ARGN}) - target_compile_options(${TARGET_NAME} PRIVATE ${MICRAS_WARNING_OPTIONS}) + cmake_parse_arguments(PARSE_ARGV 0 MICRAS_WARNINGS "" "WERROR" "") + get_property(WARNING_OPTIONS GLOBAL PROPERTY MICRAS_WARNING_OPTIONS) + + if(MICRAS_WARNINGS_WERROR) + list(APPEND WARNING_OPTIONS -Werror) + endif() + + foreach(TARGET_NAME ${MICRAS_WARNINGS_UNPARSED_ARGUMENTS}) + target_compile_options(${TARGET_NAME} PRIVATE ${WARNING_OPTIONS}) endforeach() endfunction() diff --git a/cmake/templates/run_clang_tidy.sh.in b/cmake/templates/run_clang_tidy.sh.in index 49e77b8..8de0cd4 100755 --- a/cmake/templates/run_clang_tidy.sh.in +++ b/cmake/templates/run_clang_tidy.sh.in @@ -2,11 +2,30 @@ # Positional arguments are the source files to check; they are passed to run-clang-tidy as # path regexes matched against the entries of the compilation database. +# +# The canonical copy lives in micras-lib; micras-simulation and MicrasFirmware carry the same file +# byte for byte. Each repository configures it with the clang tools it found by their versioned +# names and with the extra arguments its compiler needs (the ARM sysroot on the robot, none on the +# host), one argument per line in MICRAS_TIDY_EXTRA_ARGS. -RUN_FLAGS=() +CLANG_TIDY="@MICRAS_CLANG_TIDY@" +RUN_CLANG_TIDY="@MICRAS_RUN_CLANG_TIDY@" +CLANG_APPLY_REPLACEMENTS="@MICRAS_CLANG_APPLY_REPLACEMENTS@" + +mapfile -t EXTRA_ARGS <<'EXTRA_ARGS_END' +@MICRAS_TIDY_EXTRA_ARGS@ +EXTRA_ARGS_END + +RUN_FLAGS=(-clang-tidy-binary "${CLANG_TIDY}") + +for ARG in "${EXTRA_ARGS[@]}"; do + if [[ -n "${ARG}" ]]; then + RUN_FLAGS+=("-extra-arg-before=${ARG}") + fi +done if [[ "${1:-}" == "--fix" ]]; then - RUN_FLAGS+=(-fix -format) + RUN_FLAGS+=(-fix -format -clang-apply-replacements-binary "${CLANG_APPLY_REPLACEMENTS}") shift fi @@ -16,27 +35,24 @@ fi # Unknown check names are silently ignored during a normal run, so the configuration is # validated explicitly to keep it from rotting across clang-tidy versions. -clang-tidy --verify-config || exit 1 +"${CLANG_TIDY}" --verify-config || exit 1 echo "Running clang-tidy on all source files..." # run-clang-tidy bounds the number of concurrent clang-tidy processes, unlike a plain # background job per file, and serializes the fixes it applies to shared headers. # The filtered lines are the echoed clang-tidy invocations and the per-file diagnostic -# counters, which include the warnings suppressed in the Cube headers and are always noise. +# counters, which include the warnings suppressed in the vendor headers and are always noise. # Real diagnostics name their own file, so nothing useful is hidden. -# The shape of every turn is computed when the firmware is compiled, which takes more evaluation -# steps than clang allows a constant expression by default. -run-clang-tidy \ +# The turn tables are designed as a constant expression wherever a program compiles them, which +# takes more evaluation steps than clang allows by default. +"${RUN_CLANG_TIDY}" \ "${RUN_FLAGS[@]}" \ -quiet \ -j "$(nproc)" \ -warnings-as-errors="*" \ - -p @CMAKE_CURRENT_BINARY_DIR@ \ + -p "@CMAKE_BINARY_DIR@" \ -extra-arg-before=-Wno-unknown-warning-option \ - -extra-arg-before=--sysroot=@SYSROOT@/ \ - -extra-arg-before=-I@CXX_INCLUDE_DIR@/ \ - -extra-arg-before=-I@CXX_TRIPLE_INCLUDE_DIR@/ \ -extra-arg=-fconstexpr-steps=1000000000 \ "$@" 2>&1 | grep -Ev '^(\[ *[0-9]+/[0-9]+\]|[0-9]+ (warning|error)s? generated\.|clang-tidy(-[0-9.]+)? )' diff --git a/tests/.clang-tidy b/tests/.clang-tidy new file mode 100644 index 0000000..ce6b632 --- /dev/null +++ b/tests/.clang-tidy @@ -0,0 +1,7 @@ +--- +# The test macros expand into branches that count against the cognitive complexity of every test +# case, whatever the test itself does +InheritParentConfig: true +Checks: + -readability-function-cognitive-complexity, +... From cb09be0a527a71d1e9cceb7ece880ccda2df0250 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 05:38:54 -0300 Subject: [PATCH 530/561] =?UTF-8?q?=F0=9F=8F=97=EF=B8=8F=20Build=20the=20l?= =?UTF-8?q?ibraries=20from=20one=20root=20project?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit micras-lib is a CMake project of its own and a subproject of any consumer: a second add of it is a no-op, the language settings apply only when it is the top level, and options select the libraries, the HAL backend, the board target and the stm32 family. The HAL checks that the board target exists before it links it, reads the family from its device macro, and builds the stm32 or the host sources. The proxy fetches ST's driver pinned by the commit of v5.1.1 and links the core itself; the HAL no longer does. The chip models build as micras_proxy_models on the host backend. Every library takes C++23 without extensions and the project warnings, and the top level gets the host presets, the versioned format and lint targets, and the docs target. --- .gitignore | 2 + CMakeLists.txt | 143 +++++++++++++++++++++++++++++ CMakePresets.json | 67 ++++++++++++++ cmake/micras_docs.cmake | 31 +++++++ cmake/micras_lint.cmake | 88 ++++++++++++++++++ micras_comm/CMakeLists.txt | 16 ++-- micras_core/CMakeLists.txt | 17 ++-- micras_hal/CMakeLists.txt | 72 ++++++++++++--- micras_hal/stm32/h7/README.md | 6 ++ micras_nav/CMakeLists.txt | 24 +++-- micras_proxy/CMakeLists.txt | 44 ++++++--- micras_proxy/models/CMakeLists.txt | 40 +++----- 12 files changed, 465 insertions(+), 85 deletions(-) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 CMakePresets.json create mode 100644 cmake/micras_docs.cmake create mode 100644 cmake/micras_lint.cmake create mode 100644 micras_hal/stm32/h7/README.md diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..58b2223 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +build/ +docs/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..30da05f --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,143 @@ +cmake_minimum_required(VERSION 3.28...4.4) + +############################################################################### +## Reference projects +############################################################################### + +# The reference presets build one of the projects under reference/, which adds micras-lib back the +# way any consumer does. They are selected here because a preset always configures this directory. +if(MICRAS_LIB_REFERENCE AND NOT MICRAS_LIB_REFERENCE_ADDED) + set(MICRAS_LIB_REFERENCE_ADDED TRUE) + add_subdirectory(reference/${MICRAS_LIB_REFERENCE}) + return() +endif() + +############################################################################### +## Project +############################################################################### + +# A project that reaches micras-lib through two submodule paths uses the one its top level adds +# first; every later add_subdirectory is a no-op +get_property(MICRAS_LIB_ALREADY_ADDED GLOBAL PROPERTY MICRAS_LIB_ADDED) + +if(MICRAS_LIB_ALREADY_ADDED) + return() +endif() + +set_property(GLOBAL PROPERTY MICRAS_LIB_ADDED TRUE) + +project(micras_lib LANGUAGES C CXX) + +# As a subproject, the consumer's language settings and flags reach the libraries by inheritance +if(micras_lib_IS_TOP_LEVEL) + set(CMAKE_C_STANDARD 17) + set(CMAKE_C_STANDARD_REQUIRED ON) + set(CMAKE_C_EXTENSIONS ON) + + set(CMAKE_CXX_STANDARD 23) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) + + set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + + # No target declares a module, and the scan puts -fmodules-ts and a module mapper into the + # compilation database, which makes clang-tidy reject every file + set(CMAKE_CXX_SCAN_FOR_MODULES OFF) + + if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set(CMAKE_BUILD_TYPE Debug CACHE STRING "Build type" FORCE) + endif() +endif() + +############################################################################### +## Options +############################################################################### + +include(CMakeDependentOption) + +option(MICRAS_LIB_NAV "Add micras_nav" ON) +option(MICRAS_LIB_COMM "Add micras_comm" ON) +option(MICRAS_LIB_HAL "Add micras_hal" ON) +cmake_dependent_option(MICRAS_LIB_PROXY "Add micras_proxy, which fetches lsm6dsv-pid" ON "MICRAS_LIB_HAL" OFF) + +set(MICRAS_LIB_HAL_BACKEND "stm32" CACHE STRING "Backend of micras_hal: stm32 or host") +set_property(CACHE MICRAS_LIB_HAL_BACKEND PROPERTY STRINGS stm32 host) + +set(MICRAS_LIB_STM32_FAMILY "" CACHE STRING "STM32 family of the stm32 backend, detected from the board target when empty") +set_property(CACHE MICRAS_LIB_STM32_FAMILY PROPERTY STRINGS "" h7) + +set(MICRAS_LIB_CUBE_TARGET "stm32cubemx" CACHE STRING "Board target micras_hal links, on every backend") + +option(MICRAS_LIB_WARNINGS "Apply the project warnings to every library target" ON) +option(MICRAS_LIB_WERROR "Treat the warnings of the library targets as errors" OFF) + +option(MICRAS_LIB_TESTS "Build the tests, on the host backend; fetches doctest" ${micras_lib_IS_TOP_LEVEL}) +option(MICRAS_LIB_DOCS "Add the docs target" ${micras_lib_IS_TOP_LEVEL}) + +if(NOT MICRAS_LIB_HAL_BACKEND MATCHES "^(stm32|host)$") + message(FATAL_ERROR "micras-lib: MICRAS_LIB_HAL_BACKEND is '${MICRAS_LIB_HAL_BACKEND}'; it must be stm32 or host.") +endif() + +if(MICRAS_LIB_TESTS AND MICRAS_LIB_HAL AND + NOT (MICRAS_LIB_HAL_BACKEND STREQUAL "host" AND MICRAS_LIB_CUBE_TARGET STREQUAL "micras_lib_host_board")) + message(FATAL_ERROR + "micras-lib: the tests run on the host backend, over the fake Cube layer of tests/support. " + "Configure with the host preset (MICRAS_LIB_HAL_BACKEND=host, " + "MICRAS_LIB_CUBE_TARGET=micras_lib_host_board), or set MICRAS_LIB_TESTS=OFF.") +endif() + +include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/micras_warnings.cmake) + +############################################################################### +## Libraries +############################################################################### + +# Every library requires C++23 without extensions, whatever the consumer builds with +function(micras_lib_setup_library TARGET_NAME) + target_compile_features(${TARGET_NAME} PUBLIC cxx_std_23) + set_target_properties(${TARGET_NAME} PROPERTIES CXX_EXTENSIONS OFF) + + if(MICRAS_LIB_WARNINGS) + micras_apply_warnings(${TARGET_NAME} WERROR ${MICRAS_LIB_WERROR}) + endif() +endfunction() + +add_subdirectory(micras_core) + +# The tests' board target, defined before the HAL as any consumer's +if(MICRAS_LIB_TESTS AND MICRAS_LIB_HAL) + add_subdirectory(tests/support/host_board) +endif() + +if(MICRAS_LIB_HAL) + add_subdirectory(micras_hal) +endif() + +if(MICRAS_LIB_PROXY) + add_subdirectory(micras_proxy) +endif() + +if(MICRAS_LIB_NAV) + add_subdirectory(micras_nav) +endif() + +if(MICRAS_LIB_COMM) + add_subdirectory(micras_comm) +endif() + +############################################################################### +## Tests, style and documentation +############################################################################### + +if(MICRAS_LIB_TESTS) + enable_testing() + add_subdirectory(tests) +endif() + +if(micras_lib_IS_TOP_LEVEL) + include(cmake/micras_lint.cmake) +endif() + +if(MICRAS_LIB_DOCS) + include(cmake/micras_docs.cmake) +endif() diff --git a/CMakePresets.json b/CMakePresets.json new file mode 100644 index 0000000..ff9d9c5 --- /dev/null +++ b/CMakePresets.json @@ -0,0 +1,67 @@ +{ + "version": 6, + "cmakeMinimumRequired": { + "major": 3, + "minor": 28, + "patch": 0 + }, + "configurePresets": [ + { + "name": "host-base", + "hidden": true, + "generator": "Ninja", + "binaryDir": "${sourceDir}/build/${presetName}", + "cacheVariables": { + "CMAKE_C_COMPILER": "gcc-15", + "CMAKE_CXX_COMPILER": "g++-15", + "MICRAS_LIB_HAL_BACKEND": "host", + "MICRAS_LIB_CUBE_TARGET": "micras_lib_host_board" + } + }, + { + "name": "host", + "displayName": "Host, Debug with the address and undefined behavior sanitizers", + "inherits": "host-base", + "cacheVariables": { + "CMAKE_BUILD_TYPE": "Debug", + "CMAKE_C_FLAGS": "-fsanitize=address,undefined -fno-sanitize-recover=all -fno-omit-frame-pointer", + "CMAKE_CXX_FLAGS": "-fsanitize=address,undefined -fno-sanitize-recover=all -fno-omit-frame-pointer", + "CMAKE_EXE_LINKER_FLAGS": "-fsanitize=address,undefined" + } + }, + { + "name": "host-release", + "displayName": "Host, Release", + "inherits": "host-base", + "cacheVariables": { + "CMAKE_BUILD_TYPE": "Release" + } + } + ], + "buildPresets": [ + { + "name": "host", + "configurePreset": "host" + }, + { + "name": "host-release", + "configurePreset": "host-release" + } + ], + "testPresets": [ + { + "name": "host", + "configurePreset": "host", + "output": { + "outputOnFailure": true + } + }, + { + "name": "host-release", + "configurePreset": "host-release", + "output": { + "outputOnFailure": true + } + } + ] +} diff --git a/cmake/micras_docs.cmake b/cmake/micras_docs.cmake new file mode 100644 index 0000000..bc2616d --- /dev/null +++ b/cmake/micras_docs.cmake @@ -0,0 +1,31 @@ +############################################################################### +## Documentation +############################################################################### + +find_program(MICRAS_DOXYGEN doxygen) + +if(NOT MICRAS_DOXYGEN) + message(FATAL_ERROR "micras-lib: doxygen was not found. Install it, or set MICRAS_LIB_DOCS=OFF.") +endif() + +include(FetchContent) + +# The theme the Doxyfile names through $(DOXYGEN_AWESOME_DIR), pinned by the commit of v2.5.0 +FetchContent_Declare( + doxygen-awesome-css + GIT_REPOSITORY https://github.com/jothepro/doxygen-awesome-css.git + GIT_TAG 46483f1e5a70ffb9ecd3b82d0a1cd1b24edf13da +) + +FetchContent_MakeAvailable(doxygen-awesome-css) + +# MICRAS_LIB_VERSION is resolved by Doxygen through $(MICRAS_LIB_VERSION) in the Doxyfile. +# Backticks are used instead of $() so that the make generator does not expand it first. +add_custom_target(docs + COMMAND MICRAS_LIB_VERSION=`git describe --always --dirty --tags 2>/dev/null || echo unknown` + DOXYGEN_AWESOME_DIR=${doxygen-awesome-css_SOURCE_DIR} + MICRAS_LIB_DOCS_DIR=${CMAKE_BINARY_DIR}/docs + ${MICRAS_DOXYGEN} Doxyfile + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + COMMENT "Generating the HTML documentation in ${CMAKE_BINARY_DIR}/docs" +) diff --git a/cmake/micras_lint.cmake b/cmake/micras_lint.cmake new file mode 100644 index 0000000..73bc67a --- /dev/null +++ b/cmake/micras_lint.cmake @@ -0,0 +1,88 @@ +############################################################################### +## Formatting and linting +############################################################################### + +# The clang tools are found by their versioned names, so that a distribution update or another +# version first on the PATH can move neither the formatting rules nor the enabled check set +set(MICRAS_CLANG_VERSION 22) + +foreach(TOOL clang-format clang-tidy run-clang-tidy clang-apply-replacements) + string(TOUPPER ${TOOL} TOOL_VARIABLE) + string(REPLACE "-" "_" TOOL_VARIABLE ${TOOL_VARIABLE}) + + find_program(MICRAS_${TOOL_VARIABLE} ${TOOL}-${MICRAS_CLANG_VERSION}) + + if(NOT MICRAS_${TOOL_VARIABLE}) + message(FATAL_ERROR + "micras-lib: ${TOOL}-${MICRAS_CLANG_VERSION} was not found. The format and lint targets need " + "clang ${MICRAS_CLANG_VERSION} (apt install clang-format-${MICRAS_CLANG_VERSION} " + "clang-tidy-${MICRAS_CLANG_VERSION}), found by that name.") + endif() +endforeach() + +# micras_add_format_targets(...) +# +# format rewrites the files, format_check fails on the first one that is not formatted +function(micras_add_format_targets) + add_custom_target(format + COMMAND ${MICRAS_CLANG_FORMAT} -style=file -i ${ARGN} --verbose + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + + add_custom_target(format_check + COMMAND ${MICRAS_CLANG_FORMAT} -style=file --dry-run --Werror ${ARGN} + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) +endfunction() + +# micras_add_lint_targets(SOURCES ... [EXTRA_ARGS ...]) +# +# lint runs clang-tidy on the sources of the compilation database, with the extra arguments the +# compiler needs, and lint_fix applies the fixes it can +function(micras_add_lint_targets) + cmake_parse_arguments(PARSE_ARGV 0 LINT "" "" "SOURCES;EXTRA_ARGS") + + list(JOIN LINT_EXTRA_ARGS "\n" MICRAS_TIDY_EXTRA_ARGS) + set(MICRAS_CLANG_TIDY ${MICRAS_CLANG_TIDY}) + set(MICRAS_RUN_CLANG_TIDY ${MICRAS_RUN_CLANG_TIDY}) + set(MICRAS_CLANG_APPLY_REPLACEMENTS ${MICRAS_CLANG_APPLY_REPLACEMENTS}) + + set(SCRIPT_PATH ${CMAKE_BINARY_DIR}/run_clang_tidy.sh) + configure_file(${CMAKE_CURRENT_FUNCTION_LIST_DIR}/templates/run_clang_tidy.sh.in ${SCRIPT_PATH} @ONLY) + + add_custom_target(lint + COMMAND ${SCRIPT_PATH} ${LINT_SOURCES} + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + + add_custom_target(lint_fix + COMMAND ${SCRIPT_PATH} --fix ${LINT_SOURCES} + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) +endfunction() + +# The sources of the libraries, their tests and the bench programs, never the generated or fetched ones +get_filename_component(MICRAS_LIB_ROOT ${CMAKE_CURRENT_LIST_DIR}/.. ABSOLUTE) + +file(GLOB_RECURSE MICRAS_LIB_FORMAT_FILES CONFIGURE_DEPENDS + ${MICRAS_LIB_ROOT}/micras_*/*.cpp + ${MICRAS_LIB_ROOT}/micras_*/*.hpp + ${MICRAS_LIB_ROOT}/micras_*/*.tpp + ${MICRAS_LIB_ROOT}/micras_*/*.h + ${MICRAS_LIB_ROOT}/tests/*.cpp + ${MICRAS_LIB_ROOT}/tests/*.hpp + ${MICRAS_LIB_ROOT}/tests/*.h +) + +file(GLOB MICRAS_LIB_BENCH_FILES CONFIGURE_DEPENDS ${MICRAS_LIB_ROOT}/reference/*/bench.cpp) +list(APPEND MICRAS_LIB_FORMAT_FILES ${MICRAS_LIB_BENCH_FILES}) + +if(micras_lib_IS_TOP_LEVEL) + file(GLOB_RECURSE MICRAS_LIB_LINT_SOURCES CONFIGURE_DEPENDS + ${MICRAS_LIB_ROOT}/micras_*/*.cpp + ${MICRAS_LIB_ROOT}/tests/*.cpp + ) + + micras_add_format_targets(${MICRAS_LIB_FORMAT_FILES}) + micras_add_lint_targets(SOURCES ${MICRAS_LIB_LINT_SOURCES}) +endif() diff --git a/micras_comm/CMakeLists.txt b/micras_comm/CMakeLists.txt index ad0bdc6..4ad6c92 100644 --- a/micras_comm/CMakeLists.txt +++ b/micras_comm/CMakeLists.txt @@ -1,19 +1,19 @@ -project(micras_comm) +file(GLOB_RECURSE MICRAS_COMM_SOURCES CONFIGURE_DEPENDS "src/*.c*") -file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") - -add_library(${PROJECT_NAME} STATIC - ${PROJECT_SOURCES} +add_library(micras_comm STATIC + ${MICRAS_COMM_SOURCES} ) -add_library(micras::comm ALIAS ${PROJECT_NAME}) +add_library(micras::comm ALIAS micras_comm) -target_include_directories(${PROJECT_NAME} PUBLIC +target_include_directories(micras_comm PUBLIC include ) # Only the reflection and the framing, so that the whole session layer can be built and tested # without a single peripheral behind it -target_link_libraries(${PROJECT_NAME} PUBLIC +target_link_libraries(micras_comm PUBLIC micras::core ) + +micras_lib_setup_library(micras_comm) diff --git a/micras_core/CMakeLists.txt b/micras_core/CMakeLists.txt index 1b10633..09741c9 100644 --- a/micras_core/CMakeLists.txt +++ b/micras_core/CMakeLists.txt @@ -1,16 +1,13 @@ -project(micras_core) +file(GLOB_RECURSE MICRAS_CORE_SOURCES CONFIGURE_DEPENDS "src/*.c*") -file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") - -add_library(${PROJECT_NAME} STATIC - ${PROJECT_SOURCES} +add_library(micras_core STATIC + ${MICRAS_CORE_SOURCES} ) -add_library(micras::core ALIAS ${PROJECT_NAME}) - -# A requirement of the sources rather than a preference of whoever is building, so it propagates -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_23) +add_library(micras::core ALIAS micras_core) -target_include_directories(${PROJECT_NAME} PUBLIC +target_include_directories(micras_core PUBLIC include ) + +micras_lib_setup_library(micras_core) diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt index f66c13b..17fe667 100644 --- a/micras_hal/CMakeLists.txt +++ b/micras_hal/CMakeLists.txt @@ -1,22 +1,66 @@ -project(micras_hal) +# The board target carries main.h and the vendor or fake Cube layer behind it, which the headers +# include, so it has to exist before the HAL is added +if(NOT TARGET ${MICRAS_LIB_CUBE_TARGET}) + message(FATAL_ERROR + "micras-lib: the ${MICRAS_LIB_HAL_BACKEND} HAL links '${MICRAS_LIB_CUBE_TARGET}', which is not " + "defined yet. Define it (CubeMX's stm32cubemx, or a fake Cube layer for the host) before adding " + "micras-lib, or set MICRAS_LIB_CUBE_TARGET.") +endif() -file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") +add_library(micras_hal STATIC) +add_library(micras::hal ALIAS micras_hal) -add_library(${PROJECT_NAME} STATIC - ${PROJECT_SOURCES} +target_include_directories(micras_hal PUBLIC + include ) -add_library(micras::hal ALIAS ${PROJECT_NAME}) +# stm32cubemx, or the consumer's fake Cube layer on the host, carries the vendor include directories +# and the device macros the public headers name, so it propagates +target_link_libraries(micras_hal PUBLIC + ${MICRAS_LIB_CUBE_TARGET} +) -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_23) +if(MICRAS_LIB_HAL_BACKEND STREQUAL "stm32") + # The family is read from the device macro CubeMX puts on its interface target + if(MICRAS_LIB_STM32_FAMILY) + set(MICRAS_HAL_FAMILY ${MICRAS_LIB_STM32_FAMILY}) + else() + get_target_property(MICRAS_CUBE_DEFINITIONS ${MICRAS_LIB_CUBE_TARGET} INTERFACE_COMPILE_DEFINITIONS) -target_include_directories(${PROJECT_NAME} PUBLIC - include -) + if(MICRAS_CUBE_DEFINITIONS MATCHES "STM32H7") + set(MICRAS_HAL_FAMILY h7) + elseif(MICRAS_CUBE_DEFINITIONS MATCHES "STM32G4") + set(MICRAS_HAL_FAMILY g4) + else() + message(FATAL_ERROR "micras-lib: cannot tell the STM32 family from ${MICRAS_LIB_CUBE_TARGET}; " + "set MICRAS_LIB_STM32_FAMILY to h7 or g4.") + endif() + endif() + + if(NOT IS_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/stm32/${MICRAS_HAL_FAMILY}) + message(FATAL_ERROR "micras-lib: the stm32 backend has no code for the '${MICRAS_HAL_FAMILY}' family.") + endif() -# stm32cubemx is the interface target STM32CubeMX generates, carrying the CMSIS and vendor HAL -# include directories and the device macros. The public headers name them, so it propagates. -target_link_libraries(${PROJECT_NAME} PUBLIC - micras::core - stm32cubemx + file(GLOB MICRAS_HAL_SOURCES CONFIGURE_DEPENDS + stm32/src/*.cpp + stm32/${MICRAS_HAL_FAMILY}/src/*.cpp + ) + + target_include_directories(micras_hal PUBLIC + stm32/${MICRAS_HAL_FAMILY}/include + ) +else() + file(GLOB MICRAS_HAL_SOURCES CONFIGURE_DEPENDS + host/src/*.cpp + ) + + target_include_directories(micras_hal PUBLIC + host/include + ) +endif() + +target_sources(micras_hal PRIVATE + ${MICRAS_HAL_SOURCES} ) + +micras_lib_setup_library(micras_hal) diff --git a/micras_hal/stm32/h7/README.md b/micras_hal/stm32/h7/README.md new file mode 100644 index 0000000..37cb2d8 --- /dev/null +++ b/micras_hal/stm32/h7/README.md @@ -0,0 +1,6 @@ +# STM32H7 family + +Code of the stm32 backend that only the STM32H7 family runs, which CMake compiles when the board +target defines an STM32H7 device (or `MICRAS_LIB_STM32_FAMILY` is `h7`): `src/` holds the family's +sources and `include/micras/hal/family/` the facts its shared headers read. The code in `../src` is +the same on every family and names none. diff --git a/micras_nav/CMakeLists.txt b/micras_nav/CMakeLists.txt index ff12eb6..b6e6f5f 100644 --- a/micras_nav/CMakeLists.txt +++ b/micras_nav/CMakeLists.txt @@ -1,21 +1,25 @@ -project(micras_nav) +file(GLOB_RECURSE MICRAS_NAV_SOURCES CONFIGURE_DEPENDS "src/*.c*") -file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") - -add_library(${PROJECT_NAME} STATIC - ${PROJECT_SOURCES} +add_library(micras_nav STATIC + ${MICRAS_NAV_SOURCES} ) -add_library(micras::nav ALIAS ${PROJECT_NAME}) - -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_23) +add_library(micras::nav ALIAS micras_nav) -target_include_directories(${PROJECT_NAME} PUBLIC +target_include_directories(micras_nav PUBLIC include ) # The navigation takes plain data and gives plain data back, so it links nothing that can reach the # hardware and builds anywhere micras::core does -target_link_libraries(${PROJECT_NAME} PUBLIC +target_link_libraries(micras_nav PUBLIC micras::core ) + +# A program that designs the turn tables as a constant expression needs more evaluation steps than +# clang allows by default +target_compile_options(micras_nav INTERFACE + $<$:-fconstexpr-steps=1000000000> +) + +micras_lib_setup_library(micras_nav) diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt index 504bb31..bc9254a 100644 --- a/micras_proxy/CMakeLists.txt +++ b/micras_proxy/CMakeLists.txt @@ -1,38 +1,52 @@ -project(micras_proxy) - -file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") +file(GLOB_RECURSE MICRAS_PROXY_SOURCES CONFIGURE_DEPENDS "src/*.c*") +# The one declaration of ST's register driver, pinned by the commit of v5.1.1, so that every project +# in a build shares one copy include(FetchContent) FetchContent_Declare( lsm6dsv-pid GIT_REPOSITORY https://github.com/STMicroelectronics/lsm6dsv-pid.git - GIT_TAG v5.1.1 + GIT_TAG 6ede05a3fde7eae80bc1b47affc08257191c1aba ) FetchContent_MakeAvailable(lsm6dsv-pid) -file(GLOB_RECURSE LIB_SOURCES CONFIGURE_DEPENDS "${lsm6dsv-pid_SOURCE_DIR}/*.c") +file(GLOB_RECURSE LSM6DSV_SOURCES CONFIGURE_DEPENDS "${lsm6dsv-pid_SOURCE_DIR}/*.c") -# Remove warnings from libraries sources +# The vendor sources are not ours to warn about. Source properties belong to the directory that sets +# them, so this stays next to the target that compiles the sources. set_source_files_properties( - ${LIB_SOURCES} + ${LSM6DSV_SOURCES} PROPERTIES COMPILE_FLAGS "-w" ) -add_library(${PROJECT_NAME} STATIC - ${PROJECT_SOURCES} - ${LIB_SOURCES} +add_library(micras_proxy STATIC + ${MICRAS_PROXY_SOURCES} + ${LSM6DSV_SOURCES} ) -add_library(micras::proxy ALIAS ${PROJECT_NAME}) - -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_23) +add_library(micras::proxy ALIAS micras_proxy) -target_include_directories(${PROJECT_NAME} PUBLIC +target_include_directories(micras_proxy PUBLIC include ${lsm6dsv-pid_SOURCE_DIR} ) -target_link_libraries(${PROJECT_NAME} PUBLIC +target_link_libraries(micras_proxy PUBLIC micras::hal + micras::core +) + +# The vendor driver is C17 with extensions, whatever the consumer compiles its own C with +set_target_properties(micras_proxy PROPERTIES + C_STANDARD 17 + C_STANDARD_REQUIRED ON + C_EXTENSIONS ON ) + +micras_lib_setup_library(micras_proxy) + +# The chip models are SPI devices of the host backend, so they exist only there +if(MICRAS_LIB_HAL_BACKEND STREQUAL "host") + add_subdirectory(models) +endif() diff --git a/micras_proxy/models/CMakeLists.txt b/micras_proxy/models/CMakeLists.txt index d37754e..cc9be43 100644 --- a/micras_proxy/models/CMakeLists.txt +++ b/micras_proxy/models/CMakeLists.txt @@ -1,34 +1,18 @@ -############################################################################### -## Chip models: SPI devices for the host micras_hal -############################################################################### - -# They know the SPI device slot of the host HAL and nothing else: no Cube layer, no -# firmware, no engine. A robot's bindings attach them to its buses and feed them. -add_library(micras_hal_host_models STATIC +# SPI devices for the host micras_hal: they know the SPI device slot of the host backend and nothing +# else. A consumer's bindings attach them to its buses and feed them. +add_library(micras_proxy_models STATIC src/as5047u_model.cpp src/lsm6dsv_model.cpp ) -add_library(micras::hal_host_models ALIAS micras_hal_host_models) -target_include_directories(micras_hal_host_models PUBLIC - "${CMAKE_CURRENT_SOURCE_DIR}/include" - "${MICRAS_SIM_HAL_HOST_INCLUDE}" -) -target_compile_features(micras_hal_host_models PUBLIC cxx_std_23) -target_compile_options(micras_hal_host_models PRIVATE -Wall -Wextra) -if(MICRAS_TESTS) - include(GoogleTest) +add_library(micras::proxy_models ALIAS micras_proxy_models) - # ST's register driver header names the registers the tests address, so a model - # register at another address than the driver's fails here. - add_executable(micras_hal_host_models_tests - tests/as5047u_model_test.cpp - tests/lsm6dsv_model_test.cpp - ) - target_link_libraries(micras_hal_host_models_tests PRIVATE - micras_hal_host_models lsm6dsv_headers GTest::gtest_main - ) - target_compile_options(micras_hal_host_models_tests PRIVATE -Wall -Wextra) +target_include_directories(micras_proxy_models PUBLIC + include +) + +target_link_libraries(micras_proxy_models PUBLIC + micras::hal +) - gtest_discover_tests(micras_hal_host_models_tests) -endif() +micras_lib_setup_library(micras_proxy_models) From 3f14d1db0d5783f40436c89c71780e9b7d42a461 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 05:39:04 -0300 Subject: [PATCH 531/561] =?UTF-8?q?=E2=9C=A8=20Run=20the=20FMAC=20and=20a?= =?UTF-8?q?=20power=20loss=20on=20the=20host=20backend?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The host Fmac keeps the configured IIR filter in a port of the accelerator and computes it in q1.15, so FmacFilter builds and runs on the host and no proxy is left out anywhere. The flash port counts the flash words writes may still program, which is how a test cuts a save off as a power loss would, and starts with every field initialized. The board's example pin is a neutral one. --- .../host/include/micras/hal/host/board.hpp | 14 ++++- .../host/include/micras/hal/host/ports.hpp | 57 +++++++++++++++++++ micras_hal/host/include/stm32_host.h | 11 +++- micras_hal/host/src/board.cpp | 16 ++++-- micras_hal/host/src/flash.cpp | 5 +- micras_hal/host/src/fmac.cpp | 41 +++++++++++++ micras_hal/host/src/ports.cpp | 46 +++++++++++++++ 7 files changed, 180 insertions(+), 10 deletions(-) create mode 100644 micras_hal/host/src/fmac.cpp diff --git a/micras_hal/host/include/micras/hal/host/board.hpp b/micras_hal/host/include/micras/hal/host/board.hpp index 9e90397..24d7479 100644 --- a/micras_hal/host/include/micras/hal/host/board.hpp +++ b/micras_hal/host/include/micras/hal/host/board.hpp @@ -24,9 +24,9 @@ namespace micras::hal::host { * * @note A port is created the first time either side asks for it, keyed by the * address of the Cube handle or GPIO port that names it, so the firmware's - * own configuration is the key: `.port = LED_Red_GPIO_Port, .pin = - * LED_Red_Pin` in target.hpp and the same two names in a robot's bindings - * reach the same port. Nothing here knows a physics engine; bindings + * own configuration is the key: `.port = Status_LED_GPIO_Port, .pin = + * Status_LED_Pin` in a board configuration and the same two names in a + * robot's bindings reach the same port. Nothing here knows a physics engine; bindings * connect ports to whatever simulates the board. * * @note Not thread safe, and it does not need to be: the firmware and whatever @@ -112,6 +112,14 @@ class Board { static void spi_device(const SPI_HandleTypeDef* spi, const GPIO_TypeDef* cs_port, uint16_t cs_pin, SpiDevice& device); + /** + * @brief Get the port of the filter math accelerator. + * + * @param fmac FMAC handle. + * @return The port. + */ + static FmacPort& fmac(const void* fmac); + /** * @brief Get the flash. * diff --git a/micras_hal/host/include/micras/hal/host/ports.hpp b/micras_hal/host/include/micras/hal/host/ports.hpp index c9ec81c..65cc03e 100644 --- a/micras_hal/host/include/micras/hal/host/ports.hpp +++ b/micras_hal/host/include/micras/hal/host/ports.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -192,6 +193,62 @@ struct FlashPort : Port { * @brief Contents, erased to 0xFF. */ std::vector bytes; + + /** + * @brief Flash words the writes may still program before a simulated power loss. + * + * @note Each flash word a write programs uses one; a write that finds none left fails there, + * with the words before it programmed, as a write the power cut off leaves the flash. + * Unlimited unless a test sets it. + */ + uint32_t write_budget{std::numeric_limits::max()}; +}; + +/** + * @brief The filter math accelerator, running the filter the firmware configured. + * + * @note The accelerator computes in q1.15 and keeps the history of its filter + * in its own memory, which is what this port holds on the host. + */ +struct FmacPort : Port { + /** + * @brief Coefficients applied to the inputs, most recent first, in q1.15. + */ + std::vector feed_forward; + + /** + * @brief Coefficients applied to the previous outputs, most recent first, in q1.15, added. + */ + std::vector feedback; + + /** + * @brief Last inputs, most recent first, as many as there are feed forward coefficients. + */ + std::deque inputs; + + /** + * @brief Last outputs, most recent first, as many as there are feedback coefficients. + */ + std::deque outputs; + + /** + * @brief Start a filter with an empty history. + * + * @param feed_forward_coefficients Coefficients applied to the inputs. + * @param feedback_coefficients Coefficients applied to the previous outputs. + */ + void configure(std::span feed_forward_coefficients, std::span feedback_coefficients); + + /** + * @brief Push one input through the filter, as a write to the accelerator's input register does. + * + * @note The products are exact and their sum is kept whole, where the accelerator's accumulator + * has 26 bits; the output is the sum shifted back to q1.15 and clipped to its range. + * + * @param sample Input, in q1.15. + * @return Output, in q1.15. + */ + int16_t filter(int16_t sample); }; /** diff --git a/micras_hal/host/include/stm32_host.h b/micras_hal/host/include/stm32_host.h index 96a1a95..d944070 100644 --- a/micras_hal/host/include/stm32_host.h +++ b/micras_hal/host/include/stm32_host.h @@ -284,8 +284,17 @@ struct CRC_HandleTypeDef { * FMAC *****************************************/ +///@{ +inline constexpr uint32_t HAL_FMAC_STATE_RESET{0x00U}; +inline constexpr uint32_t HAL_FMAC_STATE_READY{0x20U}; + +///@} + /** - * @brief The filter accelerator's handle; nothing on the host uses it. + * @brief The filter accelerator's handle. + * + * @note The host backend keeps the filter itself in the accelerator's port, so + * the handle carries only the state the init function sets. */ struct FMAC_HandleTypeDef { uint32_t State; diff --git a/micras_hal/host/src/board.cpp b/micras_hal/host/src/board.cpp index f309a54..e27804f 100644 --- a/micras_hal/host/src/board.cpp +++ b/micras_hal/host/src/board.cpp @@ -4,7 +4,9 @@ #include #include +#include #include +#include #include #include #include @@ -34,10 +36,11 @@ struct Registry { std::map uarts; std::map encoders; std::map spis; + std::map fmacs; std::map gpio_names; std::map handle_names; - FlashPort flash{{.name = "flash"}}; - McuPort mcu{{.name = "mcu"}}; + FlashPort flash{{.name = "flash", .touched = false, .bound = false}, {}, std::numeric_limits::max()}; + McuPort mcu{{.name = "mcu", .touched = false, .bound = false}, 0, 0, 0, 0}; }; /** @@ -84,10 +87,9 @@ std::string gpio_name(const Key& key) { */ template typename Map::mapped_type& find_or_create(Map& ports, const typename Map::key_type& key, const Namer& name) { - auto found = ports.find(key); + const auto [found, created] = ports.try_emplace(key); - if (found == ports.end()) { - found = ports.emplace(key, typename Map::mapped_type{}).first; + if (created) { found->second.name = name(); } @@ -138,6 +140,10 @@ void Board::spi_device(const SPI_HandleTypeDef* spi, const GPIO_TypeDef* cs_port Board::gpio(cs_port, cs_pin).bound = true; } +FmacPort& Board::fmac(const void* fmac) { + return find_or_create(registry().fmacs, fmac, [fmac] { return handle_name(fmac); }); +} + FlashPort& Board::flash() { return registry().flash; } diff --git a/micras_hal/host/src/flash.cpp b/micras_hal/host/src/flash.cpp index 302cf50..26422e5 100644 --- a/micras_hal/host/src/flash.cpp +++ b/micras_hal/host/src/flash.cpp @@ -96,10 +96,13 @@ Flash::Status Flash::write(uint32_t address, std::span data) { const FlashWord word{data.subspan(offset)}; const std::span target = memory.subspan(address + offset, FlashWord::size); - if (not std::ranges::all_of(target, [](uint8_t byte) { return byte == FlashWord::erased_value; })) { + if (port.write_budget == 0 or + not std::ranges::all_of(target, [](uint8_t byte) { return byte == FlashWord::erased_value; })) { return Status::ERROR; } + port.write_budget--; + const std::span bytes{std::bit_cast(word.data()), FlashWord::size}; std::ranges::copy(bytes, target.begin()); } diff --git a/micras_hal/host/src/fmac.cpp b/micras_hal/host/src/fmac.cpp new file mode 100644 index 0000000..90679fc --- /dev/null +++ b/micras_hal/host/src/fmac.cpp @@ -0,0 +1,41 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/fmac.hpp" +#include "micras/hal/host/board.hpp" + +namespace micras::hal { +Fmac::Fmac(const Config& config) : handle{config.handle} { + if (this->handle->State == HAL_FMAC_STATE_RESET) { + config.init_function(); + } +} + +bool Fmac::configure_iir(std::span feed_forward, std::span feedback) { + this->initialized = false; + + if (feed_forward.empty() or feedback.empty() or feed_forward.size() + feedback.size() > buffer_size or + this->handle->State != HAL_FMAC_STATE_READY) { + return false; + } + + host::FmacPort& port = host::Board::fmac(this->handle); + port.touched = true; + port.configure(feed_forward, feedback); + + this->initialized = true; + return true; +} + +int16_t Fmac::update(int16_t sample) { + return host::Board::fmac(this->handle).filter(sample); +} + +bool Fmac::was_initialized() const { + return this->initialized; +} +} // namespace micras::hal diff --git a/micras_hal/host/src/ports.cpp b/micras_hal/host/src/ports.cpp index 3483ab3..53260ea 100644 --- a/micras_hal/host/src/ports.cpp +++ b/micras_hal/host/src/ports.cpp @@ -2,6 +2,12 @@ * @file */ +#include +#include +#include +#include +#include + #include "micras/hal/host/ports.hpp" namespace micras::hal::host { @@ -27,4 +33,44 @@ void UartPort::receive(uint8_t byte) { this->rx_buffer[this->rx_head] = byte; this->rx_head = (this->rx_head + 1) % this->rx_buffer.size(); } + +void FmacPort::configure( + std::span feed_forward_coefficients, std::span feedback_coefficients +) { + this->feed_forward.assign(feed_forward_coefficients.begin(), feed_forward_coefficients.end()); + this->feedback.assign(feedback_coefficients.begin(), feedback_coefficients.end()); + this->inputs.assign(this->feed_forward.size(), 0); + this->outputs.assign(this->feedback.size(), 0); +} + +int16_t FmacPort::filter(int16_t sample) { + if (this->feed_forward.empty()) { + return 0; + } + + this->inputs.pop_back(); + this->inputs.push_front(sample); + + int64_t accumulator = 0; + + for (std::size_t tap = 0; tap < this->feed_forward.size(); tap++) { + accumulator += static_cast(this->feed_forward.at(tap)) * this->inputs.at(tap); + } + + for (std::size_t tap = 0; tap < this->feedback.size(); tap++) { + accumulator += static_cast(this->feedback.at(tap)) * this->outputs.at(tap); + } + + constexpr int64_t fractional_bits{15}; + const auto output = static_cast(std::clamp( + accumulator >> fractional_bits, std::numeric_limits::min(), std::numeric_limits::max() + )); + + if (not this->outputs.empty()) { + this->outputs.pop_back(); + this->outputs.push_front(output); + } + + return output; +} } // namespace micras::hal::host From 9c9c55e71807c7508d18fb0eb7b34ec9ce04c426 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 05:39:34 -0300 Subject: [PATCH 532/561] =?UTF-8?q?=F0=9F=9A=9A=20Move=20the=20tests=20int?= =?UTF-8?q?o=20one=20folder=20per=20suite?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- tests/{ => comm}/test_frame.cpp | 0 tests/{ => comm}/test_link.cpp | 0 tests/{ => core}/test_cobs.cpp | 0 tests/{ => core}/test_variable_pool.cpp | 0 .../as5047u_model_test.cpp => tests/models/test_as5047u_model.cpp | 0 .../lsm6dsv_model_test.cpp => tests/models/test_lsm6dsv_model.cpp | 0 tests/{ => proxy}/test_storage.cpp | 0 7 files changed, 0 insertions(+), 0 deletions(-) rename tests/{ => comm}/test_frame.cpp (100%) rename tests/{ => comm}/test_link.cpp (100%) rename tests/{ => core}/test_cobs.cpp (100%) rename tests/{ => core}/test_variable_pool.cpp (100%) rename micras_proxy/models/tests/as5047u_model_test.cpp => tests/models/test_as5047u_model.cpp (100%) rename micras_proxy/models/tests/lsm6dsv_model_test.cpp => tests/models/test_lsm6dsv_model.cpp (100%) rename tests/{ => proxy}/test_storage.cpp (100%) diff --git a/tests/test_frame.cpp b/tests/comm/test_frame.cpp similarity index 100% rename from tests/test_frame.cpp rename to tests/comm/test_frame.cpp diff --git a/tests/test_link.cpp b/tests/comm/test_link.cpp similarity index 100% rename from tests/test_link.cpp rename to tests/comm/test_link.cpp diff --git a/tests/test_cobs.cpp b/tests/core/test_cobs.cpp similarity index 100% rename from tests/test_cobs.cpp rename to tests/core/test_cobs.cpp diff --git a/tests/test_variable_pool.cpp b/tests/core/test_variable_pool.cpp similarity index 100% rename from tests/test_variable_pool.cpp rename to tests/core/test_variable_pool.cpp diff --git a/micras_proxy/models/tests/as5047u_model_test.cpp b/tests/models/test_as5047u_model.cpp similarity index 100% rename from micras_proxy/models/tests/as5047u_model_test.cpp rename to tests/models/test_as5047u_model.cpp diff --git a/micras_proxy/models/tests/lsm6dsv_model_test.cpp b/tests/models/test_lsm6dsv_model.cpp similarity index 100% rename from micras_proxy/models/tests/lsm6dsv_model_test.cpp rename to tests/models/test_lsm6dsv_model.cpp diff --git a/tests/test_storage.cpp b/tests/proxy/test_storage.cpp similarity index 100% rename from tests/test_storage.cpp rename to tests/proxy/test_storage.cpp From a69bd07d6ad863b837c24d660150f4444e727eb8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 05:39:45 -0300 Subject: [PATCH 533/561] =?UTF-8?q?=E2=9C=85=20Test=20the=20libraries=20wi?= =?UTF-8?q?th=20doctest=20over=20a=20fake=20reference=20board?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Every suite is one executable whose cases CTest discovers one by one, run on the host backend under the address and undefined behavior sanitizers of the host preset. The hal and proxy suites run over tests/support/host_board, the host counterpart of the reference projects: their peripherals under their handle and pin names, and nothing of Micras. The hal suite covers the CRC, the clock quantum and handover, the GPIO, the PWM, DMA and encoder timers, the flash, the UART ring, the ADC, the MCU, the FMAC against a direct form reference and the SPI device slot; the proxy suite the storage over the power loss injector, which replaces the flash stub, FmacFilter, and Imu and RotarySensor over the chip models; the models suite the chip models' own tests. --- micras_proxy/models/src/as5047u_model.cpp | 2 +- micras_proxy/models/src/lsm6dsv_model.cpp | 12 +- tests/CMakeLists.txt | 118 +++-- tests/comm/test_frame.cpp | 66 ++- tests/comm/test_link.cpp | 544 ++++++++++++---------- tests/core/test_cobs.cpp | 186 ++++---- tests/core/test_variable_pool.cpp | 175 ++++--- tests/hal/test_adc.cpp | 73 +++ tests/hal/test_board.cpp | 58 +++ tests/hal/test_clock.cpp | 49 ++ tests/hal/test_crc.cpp | 83 ++++ tests/hal/test_flash.cpp | 70 +++ tests/hal/test_fmac.cpp | 97 ++++ tests/hal/test_mcu.cpp | 65 +++ tests/hal/test_spi.cpp | 208 +++++++++ tests/hal/test_timers.cpp | 93 ++++ tests/hal/test_uart.cpp | 62 +++ tests/models/test_as5047u_model.cpp | 253 +++++----- tests/models/test_lsm6dsv_model.cpp | 426 ++++++++--------- tests/proxy/test_fmac_filter.cpp | 82 ++++ tests/proxy/test_imu.cpp | 152 ++++++ tests/proxy/test_rotary_sensor.cpp | 109 +++++ tests/proxy/test_storage.cpp | 158 ++++--- tests/stub/micras/hal/flash.hpp | 67 --- tests/support/host_board/CMakeLists.txt | 16 + tests/support/host_board/adc.h | 22 + tests/support/host_board/crc.h | 22 + tests/support/host_board/dma.h | 17 + tests/support/host_board/fmac.h | 22 + tests/support/host_board/gpio.h | 17 + tests/support/host_board/handles.cpp | 137 ++++++ tests/support/host_board/main.h | 62 +++ tests/support/host_board/spi.h | 22 + tests/support/host_board/tim.h | 52 +++ tests/support/host_board/usart.h | 22 + tests/support/host_fixture.hpp | 186 ++++++++ tests/support/main.cpp | 6 + tests/test_host.hpp | 25 - 38 files changed, 2846 insertions(+), 990 deletions(-) create mode 100644 tests/hal/test_adc.cpp create mode 100644 tests/hal/test_board.cpp create mode 100644 tests/hal/test_clock.cpp create mode 100644 tests/hal/test_crc.cpp create mode 100644 tests/hal/test_flash.cpp create mode 100644 tests/hal/test_fmac.cpp create mode 100644 tests/hal/test_mcu.cpp create mode 100644 tests/hal/test_spi.cpp create mode 100644 tests/hal/test_timers.cpp create mode 100644 tests/hal/test_uart.cpp create mode 100644 tests/proxy/test_fmac_filter.cpp create mode 100644 tests/proxy/test_imu.cpp create mode 100644 tests/proxy/test_rotary_sensor.cpp delete mode 100644 tests/stub/micras/hal/flash.hpp create mode 100644 tests/support/host_board/CMakeLists.txt create mode 100644 tests/support/host_board/adc.h create mode 100644 tests/support/host_board/crc.h create mode 100644 tests/support/host_board/dma.h create mode 100644 tests/support/host_board/fmac.h create mode 100644 tests/support/host_board/gpio.h create mode 100644 tests/support/host_board/handles.cpp create mode 100644 tests/support/host_board/main.h create mode 100644 tests/support/host_board/spi.h create mode 100644 tests/support/host_board/tim.h create mode 100644 tests/support/host_board/usart.h create mode 100644 tests/support/host_fixture.hpp create mode 100644 tests/support/main.cpp delete mode 100644 tests/test_host.hpp diff --git a/micras_proxy/models/src/as5047u_model.cpp b/micras_proxy/models/src/as5047u_model.cpp index 3f649e1..9903a07 100644 --- a/micras_proxy/models/src/as5047u_model.cpp +++ b/micras_proxy/models/src/as5047u_model.cpp @@ -122,7 +122,7 @@ uint8_t As5047uModel::crc(uint8_t high, uint8_t low) { void As5047uModel::process(const std::array& frame) { const uint32_t raw = static_cast(std::get<0>(frame)) << 16U | static_cast(std::get<1>(frame)) << 8U | std::get<2>(frame); - const auto field = static_cast((raw >> field_shift) & field_mask); + const auto field = static_cast((raw >> field_shift) & field_mask); if (crc(std::get<0>(frame), std::get<1>(frame)) != std::get<2>(frame)) { this->errfl |= crc_error; diff --git a/micras_proxy/models/src/lsm6dsv_model.cpp b/micras_proxy/models/src/lsm6dsv_model.cpp index f077bc4..cd5c289 100644 --- a/micras_proxy/models/src/lsm6dsv_model.cpp +++ b/micras_proxy/models/src/lsm6dsv_model.cpp @@ -220,9 +220,9 @@ uint8_t Lsm6dsvModel::read(uint8_t address) { const uint8_t value = this->registers.at(address); if (address >= outx_l_g and address < outx_l_a and (address & 1) != 0) { - this->registers.at(status_reg) &= ~gda; + this->registers.at(status_reg) &= static_cast(~gda); } else if (address >= outx_l_a and address < outx_l_a + 6 and (address & 1) != 0) { - this->registers.at(status_reg) &= ~xlda; + this->registers.at(status_reg) &= static_cast(~xlda); } return value; @@ -239,7 +239,7 @@ void Lsm6dsvModel::write(uint8_t address, uint8_t value) { return; } - this->registers.at(address) = address == ctrl3 ? value & ~boot : value; + this->registers.at(address) = address == ctrl3 ? static_cast(value & ~boot) : value; } bool Lsm6dsvModel::main_page() const { @@ -248,9 +248,9 @@ bool Lsm6dsvModel::main_page() const { void Lsm6dsvModel::encode(uint8_t first, const std::array& values, double sensitivity) { for (std::size_t axis = 0; axis < values.size(); axis++) { - const double counts = sensitivity > 0.0 ? std::round(values.at(axis) / sensitivity) : 0.0; - const auto word = static_cast(static_cast(std::clamp(counts, -32768.0, 32767.0))); - const auto low = static_cast(first + (2 * axis)); + const double counts = sensitivity > 0.0 ? std::round(values.at(axis) / sensitivity) : 0.0; + const auto word = static_cast(static_cast(std::clamp(counts, -32768.0, 32767.0))); + const std::size_t low = first + (2 * axis); this->registers.at(low) = static_cast(word); this->registers.at(low + 1) = static_cast(word >> 8U); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 8023f50..dcd6c1e 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,60 +1,86 @@ -# Tests for the parts that have no hardware behind them: the variable pool, the flash image, the -# framing and the whole session layer. They build with the host compiler and run in the terminal, -# which is what makes them worth writing at all, and is configured separately from the firmware -# because it is a different toolchain: -# -# cmake -S tests/host -B build/host && cmake --build build/host && ctest --test-dir build/host +############################################################################### +## Tests, on the host backend +############################################################################### -cmake_minimum_required(VERSION 3.25...4.4) +include(FetchContent) -project(micras_host_tests CXX) +# Pinned by the commit of v2.5.3. SYSTEM keeps the project warnings out of its header. +FetchContent_Declare( + doctest + GIT_REPOSITORY https://github.com/doctest/doctest.git + GIT_TAG 2d0a9359a60c51affe2a9bebb1be1dca47868151 + SYSTEM +) -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) +set(DOCTEST_NO_INSTALL ON) +set(DOCTEST_WITH_TESTS OFF) +FetchContent_MakeAvailable(doctest) -set(MICRAS_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/../..) +include(${doctest_SOURCE_DIR}/scripts/cmake/doctest.cmake) -enable_testing() +############################################################################### +## Support +############################################################################### -add_library(micras_host_core STATIC - ${MICRAS_ROOT}/micras_core/src/cobs.cpp - ${MICRAS_ROOT}/micras_core/src/variable_pool.cpp +add_library(micras_lib_test_main STATIC + support/main.cpp ) -target_include_directories(micras_host_core PUBLIC ${MICRAS_ROOT}/micras_core/include) +target_link_libraries(micras_lib_test_main PUBLIC + doctest::doctest +) -add_library(micras_host_comm STATIC - ${MICRAS_ROOT}/micras_comm/src/frame.cpp - ${MICRAS_ROOT}/micras_comm/src/link.cpp +target_include_directories(micras_lib_test_main PUBLIC + support ) -target_include_directories(micras_host_comm PUBLIC ${MICRAS_ROOT}/micras_comm/include) -target_link_libraries(micras_host_comm PUBLIC micras_host_core) +micras_apply_warnings(micras_lib_test_main WERROR ${MICRAS_LIB_WERROR}) -# The flash of the storage is stood in for by a buffer that can be told to stop accepting writes, -# which is the only way to test what an interrupted save leaves behind -add_library(micras_host_proxy STATIC - ${MICRAS_ROOT}/micras_proxy/src/storage.cpp -) +# micras_lib_add_suite( ...) +# +# One executable per folder of tests, whose test cases CTest discovers one by one +function(micras_lib_add_suite SUITE) + file(GLOB SUITE_SOURCES CONFIGURE_DEPENDS ${SUITE}/*.cpp) -target_include_directories(micras_host_proxy PUBLIC - ${MICRAS_ROOT}/micras_proxy/include - ${CMAKE_CURRENT_SOURCE_DIR}/stub -) + add_executable(micras_lib_${SUITE}_tests + ${SUITE_SOURCES} + ) + + target_link_libraries(micras_lib_${SUITE}_tests PRIVATE + micras_lib_test_main + ${ARGN} + ) + + micras_apply_warnings(micras_lib_${SUITE}_tests WERROR ${MICRAS_LIB_WERROR}) + + doctest_discover_tests(micras_lib_${SUITE}_tests + TEST_PREFIX "${SUITE}/" + ) +endfunction() + +############################################################################### +## Suites +############################################################################### + +micras_lib_add_suite(core micras::core) + +if(MICRAS_LIB_COMM) + micras_lib_add_suite(comm micras::comm) +endif() + +if(MICRAS_LIB_NAV) + micras_lib_add_suite(nav micras::nav) + + target_compile_definitions(micras_lib_nav_tests PRIVATE + MICRAS_LIB_TEST_MAZES="${CMAKE_CURRENT_SOURCE_DIR}/data/mazes" + ) +endif() + +if(MICRAS_LIB_HAL) + micras_lib_add_suite(hal micras::hal) +endif() -target_link_libraries(micras_host_proxy PUBLIC micras_host_core) - -foreach(TEST_NAME variable_pool cobs frame storage link) - add_executable(test_${TEST_NAME} test_${TEST_NAME}.cpp) - target_include_directories(test_${TEST_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) - target_compile_options(test_${TEST_NAME} PRIVATE -Wall -Wextra -fsanitize=address,undefined) - target_link_options(test_${TEST_NAME} PRIVATE -fsanitize=address,undefined) - add_test(NAME ${TEST_NAME} COMMAND test_${TEST_NAME}) -endforeach() - -target_link_libraries(test_variable_pool PRIVATE micras_host_core) -target_link_libraries(test_cobs PRIVATE micras_host_core) -target_link_libraries(test_frame PRIVATE micras_host_comm) -target_link_libraries(test_storage PRIVATE micras_host_proxy) -target_link_libraries(test_link PRIVATE micras_host_comm) +if(MICRAS_LIB_PROXY) + micras_lib_add_suite(proxy micras::proxy micras::proxy_models) + micras_lib_add_suite(models micras::proxy_models micras::proxy) +endif() diff --git a/tests/comm/test_frame.cpp b/tests/comm/test_frame.cpp index 15d1654..a80ff86 100644 --- a/tests/comm/test_frame.cpp +++ b/tests/comm/test_frame.cpp @@ -2,18 +2,17 @@ * @file */ +#include #include -#include -#include #include #include +#include + #include "micras/comm/frame.hpp" #include "micras/comm/protocol.hpp" -#include "test_host.hpp" - -using namespace micras::comm; +namespace micras::comm { namespace { struct Vector { std::string_view name; @@ -22,8 +21,6 @@ struct Vector { std::vector frame; }; -// The same table lives in micras-monitor, as src/lib/comm/__tests__/frameVectors.ts, so that both -// implementations are pinned to the same bytes. const std::vector vectors{ {"hello", MessageType{0x01}, {}, {4, 1, 1, 1, 0}}, {"hello_ack", @@ -64,57 +61,52 @@ const std::vector vectors{ {44, 137, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 247, 247, 0}}, }; + } // namespace -int main() { - for (const Vector& vector : vectors) { - std::vector built(max_frame_size); - built.resize(encode_frame(vector.type, vector.payload, built)); +TEST_SUITE("frame") { + TEST_CASE("builds and reads the golden frames of micras-monitor's src/lib/comm/__tests__/frameVectors.ts") { + for (const Vector& vector : vectors) { + CAPTURE(vector.name); - if (built != vector.frame) { - std::printf("%.*s: built", int(vector.name.size()), vector.name.data()); + std::vector built(max_frame_size); + built.resize(encode_frame(vector.type, vector.payload, built)); - for (uint8_t byte : built) { - std::printf(" %02X", byte); - } + CHECK(built == vector.frame); - std::puts(""); - CHECK(false); - } + FrameReader reader; + bool complete = false; - FrameReader reader; - bool complete = false; + for (const uint8_t byte : vector.frame) { + complete = reader.push(byte); + } - for (uint8_t byte : vector.frame) { - complete = reader.push(byte); + CHECK(complete); + CHECK(reader.type() == vector.type); + CHECK(std::vector(reader.payload().begin(), reader.payload().end()) == vector.payload); + CHECK(reader.discarded() == 0); } - - CHECK(complete); - CHECK(reader.type() == vector.type); - CHECK(std::vector(reader.payload().begin(), reader.payload().end()) == vector.payload); - CHECK(reader.discarded() == 0); } - // A frame that lost a byte is discarded on its own, and the next one still arrives - { + TEST_CASE("discards a frame that lost a byte and reads the next one") { const std::vector& good = vectors.at(3).frame; FrameReader reader; - bool complete = false; - for (std::size_t i = 0; i + 3 < good.size(); i++) { - CHECK(not reader.push(good[i])); + for (std::size_t index = 0; index + 3 < good.size(); index++) { + CHECK_FALSE(reader.push(good.at(index))); } - CHECK(not reader.push(0x00)); + CHECK_FALSE(reader.push(0x00)); CHECK(reader.discarded() == 1); - for (uint8_t byte : good) { + bool complete = false; + + for (const uint8_t byte : good) { complete = reader.push(byte); } CHECK(complete); CHECK(reader.discarded() == 1); } - - std::printf("frame ok: %zu vectors\n", vectors.size()); } +} // namespace micras::comm diff --git a/tests/comm/test_link.cpp b/tests/comm/test_link.cpp index 353dde9..6c5cc2b 100644 --- a/tests/comm/test_link.cpp +++ b/tests/comm/test_link.cpp @@ -3,282 +3,358 @@ */ #include -#include -#include +#include +#include +#include #include #include +#include #include +#include #include +#include + #include "micras/comm/frame.hpp" #include "micras/comm/link.hpp" +#include "micras/comm/protocol.hpp" #include "micras/core/byte_stream.hpp" #include "micras/core/variable_pool.hpp" -#include "test_host.hpp" - -using namespace micras; -using namespace micras::comm; -using V = std::vector; -struct Loopback : core::IByteStream { - std::deque to_robot; - std::deque from_robot; - std::size_t capacity{4096}; +namespace micras::comm { +namespace { +using Bytes = std::vector; +class Loopback : public core::IByteStream { +public: std::size_t read(std::span into) override { - std::size_t n = std::min(into.size(), to_robot.size()); - for (std::size_t i = 0; i < n; i++) { - into[i] = to_robot.front(); - to_robot.pop_front(); + const std::size_t count = std::min(into.size(), this->to_robot.size()); + + for (std::size_t index = 0; index < count; index++) { + into[index] = this->to_robot.front(); + this->to_robot.pop_front(); } - return n; + + return count; } std::size_t write(std::span from) override { - if (capacity - from_robot.size() < from.size()) + if (capacity - this->from_robot.size() < from.size()) { return 0; - for (uint8_t b : from) - from_robot.push_back(b); + } + + this->from_robot.insert(this->from_robot.end(), from.begin(), from.end()); return from.size(); } + + std::deque& incoming() { return this->to_robot; } + + std::deque& outgoing() { return this->from_robot; } + +private: + static constexpr std::size_t capacity{4096}; + + std::deque to_robot; + std::deque from_robot; }; -struct Commands : ICommandHandler { - std::vector> seen; +class Commands : public ICommandHandler { +public: + static constexpr uint8_t known_command{42}; CommandResult handle_command(uint8_t code, uint32_t argument) override { - seen.emplace_back(code, argument); - return code == 42 ? CommandResult::OK : CommandResult::UNKNOWN; + this->received.emplace_back(code, argument); + return code == known_command ? CommandResult::OK : CommandResult::UNKNOWN; } -}; -// --- host side of the protocol --- -static void send(Loopback& io, MessageType type, const V& payload) { - V frame(max_frame_size); - frame.resize(encode_frame(type, payload, frame)); - CHECK(!frame.empty()); - for (uint8_t b : frame) - io.to_robot.push_back(b); -} + const std::vector>& seen() const { return this->received; } + +private: + std::vector> received; +}; -struct Msg { +struct Message { MessageType type; - V payload; + Bytes payload; }; -// Stands in for the application: takes everything that arrived and returns the credit for it, -// which is what keeps the window open. -static std::vector drain(Loopback& io) { - static FrameReader rx; - std::vector out; - std::size_t taken = io.from_robot.size(); - while (!io.from_robot.empty()) { - uint8_t b = io.from_robot.front(); - io.from_robot.pop_front(); - if (rx.push(b)) - out.push_back({rx.type(), V(rx.payload().begin(), rx.payload().end())}); - } - if (taken > 0) - send(io, MessageType::CREDIT, {uint8_t(taken), uint8_t(taken >> 8)}); - return out; +float to_float(uint32_t bits) { + return std::bit_cast(bits); +} + +std::vector of_type(const std::vector& messages, MessageType type) { + std::vector matching; + std::ranges::copy_if(messages, std::back_inserter(matching), [type](const Message& message) { + return message.type == type; + }); + return matching; } -static const Msg& only(const std::vector& msgs, MessageType type) { - const Msg* found = nullptr; - for (const Msg& m : msgs) { - if (m.type == type) { - CHECK(found == nullptr); - found = &m; +class Session { +public: + static constexpr uint32_t loop_time_us{125}; + + Session() { + this->pool.add("cmd/", "linear", this->linear, {.stream = true}); + this->pool.add("cmd/", "angular", this->angular, {.stream = true}); + this->pool.add("model/", "gain", this->gain, {.stream = true, .write = true, .idle = true, .persist = true}); + this->pool.add("", "run_profile", this->profile, {.stream = true, .write = true}); + } + + void send(MessageType type, const Bytes& payload) { + Bytes frame(max_frame_size); + frame.resize(encode_frame(type, payload, frame)); + REQUIRE_FALSE(frame.empty()); + this->io.incoming().insert(this->io.incoming().end(), frame.begin(), frame.end()); + } + + void run(int iterations, bool idle = true) { + for (int iteration = 0; iteration < iterations; iteration++) { + this->now += loop_time_us; + this->link.poll(idle); + this->link.pump(this->now); } } - CHECK(found != nullptr); - return *found; -} -static uint16_t u16(const V& v, size_t i) { - return uint16_t(v[i] | v[i + 1] << 8); -} + std::vector drain() { + std::vector messages; + const std::size_t taken = this->io.outgoing().size(); -static uint32_t u32(const V& v, size_t i) { - return uint32_t(v[i] | v[i + 1] << 8 | v[i + 2] << 16 | uint32_t(v[i + 3]) << 24); -} + while (not this->io.outgoing().empty()) { + const uint8_t byte = this->io.outgoing().front(); + this->io.outgoing().pop_front(); -static float f32(const V& v, size_t i) { - uint32_t r = u32(v, i); - float f; - std::memcpy(&f, &r, 4); - return f; -} + if (this->application.push(byte)) { + messages.push_back( + {this->application.type(), + Bytes(this->application.payload().begin(), this->application.payload().end())} + ); + } + } -int main() { - core::TVariablePool<32> pool; - float linear = 1.0F, angular = 2.0F, gain = 0.5F; - uint8_t profile = 0; - pool.add("cmd/", "linear", linear, {.stream = true}); - pool.add("cmd/", "angular", angular, {.stream = true}); - pool.add("model/", "gain", gain, {.stream = true, .write = true, .idle = true, .persist = true}); - pool.add("", "run_profile", profile, {.stream = true, .write = true}); - - Commands commands; - Loopback io; - Link link{io, pool, commands, {.loop_time_us = 125}}; - - auto step = [&](uint32_t t, bool idle = true) { - link.poll(idle); - link.pump(t); - }; - auto settle = [&](uint32_t& t, int n = 40, bool idle = true) { - for (int i = 0; i < n; i++) - step(t += 125, idle); - }; - - uint32_t now = 0; - - // --- hello --- - send(io, MessageType::HELLO, {}); - settle(now, 4); - auto msgs = drain(io); - CHECK(msgs.size() == 1 && msgs[0].type == MessageType::HELLO_ACK); - CHECK(msgs[0].payload[0] == protocol_version); - const uint32_t hash = u32(msgs[0].payload, 1); - CHECK(hash == pool.schema_hash()); - CHECK(u16(msgs[0].payload, 5) == 4); - CHECK(u32(msgs[0].payload, 7) == 125); - - // --- schema, paged --- - send(io, MessageType::SCHEMA_REQUEST, {0, 0}); - settle(now, 40); - msgs = drain(io); - std::map names; - for (auto& m : msgs) { - CHECK(m.type == MessageType::SCHEMA_PAGE); - CHECK(u32(m.payload, 0) == hash); - uint16_t first = u16(m.payload, 4), total = u16(m.payload, 6); - CHECK(total == 4); - uint8_t count = m.payload[8]; - size_t i = 9; - for (uint8_t k = 0; k < count; k++) { - uint8_t len = m.payload[i + 2]; - names[first + k] = std::string(m.payload.begin() + i + 3, m.payload.begin() + i + 3 + len); - i += 3 + len; + if (taken > 0) { + this->send(MessageType::CREDIT, {static_cast(taken), static_cast(taken >> 8U)}); } + + return messages; } - CHECK(names.size() == 4); - CHECK(names[0] == "cmd/linear" && names[2] == "model/gain" && names[3] == "run_profile"); - - // --- a group streams at its period, coherently --- - send(io, MessageType::GROUP_DEFINE, {0, 4, 0, 2, 0, 0, 1, 0}); // group 0, period 4, ids {0,1} - send(io, MessageType::GROUP_ENABLE, {0, 1}); - settle(now, 4); - msgs = drain(io); - CHECK(msgs.size() >= 2 && msgs[0].type == MessageType::GROUP_ACK && msgs[1].type == MessageType::GROUP_ACK); - CHECK(u16(msgs[0].payload, 1) == 4 && u16(msgs[0].payload, 3) == 8); - - linear = 3.5F; - angular = -1.25F; - drain(io); - settle(now, 16); - msgs = drain(io); - CHECK(msgs.size() == 4); // 16 iterations at period 4 - - for (size_t k = 0; k < msgs.size(); k++) { - CHECK(msgs[k].type == MessageType::SAMPLE); - CHECK(msgs[k].payload[0] == 0); - CHECK(u16(msgs[k].payload, 1) == u16(msgs[0].payload, 1) + k); - CHECK(f32(msgs[k].payload, 7) == 3.5F); - CHECK(f32(msgs[k].payload, 11) == -1.25F); + + Message only(MessageType type) { + const std::vector matching = of_type(this->drain(), type); + REQUIRE(matching.size() == 1); + return matching.front(); } - // --- writes, with the idle guard --- - send(io, MessageType::WRITE, {2, 0, 0, 0, 0x80, 0x3F}); // model/gain = 1.0 - settle(now, 4, false); - msgs = drain(io); - CHECK(only(msgs, MessageType::WRITE_ACK).payload[2] == uint8_t(core::VariablePool::WriteStatus::NEEDS_IDLE)); - CHECK(gain == 0.5F); - - send(io, MessageType::WRITE, {2, 0, 0, 0, 0x80, 0x3F}); - settle(now, 4, true); - msgs = drain(io); - CHECK(only(msgs, MessageType::WRITE_ACK).payload[2] == uint8_t(core::VariablePool::WriteStatus::OK)); - CHECK(gain == 1.0F); - - // --- read --- - send(io, MessageType::READ, {2, 0}); - settle(now, 4); - msgs = drain(io); - CHECK(f32(only(msgs, MessageType::VALUE).payload, 2) == 1.0F); - - // --- commands are edges, acted on once --- - send(io, MessageType::COMMAND, {42, 7, 0, 0, 0}); - settle(now, 8); - msgs = drain(io); - CHECK(commands.seen.size() == 1); - CHECK(commands.seen[0].first == 42 && commands.seen[0].second == 7); - CHECK(only(msgs, MessageType::COMMAND_ACK).payload[1] == uint8_t(CommandResult::OK)); - - // --- a closed credit window drops samples and the sequence shows the gap --- - drain(io); - send(io, MessageType::HELLO, {}); // resets credit to the initial window - settle(now, 4); - drain(io); - send(io, MessageType::GROUP_DEFINE, {0, 1, 0, 2, 0, 0, 1, 0}); - send(io, MessageType::GROUP_ENABLE, {0, 1}); - settle(now, 4); - drain(io); - { + uint32_t dropped_samples() { core::TVariablePool<8> probe; - link.register_variables(probe, "link/"); - - uint8_t baseline_buf[4]; - uint32_t baseline = 0; - probe.read(*probe.find("link/dropped_samples"), baseline_buf); - std::memcpy(&baseline, baseline_buf, 4); - - settle(now, 400); // far more than the window allows - msgs = drain(io); - msgs.erase( - std::remove_if(msgs.begin(), msgs.end(), [](const Msg& m) { return m.type != MessageType::SAMPLE; }), - msgs.end() - ); - CHECK(!msgs.empty() && msgs.size() < 40); // the window, not the loop, set the rate - - uint8_t buf[4]; - uint32_t dropped = 0; - probe.read(*probe.find("link/dropped_samples"), buf); - std::memcpy(&dropped, buf, 4); - CHECK(dropped - baseline + msgs.size() == 400); // every attempt is either sent or counted - - const uint16_t last_sent = u16(msgs.back().payload, 1); - - // returning credit starts it again, and the sequence shows what was missed - send(io, MessageType::CREDIT, {0x00, 0x01}); - settle(now, 8); - msgs = drain(io); - msgs.erase( - std::remove_if(msgs.begin(), msgs.end(), [](const Msg& m) { return m.type != MessageType::SAMPLE; }), - msgs.end() - ); - CHECK(!msgs.empty()); - CHECK(u16(msgs.front().payload, 1) > last_sent + 1); + this->link.register_variables(probe, "link/"); + + std::array buffer{}; + probe.read(probe.find("link/dropped_samples").value(), buffer); + return Reader{buffer}.u32(); + } + + core::TVariablePool<32> pool; + float linear{1.0F}; + float angular{2.0F}; + float gain{0.5F}; + uint8_t profile{0}; + Commands commands; + Loopback io; + Link link{io, pool, commands, {.loop_time_us = loop_time_us}}; + FrameReader application; + uint32_t now{0}; +}; +} // namespace + +TEST_SUITE("link") { + TEST_CASE_FIXTURE(Session, "answers hello with the version the schema hash and the loop time") { + this->send(MessageType::HELLO, {}); + this->run(4); + + const std::vector messages = this->drain(); + REQUIRE(messages.size() == 1); + REQUIRE(messages.front().type == MessageType::HELLO_ACK); + + Reader reader{messages.front().payload}; + CHECK(reader.u8() == protocol_version); + CHECK(reader.u32() == this->pool.schema_hash()); + CHECK(reader.u16() == 4); + CHECK(reader.u32() == loop_time_us); + } + + TEST_CASE_FIXTURE(Session, "sends the schema in pages") { + this->send(MessageType::SCHEMA_REQUEST, {0, 0}); + this->run(40); + + std::map names; + + for (const Message& message : this->drain()) { + REQUIRE(message.type == MessageType::SCHEMA_PAGE); + + Reader reader{message.payload}; + CHECK(reader.u32() == this->pool.schema_hash()); + + const uint16_t first = reader.u16(); + CHECK(reader.u16() == 4); + + const uint8_t count = reader.u8(); + + for (uint8_t entry = 0; entry < count; entry++) { + reader.u16(); + + const uint8_t length = reader.u8(); + const std::span name = reader.rest().first(length); + names[static_cast(first + entry)] = std::string(name.begin(), name.end()); + + for (uint8_t skipped = 0; skipped < length; skipped++) { + reader.u8(); + } + } + + CHECK(reader.valid()); + } + + CHECK(names.size() == 4); + CHECK(names[0] == "cmd/linear"); + CHECK(names[2] == "model/gain"); + CHECK(names[3] == "run_profile"); } - send(io, MessageType::GROUP_ENABLE, {0, 0}); - settle(now, 8); - drain(io); - - // --- a corrupted frame costs only itself --- - drain(io); - const std::size_t corrupt_at = io.to_robot.size() + 2; - send(io, MessageType::READ, {0, 0}); - io.to_robot[corrupt_at] ^= 0xFF; // flip a byte inside the first frame - send(io, MessageType::READ, {1, 0}); - settle(now, 8); - msgs = drain(io); - auto values = std::count_if(msgs.begin(), msgs.end(), [](const Msg& m) { return m.type == MessageType::VALUE; }); - CHECK(values == 1); - for (auto& m : msgs) - if (m.type == MessageType::VALUE) - CHECK(u16(m.payload, 0) == 1); - - std::puts("link ok"); + TEST_CASE_FIXTURE(Session, "streams a group at its period with coherent samples") { + this->send(MessageType::GROUP_DEFINE, {0, 4, 0, 2, 0, 0, 1, 0}); + this->send(MessageType::GROUP_ENABLE, {0, 1}); + this->run(4); + + const std::vector acks = of_type(this->drain(), MessageType::GROUP_ACK); + REQUIRE(acks.size() == 2); + + Reader ack{acks.front().payload}; + ack.u8(); + CHECK(ack.u16() == 4); + CHECK(ack.u16() == 8); + + this->linear = 3.5F; + this->angular = -1.25F; + this->drain(); + this->run(16); + + const std::vector samples = this->drain(); + REQUIRE(samples.size() == 4); + + uint16_t first_sequence{}; + + for (std::size_t index = 0; index < samples.size(); index++) { + CAPTURE(index); + REQUIRE(samples.at(index).type == MessageType::SAMPLE); + + Reader reader{samples.at(index).payload}; + CHECK(reader.u8() == 0); + + const uint16_t sequence = reader.u16(); + + if (index == 0) { + first_sequence = sequence; + } + + CHECK(sequence == first_sequence + index); + reader.u32(); + CHECK(to_float(reader.u32()) == 3.5F); + CHECK(to_float(reader.u32()) == -1.25F); + } + } + + TEST_CASE_FIXTURE(Session, "refuses a guarded write while the robot moves and accepts it when idle") { + const Bytes write_gain{2, 0, 0, 0, 0x80, 0x3F}; + + this->send(MessageType::WRITE, write_gain); + this->run(4, false); + + const Message refusal = this->only(MessageType::WRITE_ACK); + Reader refused{refusal.payload}; + refused.u16(); + CHECK(refused.u8() == static_cast(core::VariablePool::WriteStatus::NEEDS_IDLE)); + CHECK(this->gain == 0.5F); + + this->send(MessageType::WRITE, write_gain); + this->run(4, true); + + const Message acceptance = this->only(MessageType::WRITE_ACK); + Reader accepted{acceptance.payload}; + accepted.u16(); + CHECK(accepted.u8() == static_cast(core::VariablePool::WriteStatus::OK)); + CHECK(this->gain == 1.0F); + + this->send(MessageType::READ, {2, 0}); + this->run(4); + + const Message answer = this->only(MessageType::VALUE); + Reader value{answer.payload}; + value.u16(); + CHECK(to_float(value.u32()) == 1.0F); + } + + TEST_CASE_FIXTURE(Session, "acts on a command once") { + this->send(MessageType::COMMAND, {Commands::known_command, 7, 0, 0, 0}); + this->run(8); + + const Message answer = this->only(MessageType::COMMAND_ACK); + Reader ack{answer.payload}; + ack.u8(); + CHECK(ack.u8() == static_cast(CommandResult::OK)); + + REQUIRE(this->commands.seen().size() == 1); + CHECK(this->commands.seen().front().first == Commands::known_command); + CHECK(this->commands.seen().front().second == 7); + } + + TEST_CASE_FIXTURE(Session, "drops the samples a closed credit window cannot carry and counts them") { + this->send(MessageType::HELLO, {}); + this->run(4); + this->drain(); + this->send(MessageType::GROUP_DEFINE, {0, 1, 0, 2, 0, 0, 1, 0}); + this->send(MessageType::GROUP_ENABLE, {0, 1}); + this->run(4); + this->drain(); + + const uint32_t baseline = this->dropped_samples(); + constexpr int iterations{400}; + + this->run(iterations); + + const std::vector sent = of_type(this->drain(), MessageType::SAMPLE); + REQUIRE_FALSE(sent.empty()); + CHECK(sent.size() < 40); + CHECK(this->dropped_samples() - baseline + sent.size() == iterations); + + Reader last{sent.back().payload}; + last.u8(); + const uint16_t last_sent = last.u16(); + + this->send(MessageType::CREDIT, {0x00, 0x01}); + this->run(8); + + const std::vector resumed = of_type(this->drain(), MessageType::SAMPLE); + REQUIRE_FALSE(resumed.empty()); + + Reader first{resumed.front().payload}; + first.u8(); + CHECK(first.u16() > last_sent + 1); + } + + TEST_CASE_FIXTURE(Session, "loses only the frame a corrupted byte hits") { + const std::size_t corrupt_at = this->io.incoming().size() + 2; + + this->send(MessageType::READ, {0, 0}); + this->io.incoming().at(corrupt_at) ^= 0xFFU; + this->send(MessageType::READ, {1, 0}); + this->run(8); + + const std::vector values = of_type(this->drain(), MessageType::VALUE); + REQUIRE(values.size() == 1); + + Reader reader{values.front().payload}; + CHECK(reader.u16() == 1); + } } +} // namespace micras::comm diff --git a/tests/core/test_cobs.cpp b/tests/core/test_cobs.cpp index 3b46dc7..5499dba 100644 --- a/tests/core/test_cobs.cpp +++ b/tests/core/test_cobs.cpp @@ -2,132 +2,104 @@ * @file */ -#include -#include +#include +#include #include #include +#include + #include "micras/core/cobs.hpp" -#include "test_host.hpp" -using namespace micras::core; -using V = std::vector; +namespace micras::core { +namespace { +using Bytes = std::vector; -static V enc(const V& in) { - V out(cobs_encoded_size(in.size())); - out.resize(cobs_encode(in, out)); - return out; +Bytes encode(const Bytes& raw) { + Bytes encoded(cobs_encoded_size(raw.size())); + encoded.resize(cobs_encode(raw, encoded)); + return encoded; } -static V dec(const V& in) { - V out(in.size() + 1); - out.resize(cobs_decode(in, out)); - return out; +Bytes decode(const Bytes& encoded) { + Bytes decoded(encoded.size() + 1); + decoded.resize(cobs_decode(encoded, decoded)); + return decoded; } -static void vec(const V& raw, const V& expected) { - V got = enc(raw); - if (got != expected) { - std::printf("encode mismatch for %zu bytes: got %zu expected %zu\n", raw.size(), got.size(), expected.size()); - - for (auto b : got) { - std::printf("%02X ", b); - } - - std::puts(""); - - for (auto b : expected) { - std::printf("%02X ", b); - } +Bytes count(int first, int last) { + Bytes bytes; - std::puts(""); - CHECK(false); + for (int value = first; value <= last; value++) { + bytes.push_back(static_cast(value)); } - CHECK(dec(got) == raw); + + return bytes; } -static V range(int a, int b) { - V v; - for (int i = a; i <= b; i++) - v.push_back(uint8_t(i)); - return v; +Bytes join(const Bytes& head, const Bytes& tail) { + Bytes joined = head; + joined.insert(joined.end(), tail.begin(), tail.end()); + return joined; } -int main() { - vec({}, {0x01}); - vec({0x00}, {0x01, 0x01}); - vec({0x00, 0x00}, {0x01, 0x01, 0x01}); - vec({0x00, 0x11, 0x00}, {0x01, 0x02, 0x11, 0x01}); - vec({0x11, 0x22, 0x00, 0x33}, {0x03, 0x11, 0x22, 0x02, 0x33}); - vec({0x11, 0x22, 0x33, 0x44}, {0x05, 0x11, 0x22, 0x33, 0x44}); - vec({0x11, 0x00, 0x00, 0x00}, {0x02, 0x11, 0x01, 0x01, 0x01}); - - { - V r = range(1, 254); - V e{0xFF}; - for (auto b : r) - e.push_back(b); - vec(r, e); - } - { - V r = range(0, 254); - V e{0x01, 0xFF}; - for (int i = 1; i <= 254; i++) - e.push_back(i); - vec(r, e); - } - { - V r = range(1, 255); - V e{0xFF}; - for (int i = 1; i <= 254; i++) - e.push_back(i); - e.push_back(0x02); - e.push_back(0xFF); - vec(r, e); - } - { - V r = range(2, 255); - r.push_back(0x00); - V e{0xFF}; - for (int i = 2; i <= 255; i++) - e.push_back(i); - e.push_back(0x01); - e.push_back(0x01); - vec(r, e); - } - { - V r = range(3, 255); - r.push_back(0x00); - r.push_back(0x01); - V e{0xFE}; - for (int i = 3; i <= 255; i++) - e.push_back(i); - e.push_back(0x02); - e.push_back(0x01); - vec(r, e); - } +void check_vector(const Bytes& raw, const Bytes& expected) { + CAPTURE(raw.size()); + + const Bytes encoded = encode(raw); - // every encoded frame is free of the delimiter, and round trips - std::mt19937 rng{7}; - for (int trial = 0; trial < 200000; trial++) { - V raw(rng() % 600); - for (auto& b : raw) - b = uint8_t(rng() % (trial % 3 == 0 ? 3 : 256)); - V e = enc(raw); - CHECK(e.size() <= cobs_encoded_size(raw.size())); - for (auto b : e) - CHECK(b != 0); - CHECK(dec(e) == raw); + CHECK(encoded == expected); + CHECK(decode(encoded) == raw); +} +} // namespace + +TEST_SUITE("cobs") { + TEST_CASE("encodes the reference vectors and decodes them back") { + check_vector({}, {0x01}); + check_vector({0x00}, {0x01, 0x01}); + check_vector({0x00, 0x00}, {0x01, 0x01, 0x01}); + check_vector({0x00, 0x11, 0x00}, {0x01, 0x02, 0x11, 0x01}); + check_vector({0x11, 0x22, 0x00, 0x33}, {0x03, 0x11, 0x22, 0x02, 0x33}); + check_vector({0x11, 0x22, 0x33, 0x44}, {0x05, 0x11, 0x22, 0x33, 0x44}); + check_vector({0x11, 0x00, 0x00, 0x00}, {0x02, 0x11, 0x01, 0x01, 0x01}); + check_vector(count(1, 254), join({0xFF}, count(1, 254))); + check_vector(count(0, 254), join({0x01, 0xFF}, count(1, 254))); + check_vector(count(1, 255), join(join({0xFF}, count(1, 254)), {0x02, 0xFF})); + check_vector(join(count(2, 255), {0x00}), join(join({0xFF}, count(2, 255)), {0x01, 0x01})); + check_vector(join(count(3, 255), {0x00, 0x01}), join(join({0xFE}, count(3, 255)), {0x02, 0x01})); } - // malformed frames are rejected rather than half decoded - V scratch(8); - CHECK(cobs_decode(V{0x00}, scratch) == 0); - CHECK(cobs_decode(V{0x05, 0x01}, scratch) == 0); - { - V out(2); - CHECK(cobs_decode(V{0x06, 1, 2, 3, 4, 5}, out) == 0); + TEST_CASE("round trips random frames without a delimiter in the encoding") { + std::mt19937 generator{7}; + + for (int trial = 0; trial < 200000; trial++) { + Bytes raw(generator() % 600); + const uint32_t alphabet = trial % 3 == 0 ? 3 : 256; + + for (uint8_t& byte : raw) { + byte = static_cast(generator() % alphabet); + } + + const Bytes encoded = encode(raw); + bool delimiter_free = true; + + for (const uint8_t byte : encoded) { + delimiter_free = delimiter_free and byte != cobs_delimiter; + } + + REQUIRE(encoded.size() <= cobs_encoded_size(raw.size())); + REQUIRE(delimiter_free); + REQUIRE(decode(encoded) == raw); + } } - std::puts("cobs ok"); + TEST_CASE("rejects malformed frames instead of decoding half of them") { + Bytes scratch(8); + CHECK(cobs_decode(Bytes{0x00}, scratch) == 0); + CHECK(cobs_decode(Bytes{0x05, 0x01}, scratch) == 0); + + Bytes small(2); + CHECK(cobs_decode(Bytes{0x06, 1, 2, 3, 4, 5}, small) == 0); + } } +} // namespace micras::core diff --git a/tests/core/test_variable_pool.cpp b/tests/core/test_variable_pool.cpp index 5fffac1..6e907a5 100644 --- a/tests/core/test_variable_pool.cpp +++ b/tests/core/test_variable_pool.cpp @@ -2,20 +2,30 @@ * @file */ -#include +#include +#include +#include #include +#include +#include -#include "micras/core/variable_pool.hpp" -#include "test_host.hpp" +#include -using namespace micras::core; +#include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" -struct Blob : ISerializable { - std::vector data{1, 2, 3}; +namespace micras::core { +namespace { +class Blob : public ISerializable { +public: + std::vector serialize() const override { return this->data; } - std::vector serialize() const override { return data; } + void deserialize(const uint8_t* buffer, uint16_t size) override { + this->data.assign(buffer, std::next(buffer, size)); + } - void deserialize(const uint8_t* p, uint16_t n) override { data.assign(p, p + n); } +private: + std::vector data{1, 2, 3}; }; enum class Profile : uint8_t { @@ -23,63 +33,94 @@ enum class Profile : uint8_t { B }; -int main() { - TVariablePool<8> pool; - float speed = 1.5F; - const float k = 2.0F; - Profile profile = Profile::A; - Blob blob; - - auto id_speed = pool.add("speed/", "linear", speed, {.stream = true, .write = true}); - auto id_k = pool.add("model/", "kv", k, {.stream = true, .write = true, .idle = true, .persist = true}); - auto id_prof = pool.add("", "run_profile", profile, {.write = true, .idle = true, .persist = true}); - auto id_blob = pool.add("", "maze", blob, {.persist = true}); - - CHECK(id_speed == 0 && id_k == 1 && id_prof == 2 && id_blob == 3); - CHECK(pool.all().size() == 4); - CHECK(pool.at(id_k).access.write == false); - CHECK(pool.at(id_prof).type == TypeCode::U8); - CHECK(pool.at(id_blob).type == TypeCode::BLOB); - CHECK(pool.at(id_blob).access.stream == false); - - CHECK(pool.find("speed/linear").value() == 0); - CHECK(pool.find("model/kv").value() == 1); - CHECK(pool.find("run_profile").value() == 2); - CHECK(!pool.find("speed/linea").has_value()); - CHECK(!pool.find("speed/linearr").has_value()); - - uint8_t buf[8]; - CHECK(pool.read(id_speed, buf) == 4); - float out; - std::memcpy(&out, buf, 4); - CHECK(out == 1.5F); - CHECK(pool.read(id_blob, buf) == 0); - - float nv = 9.0F; - CHECK(pool.write(id_speed, {reinterpret_cast(&nv), 4}, false) == VariablePool::WriteStatus::OK); - CHECK(speed == 9.0F); - CHECK(pool.write(id_k, {reinterpret_cast(&nv), 4}, true) == VariablePool::WriteStatus::READ_ONLY); - uint8_t one = 1; - CHECK(pool.write(id_prof, {&one, 1}, false) == VariablePool::WriteStatus::NEEDS_IDLE); - CHECK(pool.write(id_prof, {&one, 1}, true) == VariablePool::WriteStatus::OK); - CHECK(profile == Profile::B); - CHECK(pool.write(id_speed, {&one, 1}, true) == VariablePool::WriteStatus::WRONG_SIZE); - CHECK(pool.write(99, {&one, 1}, true) == VariablePool::WriteStatus::NO_SUCH_ID); - - uint32_t h0 = pool.schema_hash(); - TVariablePool<8> other; - other.add("speed/", "linear", speed, {.stream = true, .write = true}); - other.add("model/", "kv", k, {.stream = true, .write = true, .idle = true, .persist = true}); - other.add("", "run_profile", profile, {.write = true, .idle = true, .persist = true}); - other.add("", "maze", blob, {.persist = true}); - CHECK(other.schema_hash() == h0); - other.add("", "extra", speed, {}); - CHECK(other.schema_hash() != h0); - - TVariablePool<1> tiny; - tiny.add("", "a", speed, {}); - CHECK(tiny.add("", "b", speed, {}) == VariablePool::invalid_id); - CHECK(tiny.all().size() == 1); - - std::printf("pool ok, sizeof(Variable)=%zu hash=%08X\n", sizeof(Variable), h0); +std::span bytes_of(const float& value) { + return {std::bit_cast(&value), sizeof(value)}; +} +} // namespace + +TEST_SUITE("variable_pool") { + TEST_CASE("registers finds reads and writes variables with their access") { + TVariablePool<8> pool; + float speed = 1.5F; + const float gain = 2.0F; + Profile profile = Profile::A; + Blob blob; + + const auto speed_id = pool.add("speed/", "linear", speed, {.stream = true, .write = true}); + const auto gain_id = + pool.add("model/", "kv", gain, {.stream = true, .write = true, .idle = true, .persist = true}); + const auto profile_id = pool.add("", "run_profile", profile, {.write = true, .idle = true, .persist = true}); + const auto blob_id = pool.add("", "maze", blob, {.persist = true}); + + CHECK(speed_id == 0); + CHECK(gain_id == 1); + CHECK(profile_id == 2); + CHECK(blob_id == 3); + CHECK(pool.all().size() == 4); + CHECK_FALSE(pool.at(gain_id).access.write); + CHECK(pool.at(profile_id).type == TypeCode::U8); + CHECK(pool.at(blob_id).type == TypeCode::BLOB); + CHECK_FALSE(pool.at(blob_id).access.stream); + + CHECK(pool.find("speed/linear") == speed_id); + CHECK(pool.find("model/kv") == gain_id); + CHECK(pool.find("run_profile") == profile_id); + CHECK_FALSE(pool.find("speed/linea").has_value()); + CHECK_FALSE(pool.find("speed/linearr").has_value()); + + std::array buffer{}; + CHECK(pool.read(speed_id, buffer) == sizeof(float)); + + float read_back{}; + std::memcpy(&read_back, buffer.data(), sizeof(read_back)); + CHECK(read_back == 1.5F); + CHECK(pool.read(blob_id, buffer) == 0); + + const float written = 9.0F; + const std::array one{1}; + + CHECK(pool.write(speed_id, bytes_of(written), false) == VariablePool::WriteStatus::OK); + CHECK(speed == 9.0F); + CHECK(pool.write(gain_id, bytes_of(written), true) == VariablePool::WriteStatus::READ_ONLY); + CHECK(pool.write(profile_id, one, false) == VariablePool::WriteStatus::NEEDS_IDLE); + CHECK(pool.write(profile_id, one, true) == VariablePool::WriteStatus::OK); + CHECK(profile == Profile::B); + CHECK(pool.write(speed_id, one, true) == VariablePool::WriteStatus::WRONG_SIZE); + CHECK(pool.write(99, one, true) == VariablePool::WriteStatus::NO_SUCH_ID); + } + + TEST_CASE("hashes the schema by the names types and access") { + float speed = 1.5F; + const float gain = 2.0F; + Profile profile = Profile::A; + Blob blob; + + const auto fill = [&](VariablePool& pool) { + pool.add("speed/", "linear", speed, {.stream = true, .write = true}); + pool.add("model/", "kv", gain, {.stream = true, .write = true, .idle = true, .persist = true}); + pool.add("", "run_profile", profile, {.write = true, .idle = true, .persist = true}); + pool.add("", "maze", blob, {.persist = true}); + }; + + TVariablePool<8> first; + TVariablePool<8> second; + fill(first); + fill(second); + + CHECK(second.schema_hash() == first.schema_hash()); + + second.add("", "extra", speed, {}); + CHECK(second.schema_hash() != first.schema_hash()); + } + + TEST_CASE("refuses a variable once the pool is full") { + TVariablePool<1> pool; + float value = 0.0F; + + pool.add("", "a", value, {}); + + CHECK(pool.add("", "b", value, {}) == VariablePool::invalid_id); + CHECK(pool.all().size() == 1); + } } +} // namespace micras::core diff --git a/tests/hal/test_adc.cpp b/tests/hal/test_adc.cpp new file mode 100644 index 0000000..9c6fcee --- /dev/null +++ b/tests/hal/test_adc.cpp @@ -0,0 +1,73 @@ +/** + * @file + */ + +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/host/board.hpp" + +namespace micras::test { +TEST_SUITE("adc") { + TEST_CASE_FIXTURE(HostBoard, "copies a finished sequence into the snapshot") { + hal::AdcDma adc{adc_config}; + std::array buffer{}; + std::array snapshot{}; + std::array read{}; + + REQUIRE(adc.was_initialized()); + REQUIRE(adc.start_dma(buffer, snapshot)); + + const hal::host::AdcPort& port = hal::host::Board::adc(adc_config.handle); + CHECK(port.size() == 2); + + port.write(0, 1500); + port.write(1, 900); + port.write(2, 100); + CHECK(adc.read_snapshot(read) == 0); + CHECK(read == std::array{0, 0}); + + port.finish_sequence(); + CHECK(adc.read_snapshot(read) == 1); + CHECK(read == std::array{1500, 900}); + CHECK(adc.get_max_reading() == 4095); + CHECK(adc.get_reference_voltage() == doctest::Approx(3.3F)); + } + + TEST_CASE_FIXTURE(HostBoard, "stops updating after an error until it recovers") { + hal::AdcDma adc{adc_config}; + std::array buffer{}; + std::array snapshot{}; + std::array read{}; + REQUIRE(adc.start_dma(buffer, snapshot)); + + const hal::host::AdcPort& port = hal::host::Board::adc(adc_config.handle); + const uint32_t restarts = hal::AdcDma::get_restarts(); + + hal::AdcDma::on_error(adc_config.handle); + port.write(0, 7); + port.finish_sequence(); + CHECK(adc.read_snapshot(read) == 0); + + adc.recover(); + CHECK(hal::AdcDma::get_restarts() == restarts + 1); + + port.finish_sequence(); + CHECK(adc.read_snapshot(read) == 1); + CHECK(read.front() == 7); + } + + TEST_CASE_FIXTURE(HostBoard, "refuses a snapshot of another size than the buffer") { + hal::AdcDma adc{adc_config}; + std::array buffer{}; + std::array snapshot{}; + + CHECK_FALSE(adc.start_dma(buffer, snapshot)); + CHECK_FALSE(adc.was_initialized()); + } +} +} // namespace micras::test diff --git a/tests/hal/test_board.cpp b/tests/hal/test_board.cpp new file mode 100644 index 0000000..9a50001 --- /dev/null +++ b/tests/hal/test_board.cpp @@ -0,0 +1,58 @@ +/** + * @file + */ + +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/host/board.hpp" + +namespace micras::test { +TEST_SUITE("board") { + TEST_CASE_FIXTURE(HostBoard, "reads what the outside drives and otherwise what the firmware wrote") { + hal::Gpio led{status_led_config}; + led.write(true); + + hal::host::GpioPort& port = hal::host::Board::gpio(status_led_config.port, status_led_config.pin); + CHECK(port.output); + CHECK(led.read()); + + port.input = false; + CHECK_FALSE(led.read()); + CHECK(port.touched); + + led.toggle(); + CHECK_FALSE(port.output); + } + + TEST_CASE_FIXTURE(HostBoard, "names a port after the label of its pin") { + const hal::Gpio button{button_config}; + static_cast(button.read()); + + CHECK(hal::host::Board::gpio(button_config.port, button_config.pin).name == "Button"); + } + + TEST_CASE_FIXTURE(HostBoard, "reports the ports the firmware touched that nothing is bound to") { + hal::Gpio led{status_led_config}; + led.write(true); + + CHECK(hal::host::Board::unbound() == std::vector{"Status_LED"}); + + hal::host::Board::gpio(status_led_config.port, status_led_config.pin).bound = true; + CHECK(hal::host::Board::unbound().empty()); + } + + TEST_CASE_FIXTURE(HostBoard, "forgets every port on a reset") { + hal::Gpio led{status_led_config}; + led.write(true); + hal::host::Board::reset(); + + CHECK(hal::host::Board::unbound().empty()); + CHECK_FALSE(hal::host::Board::gpio(status_led_config.port, status_led_config.pin).output); + } +} +} // namespace micras::test diff --git a/tests/hal/test_clock.cpp b/tests/hal/test_clock.cpp new file mode 100644 index 0000000..af87543 --- /dev/null +++ b/tests/hal/test_clock.cpp @@ -0,0 +1,49 @@ +/** + * @file + */ + +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::test { +TEST_SUITE("clock") { + TEST_CASE_FIXTURE(HostBoard, "charges a quantum per read and hands over at every step") { + int steps = 0; + hal::host::Clock::instance().set_handover(125, [&steps] { steps++; }); + + const uint32_t start = hal::Timer::get_counter(); + + while (hal::Timer::to_microseconds(hal::Timer::get_counter() - start) < 1000) { } + + CHECK(steps == 8); + CHECK(hal::host::Clock::instance().steps() == 8); + CHECK(hal::Timer::get_counter_ms() == 1); + } + + TEST_CASE_FIXTURE(HostBoard, "counts cycles at the core clock and converts them") { + const uint32_t cycles_per_microsecond = SystemCoreClock / 1000000; + + CHECK(hal::Timer::to_cycles(10) == 10 * cycles_per_microsecond); + CHECK(hal::Timer::to_microseconds(10 * cycles_per_microsecond) == 10); + + const uint32_t first = hal::Timer::get_counter(); + const uint32_t second = hal::Timer::get_counter(); + CHECK(second - first == cycles_per_microsecond); + } + + TEST_CASE_FIXTURE(HostBoard, "stops handing over once cleared") { + int steps = 0; + hal::host::Clock::instance().set_handover(1, [&steps] { steps++; }); + hal::Timer::get_counter(); + hal::host::Clock::instance().clear_handover(); + wait_us(10); + + CHECK(steps == 1); + } +} +} // namespace micras::test diff --git a/tests/hal/test_crc.cpp b/tests/hal/test_crc.cpp new file mode 100644 index 0000000..5a09783 --- /dev/null +++ b/tests/hal/test_crc.cpp @@ -0,0 +1,83 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/crc.hpp" + +namespace micras::test { +namespace { +uint8_t reference_crc8(std::span data, uint8_t polynomial, uint8_t initial) { + uint8_t crc = initial; + + for (const uint8_t byte : data) { + crc ^= byte; + + for (int bit = 0; bit < 8; bit++) { + crc = (crc & 0x80U) != 0 ? static_cast((crc << 1U) ^ polynomial) : static_cast(crc << 1U); + } + } + + return crc; +} +} // namespace + +TEST_SUITE("crc") { + TEST_CASE_FIXTURE(HostBoard, "computes the check value of CRC-8/SAE-J1850") { + CRC_HandleTypeDef handle{}; + handle.Init = { + .DefaultPolynomialUse = DEFAULT_POLYNOMIAL_DISABLE, + .DefaultInitValueUse = DEFAULT_INIT_VALUE_DISABLE, + .GeneratingPolynomial = 0x1D, + .CRCLength = CRC_POLYLENGTH_8B, + .InitValue = 0xFF, + .InputDataInversionMode = CRC_INPUTDATA_INVERSION_NONE, + .OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_DISABLE, + }; + hal::Crc crc{{.handle = &handle}}; + + constexpr std::string_view check{"123456789"}; + const std::vector bytes(check.begin(), check.end()); + + CHECK((crc.calculate(bytes) ^ 0xFFU) == 0x4B); + } + + TEST_CASE_FIXTURE(HostBoard, "computes what the unit is configured for") { + hal::Crc crc{crc_config}; + + for (const std::array bytes : std::vector>{ + {0x00, 0x00}, {0x40, 0x01}, {0x40, 0x1A}, {0x00, 0x80}, {0x3F, 0xFF}, {0xC0, 0x15} + }) { + CAPTURE(bytes); + CHECK(crc.calculate(bytes) == reference_crc8(bytes, 0x1D, 0xC4)); + } + } + + TEST_CASE_FIXTURE(HostBoard, "computes the standard CRC-32 with the default polynomial") { + CRC_HandleTypeDef handle{}; + handle.Init = { + .DefaultPolynomialUse = DEFAULT_POLYNOMIAL_ENABLE, + .DefaultInitValueUse = DEFAULT_INIT_VALUE_ENABLE, + .GeneratingPolynomial = 0, + .CRCLength = CRC_POLYLENGTH_32B, + .InitValue = 0, + .InputDataInversionMode = CRC_INPUTDATA_INVERSION_BYTE, + .OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_ENABLE, + }; + hal::Crc crc{{.handle = &handle}}; + + constexpr std::string_view check{"123456789"}; + const std::vector bytes(check.begin(), check.end()); + + CHECK((crc.calculate(bytes) ^ 0xFFFFFFFFU) == 0xCBF43926U); + } +} +} // namespace micras::test diff --git a/tests/hal/test_flash.cpp b/tests/hal/test_flash.cpp new file mode 100644 index 0000000..490a4fa --- /dev/null +++ b/tests/hal/test_flash.cpp @@ -0,0 +1,70 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/flash.hpp" +#include "micras/hal/host/board.hpp" + +namespace micras::test { +namespace { +constexpr std::array data{1, 2, 3, 4, 5}; + +bool is_erased(std::span bytes) { + return std::ranges::all_of(bytes, [](uint8_t byte) { return byte == hal::FlashWord::erased_value; }); +} +} // namespace + +TEST_SUITE("flash") { + TEST_CASE_FIXTURE(HostBoard, "reserves the upper half of the sectors") { + CHECK(hal::Flash::sector_size == 128 * 1024); + CHECK(hal::Flash::total_sectors == 4); + CHECK(hal::FlashWord::size == 32); + CHECK(is_erased(hal::Flash::read(0, hal::Flash::total_size))); + } + + TEST_CASE_FIXTURE(HostBoard, "writes a flash word once and reads it back padded") { + REQUIRE(hal::Flash::write(1, 0, data) == hal::Flash::Status::OK); + + const std::span word = hal::Flash::read(1, 0, hal::FlashWord::size); + CHECK(std::ranges::equal(word.first(data.size()), data)); + CHECK(is_erased(word.subspan(data.size()))); + CHECK(hal::host::Board::flash().touched); + + CHECK(hal::Flash::write(1, 0, data) == hal::Flash::Status::ERROR); + } + + TEST_CASE_FIXTURE(HostBoard, "writes again after the sector is erased") { + REQUIRE(hal::Flash::write(0, 0, data) == hal::Flash::Status::OK); + REQUIRE(hal::Flash::erase_sectors(0) == hal::Flash::Status::OK); + + CHECK(is_erased(hal::Flash::read(0, 0, hal::FlashWord::size))); + CHECK(hal::Flash::write(0, 0, data) == hal::Flash::Status::OK); + } + + TEST_CASE_FIXTURE(HostBoard, "refuses misaligned and out of bounds accesses") { + CHECK(hal::Flash::write(0, 4, data) == hal::Flash::Status::MISALIGNED); + CHECK(hal::Flash::write(hal::Flash::total_sectors, 0, data) == hal::Flash::Status::OUT_OF_BOUNDS); + CHECK(hal::Flash::write(hal::Flash::total_size, data) == hal::Flash::Status::OUT_OF_BOUNDS); + CHECK(hal::Flash::erase_sectors(3, 2) == hal::Flash::Status::OUT_OF_BOUNDS); + CHECK(hal::Flash::read(hal::Flash::total_size - 1, 2).empty()); + } + + TEST_CASE_FIXTURE(HostBoard, "stops a write where the simulated power loss cuts it off") { + std::array long_data{}; + long_data.fill(0x42); + hal::host::Board::flash().write_budget = 2; + + CHECK(hal::Flash::write(0, 0, long_data) == hal::Flash::Status::ERROR); + CHECK_FALSE(is_erased(hal::Flash::read(0, 0, 2 * hal::FlashWord::size))); + CHECK(is_erased(hal::Flash::read(0, 2 * hal::FlashWord::size, hal::FlashWord::size))); + } +} +} // namespace micras::test diff --git a/tests/hal/test_fmac.cpp b/tests/hal/test_fmac.cpp new file mode 100644 index 0000000..6e81a4b --- /dev/null +++ b/tests/hal/test_fmac.cpp @@ -0,0 +1,97 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/fmac.hpp" + +namespace micras::test { +namespace { +constexpr double q15_scale{32768.0}; + +constexpr std::array feed_forward{2048, 4096, 2048}; + +constexpr std::array feedback{29491, -13107}; + +std::vector direct_form(const std::vector& inputs) { + std::vector outputs; + std::array input_history{}; + std::array output_history{}; + + for (const double input : inputs) { + input_history = {input, input_history.at(0), input_history.at(1)}; + + double output = 0.0; + + for (std::size_t tap = 0; tap < feed_forward.size(); tap++) { + output += feed_forward.at(tap) / q15_scale * input_history.at(tap); + } + + for (std::size_t tap = 0; tap < feedback.size(); tap++) { + output += feedback.at(tap) / q15_scale * output_history.at(tap); + } + + output_history = {output, output_history.at(0)}; + outputs.push_back(output); + } + + return outputs; +} +} // namespace + +TEST_SUITE("fmac") { + TEST_CASE_FIXTURE(HostBoard, "runs the configured filter as a direct form reference does") { + hal::Fmac fmac{fmac_config}; + REQUIRE(fmac.configure_iir(feed_forward, feedback)); + CHECK(fmac.was_initialized()); + + std::vector inputs; + + for (int sample = 0; sample < 200; sample++) { + inputs.push_back(0.5 * std::sin(0.1 * sample) + (sample > 100 ? 0.25 : 0.0)); + } + + const std::vector expected = direct_form(inputs); + + for (std::size_t sample = 0; sample < inputs.size(); sample++) { + CAPTURE(sample); + const auto input = static_cast(std::lround(inputs.at(sample) * q15_scale)); + const double output = fmac.update(input) / q15_scale; + + CHECK(std::abs(output - expected.at(sample)) < 1e-3); + } + } + + TEST_CASE_FIXTURE(HostBoard, "clips the output to the q1.15 range") { + hal::Fmac fmac{fmac_config}; + constexpr std::array gain{32767}; + constexpr std::array integrator{32767}; + REQUIRE(fmac.configure_iir(gain, integrator)); + + int16_t output = 0; + + for (int sample = 0; sample < 10; sample++) { + output = fmac.update(30000); + } + + CHECK(output == 32767); + } + + TEST_CASE_FIXTURE(HostBoard, "refuses a filter that does not fit its buffers") { + hal::Fmac fmac{fmac_config}; + constexpr std::array too_many{}; + + CHECK_FALSE(fmac.configure_iir(too_many, too_many)); + CHECK_FALSE(fmac.configure_iir({}, feedback)); + CHECK_FALSE(fmac.was_initialized()); + } +} +} // namespace micras::test diff --git a/tests/hal/test_mcu.cpp b/tests/hal/test_mcu.cpp new file mode 100644 index 0000000..48b9d1e --- /dev/null +++ b/tests/hal/test_mcu.cpp @@ -0,0 +1,65 @@ +/** + * @file + */ + +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/mcu.hpp" +#include "micras/hal/pwm.hpp" + +extern "C" { +void SystemClock_Config(); +void PeriphCommonClock_Config(); +} + +namespace micras::test { +TEST_SUITE("mcu") { + TEST_CASE_FIXTURE(HostBoard, "runs the clock and peripheral init functions it is given") { + constexpr std::array inits{MX_TIM15_Init, MX_UART4_Init}; + + hal::Mcu::init({ + .clock_init = SystemClock_Config, + .peripheral_clock_init = PeriphCommonClock_Config, + .peripheral_inits = inits, + .cpu_frequency_boost = false, + }); + + CHECK(htim15.State == HAL_TIM_STATE_READY); + CHECK(huart4.gState == HAL_UART_STATE_READY); + CHECK(hal::Mcu::is_cpu_frequency_supported()); + CHECK_FALSE(hal::Mcu::was_reset_by_watchdog()); + } + + TEST_CASE_FIXTURE(HostBoard, "counts a watchdog expiry when a refresh comes late") { + hal::Mcu::set_watchdog_timeout(10); + wait_us(5000); + hal::Mcu::refresh_watchdog(); + CHECK(hal::host::Board::mcu().watchdog_expiries == 0); + + wait_us(11000); + hal::Mcu::refresh_watchdog(); + CHECK(hal::host::Board::mcu().watchdog_expiries == 1); + } + + TEST_CASE_FIXTURE(HostBoard, "stops the outputs in an emergency") { + hal::Pwm pwm{pwm_config}; + hal::Gpio enable{status_led_config}; + pwm.set_duty_cycle(80.0F); + enable.write(true); + + const std::array outputs{pwm_config}; + const std::array enables{status_led_config}; + hal::Mcu::emergency_stop(outputs, enables); + + CHECK(hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel).duty_cycle == 0.0F); + CHECK_FALSE(hal::host::Board::gpio(status_led_config.port, status_led_config.pin).output); + CHECK(hal::host::Board::mcu().emergency_stops == 1); + } +} +} // namespace micras::test diff --git a/tests/hal/test_spi.cpp b/tests/hal/test_spi.cpp new file mode 100644 index 0000000..b5d4584 --- /dev/null +++ b/tests/hal/test_spi.cpp @@ -0,0 +1,208 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/host/spi_device.hpp" +#include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" + +namespace micras::test { +namespace { +using hal::host::Board; +using hal::host::SpiDevice; + +class RecordingDevice : public SpiDevice { +public: + RecordingDevice(Mode mode, uint8_t offset) : SpiDevice{mode}, offset{offset} { } + + void select() override { this->log.emplace_back("select"); } + + void exchange(std::span transmitted, std::span received) override { + this->log.push_back("exchange " + std::to_string(transmitted.size())); + + for (std::size_t index = 0; index < transmitted.size(); index++) { + this->received_bytes.push_back(transmitted[index]); + received[index] = static_cast(transmitted[index] + this->offset); + } + } + + void deselect() override { this->log.emplace_back("deselect"); } + + const std::vector& events() const { return this->log; } + + const std::vector& bytes() const { return this->received_bytes; } + +private: + uint8_t offset; + std::vector log; + std::vector received_bytes; +}; + +void attach(const hal::Spi::Config& config, SpiDevice& device) { + Board::spi_device(config.handle, config.cs_gpio.port, config.cs_gpio.pin, device); +} + +uint32_t wait_for_transfer(const hal::Spi& spi) { + const uint64_t start = hal::host::Clock::instance().now(); + + while (spi.get_transfer() == hal::Spi::Transfer::RUNNING) { + hal::Timer::get_counter(); + } + + return static_cast((hal::host::Clock::instance().now() - start) / (SystemCoreClock / 1000000)); +} +} // namespace + +TEST_SUITE("spi") { + TEST_CASE_FIXTURE(HostBoard, "routes each transfer to the device its chip select selects") { + RecordingDevice first_chip{SpiDevice::Mode::MODE_3, 0x10}; + RecordingDevice second_chip{SpiDevice::Mode::MODE_1, 0x20}; + attach(spi_config, first_chip); + attach(second_spi_config, second_chip); + hal::Spi first{spi_config}; + hal::Spi second{second_spi_config}; + + const std::array command{0x8F}; + const std::array frame{0x40, 0x01}; + std::array answer{}; + + REQUIRE(first.select_device()); + CHECK(first.transmit(command)); + first.unselect_device(); + REQUIRE(second.select_device()); + CHECK(second.transmit_receive(frame, answer)); + second.unselect_device(); + + CHECK(first_chip.bytes() == std::vector{0x8F}); + CHECK(second_chip.bytes() == std::vector{0x40, 0x01}); + CHECK(answer == std::array{0x60, 0x21}); + CHECK(Board::unbound().empty()); + } + + TEST_CASE_FIXTURE(HostBoard, "makes one transaction of the calls between select and unselect") { + RecordingDevice chip{SpiDevice::Mode::MODE_3, 1}; + attach(spi_config, chip); + hal::Spi spi{spi_config}; + + const std::array command{0x8F}; + std::array data{}; + + REQUIRE(spi.select_device()); + CHECK(spi.transmit(command)); + CHECK(spi.receive(data)); + spi.unselect_device(); + + CHECK(chip.events() == std::vector{"select", "exchange 1", "exchange 2", "deselect"}); + CHECK(data == std::array{1, 1}); + } + + TEST_CASE_FIXTURE(HostBoard, "reaches no device whose chip select is high") { + RecordingDevice chip{SpiDevice::Mode::MODE_3, 1}; + attach(spi_config, chip); + hal::Spi spi{spi_config}; + + std::array data{}; + CHECK(spi.receive(data)); + + CHECK(chip.events().empty()); + CHECK(data == std::array{0xFF, 0xFF}); + } + + TEST_CASE_FIXTURE(HostBoard, "answers all ones where no device is attached") { + hal::Spi spi{spi_config}; + + std::array data{}; + REQUIRE(spi.select_device()); + CHECK(spi.receive(data)); + spi.unselect_device(); + + CHECK(data == std::array{0xFF, 0xFF}); + CHECK(Board::unbound() == std::vector{"SPI_CSn", "hspi3 SPI_CSn"}); + } + + TEST_CASE_FIXTURE(HostBoard, "answers all ones in a mode the device does not answer in") { + RecordingDevice chip{SpiDevice::Mode::MODE_0, 1}; + attach(spi_config, chip); + hal::Spi spi{spi_config}; + + const std::array transmitted{0x8F, 0x00}; + std::array received{}; + REQUIRE(spi.select_device()); + CHECK(spi.transmit_receive(transmitted, received)); + spi.unselect_device(); + + CHECK(chip.events() == std::vector{"select", "deselect"}); + CHECK(received == std::array{0xFF, 0xFF}); + } + + TEST_CASE_FIXTURE(HostBoard, "keeps the device selected while a transfer runs") { + RecordingDevice chip{SpiDevice::Mode::MODE_3, 1}; + attach(spi_config, chip); + hal::Spi spi{spi_config}; + + std::array transmitted{}; + std::array received{}; + const hal::host::GpioPort& chip_select = Board::gpio(spi_config.cs_gpio.port, spi_config.cs_gpio.pin); + + REQUIRE(spi.start_transfer(transmitted, received)); + + CHECK(spi.get_transfer() == hal::Spi::Transfer::RUNNING); + CHECK_FALSE(chip_select.output); + CHECK(chip.events() == std::vector{"select", "exchange 17"}); + } + + TEST_CASE_FIXTURE(HostBoard, "completes a transfer when its last bit is out") { + RecordingDevice chip{SpiDevice::Mode::MODE_3, 1}; + attach(spi_config, chip); + hal::Spi spi{spi_config}; + + std::array transmitted{}; + std::array received{}; + const hal::host::GpioPort& chip_select = Board::gpio(spi_config.cs_gpio.port, spi_config.cs_gpio.pin); + + REQUIRE(spi.start_transfer(transmitted, received)); + const uint32_t elapsed = wait_for_transfer(spi); + + CHECK(spi.get_transfer() == hal::Spi::Transfer::COMPLETE); + CHECK(chip_select.output); + CHECK(chip.events() == std::vector{"select", "exchange 17", "deselect"}); + CHECK(received.back() == 1); + CHECK(elapsed >= 35); + CHECK(elapsed <= 36); + } + + TEST_CASE_FIXTURE(HostBoard, "waits for the bus before selecting another device") { + RecordingDevice first_chip{SpiDevice::Mode::MODE_3, 1}; + RecordingDevice second_chip{SpiDevice::Mode::MODE_1, 1}; + attach(spi_config, first_chip); + attach(second_spi_config, second_chip); + hal::Spi first{spi_config}; + hal::Spi second{second_spi_config}; + + std::array transmitted{}; + std::array received{}; + const uint64_t start = hal::host::Clock::instance().now(); + + REQUIRE(first.start_transfer(transmitted, received)); + REQUIRE(second.select_device()); + const uint64_t waited = hal::host::Clock::instance().now() - start; + second.unselect_device(); + + CHECK(first.get_transfer() == hal::Spi::Transfer::COMPLETE); + CHECK(first_chip.events().back() == "deselect"); + CHECK(waited >= 35U * (SystemCoreClock / 1000000)); + } +} +} // namespace micras::test diff --git a/tests/hal/test_timers.cpp b/tests/hal/test_timers.cpp new file mode 100644 index 0000000..2ffe027 --- /dev/null +++ b/tests/hal/test_timers.cpp @@ -0,0 +1,93 @@ +/** + * @file + */ + +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/encoder.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/pwm_dma.hpp" + +namespace micras::test { +TEST_SUITE("timers") { + TEST_CASE_FIXTURE(HostBoard, "runs a PWM channel at the frequency its registers give") { + const hal::Pwm pwm{pwm_config}; + + CHECK(pwm.was_initialized()); + CHECK(pwm.get_frequency() == doctest::Approx(1000.0F)); + CHECK(hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel).frequency == doctest::Approx(1000.0F)); + } + + TEST_CASE_FIXTURE(HostBoard, "reports the duty cycle the compare register gives") { + hal::Pwm pwm{pwm_config}; + const hal::host::PwmPort& port = hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel); + + pwm.set_duty_cycle(25.0F); + CHECK(port.duty_cycle == doctest::Approx(25.0F)); + CHECK(port.touched); + + pwm.set_duty_cycle(150.0F); + CHECK(port.duty_cycle == doctest::Approx(100.0F)); + + pwm.set_duty_cycle(-10.0F); + CHECK(port.duty_cycle == doctest::Approx(0.0F)); + } + + TEST_CASE_FIXTURE(HostBoard, "reports the active fraction of an inverted channel") { + hal::Pwm::Config config = pwm_config; + config.inverted = true; + hal::Pwm pwm{config}; + + pwm.set_duty_cycle(30.0F); + + CHECK(hal::host::Board::pwm(config.handle, config.timer_channel).duty_cycle == doctest::Approx(30.0F)); + CHECK((htim15.Instance->CCER & TIM_CCER_CC1P) != 0); + } + + TEST_CASE_FIXTURE(HostBoard, "changes the frequency through the autoreload register") { + hal::Pwm pwm{pwm_config}; + + pwm.set_frequency(2000); + + CHECK(htim15.Instance->ARR == 499); + CHECK(pwm.get_frequency() == doctest::Approx(2000.0F)); + CHECK(hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel).frequency == doctest::Approx(2000.0F)); + } + + TEST_CASE_FIXTURE(HostBoard, "keeps a DMA transfer of compare values busy for one period per value") { + hal::PwmDma pwm_dma{pwm_dma_config}; + std::array compares{}; + compares.fill(pwm_dma.get_compare(50.0F)); + + CHECK(pwm_dma.was_initialized()); + CHECK(compares.front() == 125); + + pwm_dma.start_dma(compares); + CHECK(pwm_dma.is_busy()); + CHECK(hal::host::Board::pwm_dma(pwm_dma_config.handle, pwm_dma_config.timer_channel).compares.size() == 8); + + wait_us(7); + CHECK(pwm_dma.is_busy()); + + wait_us(2); + CHECK_FALSE(pwm_dma.is_busy()); + } + + TEST_CASE_FIXTURE(HostBoard, "counts encoder pulses from where the encoder started") { + hal::host::EncoderPort& port = hal::host::Board::encoder(encoder_config.handle); + port.count = 1000; + + const hal::Encoder encoder{encoder_config}; + CHECK(encoder.was_initialized()); + CHECK(encoder.get_counter() == 0); + + port.count = 900; + CHECK(encoder.get_counter() == -100); + } +} +} // namespace micras::test diff --git a/tests/hal/test_uart.cpp b/tests/hal/test_uart.cpp new file mode 100644 index 0000000..678867c --- /dev/null +++ b/tests/hal/test_uart.cpp @@ -0,0 +1,62 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/uart_dma.hpp" + +namespace micras::test { +TEST_SUITE("uart") { + TEST_CASE_FIXTURE(HostBoard, "reads what arrived through the circular receive buffer") { + hal::UartDma uart{uart_config}; + std::array ring{}; + REQUIRE(uart.start_rx(ring)); + CHECK(uart.was_initialized()); + + hal::host::UartPort& port = hal::host::Board::uart(uart_config.handle); + std::array received{}; + + for (uint8_t byte = 1; byte <= 6; byte++) { + port.receive(byte); + } + + CHECK(uart.read(received) == 6); + CHECK(received == std::array{1, 2, 3, 4, 5, 6, 0, 0}); + + for (uint8_t byte = 7; byte <= 11; byte++) { + port.receive(byte); + } + + CHECK(uart.read(std::span{received}.first(3)) == 3); + CHECK(received == std::array{7, 8, 9, 4, 5, 6, 0, 0}); + + received = {}; + CHECK(uart.read(received) == 2); + CHECK(received == std::array{10, 11, 0, 0, 0, 0, 0, 0}); + CHECK(uart.read(received) == 0); + } + + TEST_CASE_FIXTURE(HostBoard, "hands the bytes to transmit to the port and refuses more until they leave") { + hal::UartDma uart{uart_config}; + const std::array message{0x10, 0x20, 0x30}; + + REQUIRE(uart.start_tx(message)); + CHECK(uart.is_transmitting()); + CHECK_FALSE(uart.start_tx(message)); + + hal::host::UartPort& port = hal::host::Board::uart(uart_config.handle); + CHECK(port.tx == std::deque{0x10, 0x20, 0x30}); + + port.tx.clear(); + CHECK_FALSE(uart.is_transmitting()); + } +} +} // namespace micras::test diff --git a/tests/models/test_as5047u_model.cpp b/tests/models/test_as5047u_model.cpp index 17415a7..fa85660 100644 --- a/tests/models/test_as5047u_model.cpp +++ b/tests/models/test_as5047u_model.cpp @@ -5,7 +5,7 @@ #include #include -#include +#include #include "micras/models/as5047u_model.hpp" @@ -62,158 +62,163 @@ void write_register(As5047uModel& chip, uint16_t address, uint16_t data) { transfer(chip, data_frame(data)); } -TEST(As5047uModel, AnswersInSpiMode1) { - const As5047uModel chip; - - EXPECT_EQ(chip.mode(), hal::host::SpiDevice::Mode::MODE_1); -} +} // namespace -TEST(As5047uModel, ComputesTheFrameCrc) { - EXPECT_EQ(As5047uModel::crc(0x00, 0x00), 0xF1); - EXPECT_EQ(As5047uModel::crc(0x40, 0x01), 0x06); - EXPECT_EQ(As5047uModel::crc(0x40, 0x1A), 0x04); - EXPECT_EQ(As5047uModel::crc(0x00, 0x80), 0xD7); -} +TEST_SUITE("as5047u_model") { + TEST_CASE("answers in SPI mode 1") { + const As5047uModel chip; -TEST(As5047uModel, PowersOnWithZeroedRegistersAndAnEmptyAnswer) { - As5047uModel chip; + CHECK(chip.mode() == hal::host::SpiDevice::Mode::MODE_1); + } - for (const uint16_t address : - {As5047uModel::errfl_address, As5047uModel::disable_address, As5047uModel::zposm_address, - As5047uModel::zposl_address, As5047uModel::settings1_address, As5047uModel::settings2_address, - As5047uModel::settings3_address, As5047uModel::ecc_address}) { - EXPECT_EQ(chip.peek(address), 0) << "register " << address; + TEST_CASE("computes the frame CRC") { + CHECK(As5047uModel::crc(0x00, 0x00) == 0xF1); + CHECK(As5047uModel::crc(0x40, 0x01) == 0x06); + CHECK(As5047uModel::crc(0x40, 0x1A) == 0x04); + CHECK(As5047uModel::crc(0x00, 0x80) == 0xD7); } - EXPECT_EQ(transfer(chip, read_command(As5047uModel::nop_address)), 0x0000F1U); -} + TEST_CASE("powers on with zeroed registers and an empty answer") { + As5047uModel chip; -TEST(As5047uModel, AnswersEachFrameInTheNextOne) { - As5047uModel chip; - write_register(chip, As5047uModel::settings3_address, 0x80); - write_register(chip, As5047uModel::zposm_address, 0x12); + for (const uint16_t address : + {As5047uModel::errfl_address, As5047uModel::disable_address, As5047uModel::zposm_address, + As5047uModel::zposl_address, As5047uModel::settings1_address, As5047uModel::settings2_address, + As5047uModel::settings3_address, As5047uModel::ecc_address}) { + INFO("register " << address); + CHECK(chip.peek(address) == 0); + } - transfer(chip, read_command(As5047uModel::settings3_address)); - const uint32_t settings3 = transfer(chip, read_command(As5047uModel::zposm_address)); - const uint32_t zposm = transfer(chip, read_command(As5047uModel::nop_address)); + CHECK(transfer(chip, read_command(As5047uModel::nop_address)) == 0x0000F1U); + } - EXPECT_EQ(data_of(settings3), 0x80); - EXPECT_EQ(data_of(zposm), 0x12); - EXPECT_TRUE(valid(settings3)); - EXPECT_TRUE(valid(zposm)); -} + TEST_CASE("answers each frame in the next one") { + As5047uModel chip; + write_register(chip, As5047uModel::settings3_address, 0x80); + write_register(chip, As5047uModel::zposm_address, 0x12); -TEST(As5047uModel, WritesWithACommandAndADataFrame) { - As5047uModel chip; - write_register(chip, As5047uModel::settings2_address, 0x04); + transfer(chip, read_command(As5047uModel::settings3_address)); + const uint32_t settings3 = transfer(chip, read_command(As5047uModel::zposm_address)); + const uint32_t zposm = transfer(chip, read_command(As5047uModel::nop_address)); - transfer(chip, write_command(As5047uModel::settings2_address)); - const uint32_t old_content = transfer(chip, data_frame(0x21)); - const uint32_t new_content = transfer(chip, read_command(As5047uModel::nop_address)); + CHECK(data_of(settings3) == 0x80); + CHECK(data_of(zposm) == 0x12); + CHECK(valid(settings3)); + CHECK(valid(zposm)); + } - EXPECT_EQ(data_of(old_content), 0x04); - EXPECT_EQ(data_of(new_content), 0x21); - EXPECT_EQ(chip.peek(As5047uModel::settings2_address), 0x21); -} + TEST_CASE("writes with a command and a data frame") { + As5047uModel chip; + write_register(chip, As5047uModel::settings2_address, 0x04); -TEST(As5047uModel, HoldsEveryVolatileRegister) { - As5047uModel chip; - uint16_t value = 0x11; - - for (const uint16_t address : - {As5047uModel::disable_address, As5047uModel::zposm_address, As5047uModel::zposl_address, - As5047uModel::settings1_address, As5047uModel::settings2_address, As5047uModel::settings3_address, - As5047uModel::ecc_address}) { - write_register(chip, address, value); - EXPECT_EQ(read_register(chip, address), value) << "register " << address; - value += 0x11; - } -} + transfer(chip, write_command(As5047uModel::settings2_address)); + const uint32_t old_content = transfer(chip, data_frame(0x21)); + const uint32_t new_content = transfer(chip, read_command(As5047uModel::nop_address)); -TEST(As5047uModel, IgnoresWritesToOtherAddresses) { - As5047uModel chip; + CHECK(data_of(old_content) == 0x04); + CHECK(data_of(new_content) == 0x21); + CHECK(chip.peek(As5047uModel::settings2_address) == 0x21); + } - write_register(chip, As5047uModel::errfl_address, 0x7F); - write_register(chip, 0x3FFF, 0x7F); + TEST_CASE("holds every volatile register") { + As5047uModel chip; + uint16_t value = 0x11; + + for (const uint16_t address : + {As5047uModel::disable_address, As5047uModel::zposm_address, As5047uModel::zposl_address, + As5047uModel::settings1_address, As5047uModel::settings2_address, As5047uModel::settings3_address, + As5047uModel::ecc_address}) { + write_register(chip, address, value); + INFO("register " << address); + CHECK(read_register(chip, address) == value); + value += 0x11; + } + } - EXPECT_EQ(chip.peek(As5047uModel::errfl_address), 0); - EXPECT_EQ(read_register(chip, 0x3FFF), 0); -} + TEST_CASE("ignores writes to other addresses") { + As5047uModel chip; -TEST(As5047uModel, RejectsAFrameWithAWrongCrc) { - As5047uModel chip; - Frame command = write_command(As5047uModel::settings3_address); - std::get<2>(command) ^= 0x01; + write_register(chip, As5047uModel::errfl_address, 0x7F); + write_register(chip, 0x3FFF, 0x7F); - transfer(chip, command); - const uint32_t after_error = transfer(chip, data_frame(0x80)); + CHECK(chip.peek(As5047uModel::errfl_address) == 0); + CHECK(read_register(chip, 0x3FFF) == 0); + } - EXPECT_EQ(chip.peek(As5047uModel::settings3_address), 0); - EXPECT_EQ(chip.peek(As5047uModel::errfl_address), As5047uModel::crc_error); - EXPECT_NE(after_error & error_flag, 0U); - EXPECT_TRUE(valid(after_error)); -} + TEST_CASE("rejects a frame with a wrong CRC") { + As5047uModel chip; + Frame command = write_command(As5047uModel::settings3_address); + std::get<2>(command) ^= 0x01; -TEST(As5047uModel, DropsAWriteWhoseDataFrameHasAWrongCrc) { - As5047uModel chip; - Frame data = data_frame(0x80); - std::get<2>(data) ^= 0x80; + transfer(chip, command); + const uint32_t after_error = transfer(chip, data_frame(0x80)); - transfer(chip, write_command(As5047uModel::settings3_address)); - transfer(chip, data); - transfer(chip, data_frame(0x80)); + CHECK(chip.peek(As5047uModel::settings3_address) == 0); + CHECK(chip.peek(As5047uModel::errfl_address) == As5047uModel::crc_error); + CHECK((after_error & error_flag) != 0U); + CHECK(valid(after_error)); + } - EXPECT_EQ(chip.peek(As5047uModel::settings3_address), 0); - EXPECT_EQ(chip.peek(As5047uModel::errfl_address), As5047uModel::crc_error); -} + TEST_CASE("drops a write whose data frame has a wrong CRC") { + As5047uModel chip; + Frame data = data_frame(0x80); + std::get<2>(data) ^= 0x80; -TEST(As5047uModel, ClearsTheErrorFlagsWhenErrflIsRead) { - As5047uModel chip; - Frame command = read_command(As5047uModel::settings3_address); - std::get<0>(command) ^= 0x01; - transfer(chip, command); + transfer(chip, write_command(As5047uModel::settings3_address)); + transfer(chip, data); + transfer(chip, data_frame(0x80)); - transfer(chip, read_command(As5047uModel::errfl_address)); - const uint32_t errfl = transfer(chip, read_command(As5047uModel::nop_address)); - const uint32_t after = transfer(chip, read_command(As5047uModel::nop_address)); + CHECK(chip.peek(As5047uModel::settings3_address) == 0); + CHECK(chip.peek(As5047uModel::errfl_address) == As5047uModel::crc_error); + } - EXPECT_EQ(data_of(errfl), As5047uModel::crc_error); - EXPECT_NE(errfl & error_flag, 0U); - EXPECT_EQ(after & error_flag, 0U); - EXPECT_EQ(chip.peek(As5047uModel::errfl_address), 0); -} + TEST_CASE("clears the error flags when ERRFL is read") { + As5047uModel chip; + Frame command = read_command(As5047uModel::settings3_address); + std::get<0>(command) ^= 0x01; + transfer(chip, command); -TEST(As5047uModel, FlagsAFrameCutShortByTheChipSelect) { - As5047uModel chip; - const Frame command = write_command(As5047uModel::settings3_address); - const std::array partial{std::get<0>(command), std::get<1>(command)}; - std::array received{}; + transfer(chip, read_command(As5047uModel::errfl_address)); + const uint32_t errfl = transfer(chip, read_command(As5047uModel::nop_address)); + const uint32_t after = transfer(chip, read_command(As5047uModel::nop_address)); - chip.select(); - chip.exchange(partial, received); - chip.deselect(); - transfer(chip, data_frame(0x80)); + CHECK(data_of(errfl) == As5047uModel::crc_error); + CHECK((errfl & error_flag) != 0U); + CHECK((after & error_flag) == 0U); + CHECK(chip.peek(As5047uModel::errfl_address) == 0); + } - EXPECT_EQ(chip.peek(As5047uModel::settings3_address), 0); - EXPECT_EQ(chip.peek(As5047uModel::errfl_address), As5047uModel::framing_error); -} + TEST_CASE("flags a frame cut short by the chip select") { + As5047uModel chip; + const Frame command = write_command(As5047uModel::settings3_address); + const std::array partial{std::get<0>(command), std::get<1>(command)}; + std::array received{}; -TEST(As5047uModel, TakesAFrameSplitAcrossExchanges) { - As5047uModel chip; - const Frame command = write_command(As5047uModel::zposl_address); - const std::array first{std::get<0>(command)}; - const std::array rest{std::get<1>(command), std::get<2>(command)}; - std::array first_received{}; - std::array rest_received{}; + chip.select(); + chip.exchange(partial, received); + chip.deselect(); + transfer(chip, data_frame(0x80)); - chip.select(); - chip.exchange(first, first_received); - chip.exchange(rest, rest_received); - chip.deselect(); - transfer(chip, data_frame(0x3F)); + CHECK(chip.peek(As5047uModel::settings3_address) == 0); + CHECK(chip.peek(As5047uModel::errfl_address) == As5047uModel::framing_error); + } - EXPECT_EQ(chip.peek(As5047uModel::zposl_address), 0x3F); - EXPECT_EQ(chip.peek(As5047uModel::errfl_address), 0); + TEST_CASE("takes a frame split across exchanges") { + As5047uModel chip; + const Frame command = write_command(As5047uModel::zposl_address); + const std::array first{std::get<0>(command)}; + const std::array rest{std::get<1>(command), std::get<2>(command)}; + std::array first_received{}; + std::array rest_received{}; + + chip.select(); + chip.exchange(first, first_received); + chip.exchange(rest, rest_received); + chip.deselect(); + transfer(chip, data_frame(0x3F)); + + CHECK(chip.peek(As5047uModel::zposl_address) == 0x3F); + CHECK(chip.peek(As5047uModel::errfl_address) == 0); + } } -} // namespace } // namespace micras::models diff --git a/tests/models/test_lsm6dsv_model.cpp b/tests/models/test_lsm6dsv_model.cpp index 2ca6fad..f12ed47 100644 --- a/tests/models/test_lsm6dsv_model.cpp +++ b/tests/models/test_lsm6dsv_model.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include "lsm6dsv_reg.h" #include "micras/models/lsm6dsv_model.hpp" @@ -50,278 +50,282 @@ std::vector read_burst(Lsm6dsvModel& chip) { return transact(chip, command); } -TEST(Lsm6dsvModel, AnswersInSpiMode3) { - const Lsm6dsvModel chip; +} // namespace - EXPECT_EQ(chip.mode(), hal::host::SpiDevice::Mode::MODE_3); -} +TEST_SUITE("lsm6dsv_model") { + TEST_CASE("answers in SPI mode 3") { + const Lsm6dsvModel chip; -TEST(Lsm6dsvModel, PowersOnWithTheDatasheetDefaults) { - const Lsm6dsvModel chip; + CHECK(chip.mode() == hal::host::SpiDevice::Mode::MODE_3); + } - for (uint8_t address = 0; address < Lsm6dsvModel::register_count; address++) { - uint8_t expected = 0x00; + TEST_CASE("powers on with the datasheet defaults") { + const Lsm6dsvModel chip; - if (address == LSM6DSV_WHO_AM_I) { - expected = LSM6DSV_ID; - } else if (address == LSM6DSV_PIN_CTRL) { - expected = 0x23; - } else if (address == LSM6DSV_CTRL3) { - expected = 0x44; - } + for (uint8_t address = 0; address < Lsm6dsvModel::register_count; address++) { + uint8_t expected = 0x00; - EXPECT_EQ(chip.peek(address), expected) << "register " << static_cast(address); - } -} + if (address == LSM6DSV_WHO_AM_I) { + expected = LSM6DSV_ID; + } else if (address == LSM6DSV_PIN_CTRL) { + expected = 0x23; + } else if (address == LSM6DSV_CTRL3) { + expected = 0x44; + } -TEST(Lsm6dsvModel, AnswersWhoAmIWithTheDeviceId) { - Lsm6dsvModel chip; + INFO("register " << static_cast(address)); + CHECK(chip.peek(address) == expected); + } + } - const std::vector received = transact(chip, {LSM6DSV_WHO_AM_I | read_flag, 0xFF}); + TEST_CASE("answers WHO_AM_I with the device id") { + Lsm6dsvModel chip; - EXPECT_EQ(received.at(0), 0x00); - EXPECT_EQ(received.at(1), LSM6DSV_ID); -} + const std::vector received = transact(chip, {LSM6DSV_WHO_AM_I | read_flag, 0xFF}); -TEST(Lsm6dsvModel, AdvancesTheAddressAfterEveryByte) { - Lsm6dsvModel chip; + CHECK(received.at(0) == 0x00); + CHECK(received.at(1) == LSM6DSV_ID); + } - transact(chip, {LSM6DSV_CTRL1, 0x16, 0x1C}); - const std::vector received = transact(chip, {LSM6DSV_CTRL1 | read_flag, 0, 0}); + TEST_CASE("advances the address after every byte") { + Lsm6dsvModel chip; - EXPECT_EQ(received.at(1), 0x16); - EXPECT_EQ(received.at(2), 0x1C); -} + transact(chip, {LSM6DSV_CTRL1, 0x16, 0x1C}); + const std::vector received = transact(chip, {LSM6DSV_CTRL1 | read_flag, 0, 0}); -TEST(Lsm6dsvModel, KeepsOneTransactionAcrossSeveralExchanges) { - Lsm6dsvModel chip; - write(chip, LSM6DSV_CTRL2, 0x1C); + CHECK(received.at(1) == 0x16); + CHECK(received.at(2) == 0x1C); + } - const std::array command{LSM6DSV_CTRL1 | read_flag}; - std::array ignored{}; - const std::array dummy{}; - std::array received{}; + TEST_CASE("keeps one transaction across several exchanges") { + Lsm6dsvModel chip; + write(chip, LSM6DSV_CTRL2, 0x1C); - chip.select(); - chip.exchange(command, ignored); - chip.exchange(dummy, received); - chip.deselect(); + const std::array command{LSM6DSV_CTRL1 | read_flag}; + std::array ignored{}; + const std::array dummy{}; + std::array received{}; - EXPECT_EQ(std::get<0>(received), 0x00); - EXPECT_EQ(std::get<1>(received), 0x1C); -} + chip.select(); + chip.exchange(command, ignored); + chip.exchange(dummy, received); + chip.deselect(); -TEST(Lsm6dsvModel, StartsEveryTransactionWithACommand) { - Lsm6dsvModel chip; + CHECK(std::get<0>(received) == 0x00); + CHECK(std::get<1>(received) == 0x1C); + } - transact(chip, {LSM6DSV_CTRL1, 0x16}); - transact(chip, {LSM6DSV_CTRL2, 0x1C}); + TEST_CASE("starts every transaction with a command") { + Lsm6dsvModel chip; - EXPECT_EQ(chip.peek(LSM6DSV_CTRL1), 0x16); - EXPECT_EQ(chip.peek(LSM6DSV_CTRL2), 0x1C); -} + transact(chip, {LSM6DSV_CTRL1, 0x16}); + transact(chip, {LSM6DSV_CTRL2, 0x1C}); -TEST(Lsm6dsvModel, StaysOnOneRegisterWithoutAutoIncrement) { - Lsm6dsvModel chip; - write(chip, LSM6DSV_CTRL3, 0x40); + CHECK(chip.peek(LSM6DSV_CTRL1) == 0x16); + CHECK(chip.peek(LSM6DSV_CTRL2) == 0x1C); + } - transact(chip, {LSM6DSV_CTRL1, 0x16, 0x1C}); - const std::vector received = transact(chip, {LSM6DSV_CTRL1 | read_flag, 0, 0}); + TEST_CASE("stays on one register without auto increment") { + Lsm6dsvModel chip; + write(chip, LSM6DSV_CTRL3, 0x40); - EXPECT_EQ(received.at(1), 0x1C); - EXPECT_EQ(received.at(2), 0x1C); - EXPECT_EQ(chip.peek(LSM6DSV_CTRL2), 0x00); -} + transact(chip, {LSM6DSV_CTRL1, 0x16, 0x1C}); + const std::vector received = transact(chip, {LSM6DSV_CTRL1 | read_flag, 0, 0}); -TEST(Lsm6dsvModel, IgnoresWritesToReadOnlyAndReservedRegisters) { - Lsm6dsvModel chip; + CHECK(received.at(1) == 0x1C); + CHECK(received.at(2) == 0x1C); + CHECK(chip.peek(LSM6DSV_CTRL2) == 0x00); + } - write(chip, LSM6DSV_WHO_AM_I, 0x00); - write(chip, LSM6DSV_STATUS_REG, 0xFF); - write(chip, LSM6DSV_OUTX_L_G, 0x12); - write(chip, 0x04, 0x34); + TEST_CASE("ignores writes to read only and reserved registers") { + Lsm6dsvModel chip; - EXPECT_EQ(chip.peek(LSM6DSV_WHO_AM_I), LSM6DSV_ID); - EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_OUTX_L_G), 0x00); - EXPECT_EQ(chip.peek(0x04), 0x00); -} + write(chip, LSM6DSV_WHO_AM_I, 0x00); + write(chip, LSM6DSV_STATUS_REG, 0xFF); + write(chip, LSM6DSV_OUTX_L_G, 0x12); + write(chip, 0x04, 0x34); -TEST(Lsm6dsvModel, RestoresTheDefaultsOnASoftwarePowerOnReset) { - Lsm6dsvModel chip; - configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); - chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); - - transact(chip, {LSM6DSV_FUNC_CFG_ACCESS, 0x04, 0x55}); - - EXPECT_EQ(chip.peek(LSM6DSV_FUNC_CFG_ACCESS), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_PIN_CTRL), 0x23); - EXPECT_EQ(chip.peek(LSM6DSV_CTRL1), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_CTRL2), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_CTRL3), 0x44); - EXPECT_EQ(chip.peek(LSM6DSV_CTRL6), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_OUTX_L_G), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_HAODR_CFG), 0x00); - EXPECT_EQ(read(chip, LSM6DSV_WHO_AM_I), LSM6DSV_ID); -} + CHECK(chip.peek(LSM6DSV_WHO_AM_I) == LSM6DSV_ID); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x00); + CHECK(chip.peek(LSM6DSV_OUTX_L_G) == 0x00); + CHECK(chip.peek(0x04) == 0x00); + } -TEST(Lsm6dsvModel, RestoresTheDefaultsOnASoftwareReset) { - Lsm6dsvModel chip; - configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + TEST_CASE("restores the defaults on a software power on reset") { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + + transact(chip, {LSM6DSV_FUNC_CFG_ACCESS, 0x04, 0x55}); + + CHECK(chip.peek(LSM6DSV_FUNC_CFG_ACCESS) == 0x00); + CHECK(chip.peek(LSM6DSV_PIN_CTRL) == 0x23); + CHECK(chip.peek(LSM6DSV_CTRL1) == 0x00); + CHECK(chip.peek(LSM6DSV_CTRL2) == 0x00); + CHECK(chip.peek(LSM6DSV_CTRL3) == 0x44); + CHECK(chip.peek(LSM6DSV_CTRL6) == 0x00); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x00); + CHECK(chip.peek(LSM6DSV_OUTX_L_G) == 0x00); + CHECK(chip.peek(LSM6DSV_HAODR_CFG) == 0x00); + CHECK(read(chip, LSM6DSV_WHO_AM_I) == LSM6DSV_ID); + } - write(chip, LSM6DSV_CTRL3, 0x45); + TEST_CASE("restores the defaults on a software reset") { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); - EXPECT_EQ(chip.peek(LSM6DSV_CTRL3), 0x44); - EXPECT_EQ(chip.peek(LSM6DSV_CTRL2), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_CTRL6), 0x00); -} + write(chip, LSM6DSV_CTRL3, 0x45); -TEST(Lsm6dsvModel, HidesTheMainPageWhileAnotherIsSelected) { - Lsm6dsvModel chip; + CHECK(chip.peek(LSM6DSV_CTRL3) == 0x44); + CHECK(chip.peek(LSM6DSV_CTRL2) == 0x00); + CHECK(chip.peek(LSM6DSV_CTRL6) == 0x00); + } - write(chip, LSM6DSV_FUNC_CFG_ACCESS, 0x80); - EXPECT_EQ(read(chip, LSM6DSV_WHO_AM_I), 0x00); - write(chip, LSM6DSV_CTRL1, 0x1C); + TEST_CASE("hides the main page while another is selected") { + Lsm6dsvModel chip; - write(chip, LSM6DSV_FUNC_CFG_ACCESS, 0x00); - EXPECT_EQ(read(chip, LSM6DSV_WHO_AM_I), LSM6DSV_ID); - EXPECT_EQ(chip.peek(LSM6DSV_CTRL1), 0x00); -} + write(chip, LSM6DSV_FUNC_CFG_ACCESS, 0x80); + CHECK(read(chip, LSM6DSV_WHO_AM_I) == 0x00); + write(chip, LSM6DSV_CTRL1, 0x1C); -TEST(Lsm6dsvModel, IgnoresSamplesWhileTheSensorsAreOff) { - Lsm6dsvModel chip; + write(chip, LSM6DSV_FUNC_CFG_ACCESS, 0x00); + CHECK(read(chip, LSM6DSV_WHO_AM_I) == LSM6DSV_ID); + CHECK(chip.peek(LSM6DSV_CTRL1) == 0x00); + } - chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + TEST_CASE("ignores samples while the sensors are off") { + Lsm6dsvModel chip; - EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_OUTX_L_G), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_OUTX_L_A), 0x00); -} + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); -TEST(Lsm6dsvModel, SetsTheDataReadyBitOfEachSensorThatIsOn) { - Lsm6dsvModel chip; - write(chip, LSM6DSV_CTRL2, 0x1C); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x00); + CHECK(chip.peek(LSM6DSV_OUTX_L_G) == 0x00); + CHECK(chip.peek(LSM6DSV_OUTX_L_A) == 0x00); + } - chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + TEST_CASE("sets the data ready bit of each sensor that is on") { + Lsm6dsvModel chip; + write(chip, LSM6DSV_CTRL2, 0x1C); - EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x02); - EXPECT_EQ(chip.peek(LSM6DSV_OUTX_L_A), 0x00); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); - write(chip, LSM6DSV_CTRL1, 0x1C); - chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x02); + CHECK(chip.peek(LSM6DSV_OUTX_L_A) == 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x03); -} + write(chip, LSM6DSV_CTRL1, 0x1C); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); -TEST(Lsm6dsvModel, ClearsTheDataReadyBitsWithTheBurstThatReadsTheSample) { - Lsm6dsvModel chip; - configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); - chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x03); + } - const std::vector first = read_burst(chip); - const std::vector second = read_burst(chip); + TEST_CASE("clears the data ready bits with the burst that reads the sample") { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); - EXPECT_EQ(first.at(1), 0x03); - EXPECT_EQ(second.at(1), 0x00); - EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x00); - EXPECT_EQ(word(second, 5), word(first, 5)); -} + const std::vector first = read_burst(chip); + const std::vector second = read_burst(chip); -TEST(Lsm6dsvModel, ClearsEachDataReadyBitWithAHighByteOfItsSensor) { - Lsm6dsvModel chip; - configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); - chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); + CHECK(first.at(1) == 0x03); + CHECK(second.at(1) == 0x00); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x00); + CHECK(word(second, 5) == word(first, 5)); + } - read(chip, LSM6DSV_OUTX_L_G); - EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x03); + TEST_CASE("clears each data ready bit with a high byte of its sensor") { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}); - read(chip, LSM6DSV_OUTY_H_G); - EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x01); + read(chip, LSM6DSV_OUTX_L_G); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x03); - read(chip, LSM6DSV_OUTZ_H_A); - EXPECT_EQ(chip.peek(LSM6DSV_STATUS_REG), 0x00); -} + read(chip, LSM6DSV_OUTY_H_G); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x01); -TEST(Lsm6dsvModel, EncodesTheSampleWithTheFullScaleInTheRegisters) { - Lsm6dsvModel chip; - configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); - chip.push_sample({1.0, -1.0, 0.0}, {9.80665, -9.80665, 0.0}); + read(chip, LSM6DSV_OUTZ_H_A); + CHECK(chip.peek(LSM6DSV_STATUS_REG) == 0x00); + } - const std::vector wide = read_burst(chip); + TEST_CASE("encodes the sample with the full scale in the registers") { + Lsm6dsvModel chip; + configure(chip, LSM6DSV_4000dps, LSM6DSV_8g); + chip.push_sample({1.0, -1.0, 0.0}, {9.80665, -9.80665, 0.0}); - EXPECT_EQ(word(wide, 5), 409); - EXPECT_EQ(word(wide, 7), -409); - EXPECT_EQ(word(wide, 9), 0); - EXPECT_EQ(word(wide, 11), 4098); - EXPECT_EQ(word(wide, 13), -4098); - EXPECT_EQ(word(wide, 15), 0); + const std::vector wide = read_burst(chip); - write(chip, LSM6DSV_CTRL6, LSM6DSV_125dps); - write(chip, LSM6DSV_CTRL8, LSM6DSV_2g); - chip.push_sample({1.0, 100.0, -100.0}, {9.80665, 100.0, -100.0}); + CHECK(word(wide, 5) == 409); + CHECK(word(wide, 7) == -409); + CHECK(word(wide, 9) == 0); + CHECK(word(wide, 11) == 4098); + CHECK(word(wide, 13) == -4098); + CHECK(word(wide, 15) == 0); - const std::vector narrow = read_burst(chip); + write(chip, LSM6DSV_CTRL6, LSM6DSV_125dps); + write(chip, LSM6DSV_CTRL8, LSM6DSV_2g); + chip.push_sample({1.0, 100.0, -100.0}, {9.80665, 100.0, -100.0}); - EXPECT_EQ(word(narrow, 5), 13096); - EXPECT_EQ(word(narrow, 7), 32767); - EXPECT_EQ(word(narrow, 9), -32768); - EXPECT_EQ(word(narrow, 11), 16393); - EXPECT_EQ(word(narrow, 13), 32767); - EXPECT_EQ(word(narrow, 15), -32768); -} + const std::vector narrow = read_burst(chip); -TEST(Lsm6dsvModel, GivesTheSensitivityOfEveryFullScale) { - Lsm6dsvModel chip; - constexpr double mdps{std::numbers::pi / 180000.0}; - constexpr double mg{0.00980665}; - - const std::array, 6> gyroscope{{ - {LSM6DSV_125dps, 4.375}, - {LSM6DSV_250dps, 8.75}, - {LSM6DSV_500dps, 17.5}, - {LSM6DSV_1000dps, 35.0}, - {LSM6DSV_2000dps, 70.0}, - {LSM6DSV_4000dps, 140.0}, - }}; - const std::array, 4> accelerometer{{ - {LSM6DSV_2g, 0.061}, - {LSM6DSV_4g, 0.122}, - {LSM6DSV_8g, 0.244}, - {LSM6DSV_16g, 0.488}, - }}; - - for (const auto& [code, sensitivity] : gyroscope) { - write(chip, LSM6DSV_CTRL6, code); - EXPECT_DOUBLE_EQ(chip.gyroscope_sensitivity(), sensitivity * mdps); + CHECK(word(narrow, 5) == 13096); + CHECK(word(narrow, 7) == 32767); + CHECK(word(narrow, 9) == -32768); + CHECK(word(narrow, 11) == 16393); + CHECK(word(narrow, 13) == 32767); + CHECK(word(narrow, 15) == -32768); } - for (const auto& [code, sensitivity] : accelerometer) { - write(chip, LSM6DSV_CTRL8, code); - EXPECT_DOUBLE_EQ(chip.accelerometer_sensitivity(), sensitivity * mg); + TEST_CASE("gives the sensitivity of every full scale") { + Lsm6dsvModel chip; + constexpr double mdps{std::numbers::pi / 180000.0}; + constexpr double mg{0.00980665}; + + const std::array, 6> gyroscope{{ + {LSM6DSV_125dps, 4.375}, + {LSM6DSV_250dps, 8.75}, + {LSM6DSV_500dps, 17.5}, + {LSM6DSV_1000dps, 35.0}, + {LSM6DSV_2000dps, 70.0}, + {LSM6DSV_4000dps, 140.0}, + }}; + const std::array, 4> accelerometer{{ + {LSM6DSV_2g, 0.061}, + {LSM6DSV_4g, 0.122}, + {LSM6DSV_8g, 0.244}, + {LSM6DSV_16g, 0.488}, + }}; + + for (const auto& [code, sensitivity] : gyroscope) { + write(chip, LSM6DSV_CTRL6, code); + CHECK(chip.gyroscope_sensitivity() == doctest::Approx(sensitivity * mdps)); + } + + for (const auto& [code, sensitivity] : accelerometer) { + write(chip, LSM6DSV_CTRL8, code); + CHECK(chip.accelerometer_sensitivity() == doctest::Approx(sensitivity * mg)); + } } -} -TEST(Lsm6dsvModel, DecodesTheOutputDataRates) { - Lsm6dsvModel chip; + TEST_CASE("decodes the output data rates") { + Lsm6dsvModel chip; - EXPECT_EQ(chip.gyroscope_data_rate(), 0.0); - EXPECT_EQ(chip.accelerometer_data_rate(), 0.0); + CHECK(chip.gyroscope_data_rate() == 0.0); + CHECK(chip.accelerometer_data_rate() == 0.0); - write(chip, LSM6DSV_CTRL2, 0x10 | (LSM6DSV_ODR_HA01_AT_8000Hz & 0x0F)); - write(chip, LSM6DSV_CTRL1, 0x10 | (LSM6DSV_ODR_HA01_AT_125Hz & 0x0F)); + write(chip, LSM6DSV_CTRL2, 0x10 | (LSM6DSV_ODR_HA01_AT_8000Hz & 0x0F)); + write(chip, LSM6DSV_CTRL1, 0x10 | (LSM6DSV_ODR_HA01_AT_125Hz & 0x0F)); - EXPECT_EQ(chip.gyroscope_data_rate(), 7680.0); + CHECK(chip.gyroscope_data_rate() == 7680.0); - write(chip, LSM6DSV_HAODR_CFG, 0x01); - EXPECT_EQ(chip.gyroscope_data_rate(), 8000.0); - EXPECT_EQ(chip.accelerometer_data_rate(), 125.0); + write(chip, LSM6DSV_HAODR_CFG, 0x01); + CHECK(chip.gyroscope_data_rate() == 8000.0); + CHECK(chip.accelerometer_data_rate() == 125.0); - write(chip, LSM6DSV_HAODR_CFG, 0x02); - EXPECT_EQ(chip.gyroscope_data_rate(), 6400.0); + write(chip, LSM6DSV_HAODR_CFG, 0x02); + CHECK(chip.gyroscope_data_rate() == 6400.0); - write(chip, LSM6DSV_CTRL1, LSM6DSV_ODR_AT_1Hz875); - EXPECT_EQ(chip.accelerometer_data_rate(), 1.875); + write(chip, LSM6DSV_CTRL1, LSM6DSV_ODR_AT_1Hz875); + CHECK(chip.accelerometer_data_rate() == 1.875); + } } -} // namespace } // namespace micras::models diff --git a/tests/proxy/test_fmac_filter.cpp b/tests/proxy/test_fmac_filter.cpp new file mode 100644 index 0000000..5d36d3c --- /dev/null +++ b/tests/proxy/test_fmac_filter.cpp @@ -0,0 +1,82 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/core/butterworth_filter.hpp" +#include "micras/proxy/fmac_filter.hpp" + +namespace micras::test { +namespace { +const core::ButterworthFilter::Config filter_config{ + .cutoff_frequency = 200.0F, + .sampling_frequency = 1000.0F, +}; + +std::vector direct_form(const std::vector& inputs) { + const core::ButterworthFilter::Coefficients coefficients = + core::ButterworthFilter::compute_coefficients(filter_config); + std::array input_history{}; + std::array output_history{}; + std::vector outputs; + + for (const double input : inputs) { + input_history = {input, input_history.at(0), input_history.at(1)}; + + double output = 0.0; + + for (std::size_t tap = 0; tap < input_history.size(); tap++) { + output += static_cast(coefficients.feed_forward.at(tap)) * input_history.at(tap); + } + + for (std::size_t tap = 0; tap < output_history.size(); tap++) { + output -= static_cast(coefficients.feedback.at(tap)) * output_history.at(tap); + } + + output_history = {output, output_history.at(0)}; + outputs.push_back(output); + } + + return outputs; +} +} // namespace + +TEST_SUITE("fmac_filter") { + TEST_CASE_FIXTURE(HostBoard, "filters as the Butterworth filter it was designed as") { + proxy::FmacFilter filter{{.fmac = fmac_config, .filter = filter_config}}; + REQUIRE(filter.was_initialized()); + + std::vector inputs; + + for (int sample = 0; sample < 300; sample++) { + inputs.push_back((sample < 150 ? 0.5 : -0.25) + 0.1 * std::sin(0.9 * sample)); + } + + const std::vector expected = direct_form(inputs); + + for (std::size_t sample = 0; sample < inputs.size(); sample++) { + CAPTURE(sample); + const double output = filter.update(static_cast(inputs.at(sample))); + + CHECK(std::abs(output - expected.at(sample)) < 2e-3); + CHECK(filter.get_last() == static_cast(output)); + } + } + + TEST_CASE_FIXTURE(HostBoard, "stays off for a filter whose coefficients q1.15 cannot hold") { + proxy::FmacFilter filter{ + {.fmac = fmac_config, .filter = {.cutoff_frequency = 50.0F, .sampling_frequency = 1000.0F}} + }; + + CHECK_FALSE(filter.was_initialized()); + CHECK(filter.update(0.5F) == 0.0F); + } +} +} // namespace micras::test diff --git a/tests/proxy/test_imu.cpp b/tests/proxy/test_imu.cpp new file mode 100644 index 0000000..07dae91 --- /dev/null +++ b/tests/proxy/test_imu.cpp @@ -0,0 +1,152 @@ +/** + * @file + */ + +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/models/lsm6dsv_model.hpp" +#include "micras/proxy/imu.hpp" + +namespace micras::test { +namespace { +const proxy::Imu::Config imu_config{ + .spi = spi_config, + .gyroscope_mode = LSM6DSV_GY_HIGH_ACCURACY_ODR_MD, + .accelerometer_mode = LSM6DSV_XL_HIGH_ACCURACY_ODR_MD, + .gyroscope_data_rate = LSM6DSV_ODR_HA01_AT_8000Hz, + .accelerometer_data_rate = LSM6DSV_ODR_HA01_AT_8000Hz, + .gyroscope_scale = LSM6DSV_4000dps, + .accelerometer_scale = LSM6DSV_8g, + .gyroscope_filter = LSM6DSV_GY_ULTRA_LIGHT, + .accelerometer_filter = LSM6DSV_XL_MEDIUM, +}; + +void attach(const proxy::Imu::Config& config, hal::host::SpiDevice& device) { + hal::host::Board::spi_device(config.spi.handle, config.spi.cs_gpio.port, config.spi.cs_gpio.pin, device); +} + +bool near(double value, double expected, double tolerance) { + return value >= expected - tolerance and value <= expected + tolerance; +} +} // namespace + +TEST_SUITE("imu") { + TEST_CASE_FIXTURE(HostBoard, "sets the chip up as its configuration says") { + models::Lsm6dsvModel chip; + attach(imu_config, chip); + + const proxy::Imu imu{imu_config}; + + CHECK(imu.was_initialized()); + CHECK(chip.gyroscope_data_rate() == 8000.0); + CHECK(chip.accelerometer_data_rate() == 8000.0); + CHECK((chip.peek(LSM6DSV_CTRL6) & 0x0FU) == LSM6DSV_4000dps); + CHECK((chip.peek(LSM6DSV_CTRL8) & 0x03U) == LSM6DSV_8g); + CHECK((chip.peek(LSM6DSV_CTRL3) & 0x40U) == 0x40); + CHECK((chip.peek(LSM6DSV_CTRL7) & 0x01U) == 0x01); + CHECK((chip.peek(LSM6DSV_CTRL9) & 0x08U) == 0x08); + } + + TEST_CASE_FIXTURE(HostBoard, "waits for the chip to start before configuring it") { + models::Lsm6dsvModel chip; + attach(imu_config, chip); + const uint64_t start = hal::host::Clock::instance().now(); + + const proxy::Imu imu{imu_config}; + const uint64_t elapsed_us = (hal::host::Clock::instance().now() - start) / (SystemCoreClock / 1000000); + + CHECK(imu.was_initialized()); + CHECK(elapsed_us >= 39900); + CHECK(elapsed_us < 41000); + } + + TEST_CASE_FIXTURE(HostBoard, "reads a sample one update after the update that asked for it") { + models::Lsm6dsvModel chip; + attach(imu_config, chip); + proxy::Imu imu{imu_config}; + + chip.push_sample({0.1, -0.2, 3.0}, {1.0, -2.0, 9.80665}); + imu.update(); + CHECK_FALSE(imu.is_new()); + + wait_us(125); + imu.update(); + + REQUIRE(imu.is_new()); + const double gyroscope = chip.gyroscope_sensitivity(); + const double accelerometer = chip.accelerometer_sensitivity(); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::X), 0.1, gyroscope)); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::Y), -0.2, gyroscope)); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::Z), 3.0, gyroscope)); + CHECK(near(imu.get_linear_acceleration(proxy::Imu::Axis::X), 1.0, accelerometer)); + CHECK(near(imu.get_linear_acceleration(proxy::Imu::Axis::Y), -2.0, accelerometer)); + CHECK(near(imu.get_linear_acceleration(proxy::Imu::Axis::Z), 9.80665, accelerometer)); + + wait_us(125); + imu.update(); + CHECK_FALSE(imu.is_new()); + } + + TEST_CASE_FIXTURE(HostBoard, "reads the same motion at another full scale") { + proxy::Imu::Config config = imu_config; + config.gyroscope_scale = LSM6DSV_250dps; + config.accelerometer_scale = LSM6DSV_2g; + models::Lsm6dsvModel chip; + attach(config, chip); + proxy::Imu imu{config}; + + chip.push_sample({0.5, 0.0, -1.5}, {0.0, 3.0, 9.80665}); + imu.update(); + wait_us(125); + imu.update(); + + REQUIRE(imu.is_new()); + const double gyroscope = chip.gyroscope_sensitivity(); + const double accelerometer = chip.accelerometer_sensitivity(); + CHECK((chip.peek(LSM6DSV_CTRL6) & 0x0FU) == LSM6DSV_250dps); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::X), 0.5, gyroscope)); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::Z), -1.5, gyroscope)); + CHECK(near(imu.get_linear_acceleration(proxy::Imu::Axis::Y), 3.0, accelerometer)); + CHECK(near(imu.get_linear_acceleration(proxy::Imu::Axis::Z), 9.80665, accelerometer)); + } + + TEST_CASE_FIXTURE(HostBoard, "saturates a motion beyond the full scale it was set to") { + proxy::Imu::Config config = imu_config; + config.gyroscope_scale = LSM6DSV_250dps; + models::Lsm6dsvModel chip; + attach(config, chip); + proxy::Imu imu{config}; + + chip.push_sample({10.0, 0.0, 0.0}, {0.0, 0.0, 9.80665}); + imu.update(); + wait_us(125); + imu.update(); + + REQUIRE(imu.is_new()); + CHECK(near(imu.get_angular_velocity(proxy::Imu::Axis::X), 32767 * chip.gyroscope_sensitivity(), 1e-3)); + CHECK(imu.get_angular_velocity(proxy::Imu::Axis::X) < 5.1F); + } + + TEST_CASE_FIXTURE(HostBoard, "fails without its chip") { + const proxy::Imu imu{imu_config}; + + CHECK_FALSE(imu.was_initialized()); + } + + TEST_CASE_FIXTURE(HostBoard, "fails in the wrong SPI mode") { + proxy::Imu::Config config = imu_config; + config.spi.clock_polarity = SPI_POLARITY_LOW; + models::Lsm6dsvModel chip; + attach(config, chip); + + const proxy::Imu imu{config}; + + CHECK_FALSE(imu.was_initialized()); + } +} +} // namespace micras::test diff --git a/tests/proxy/test_rotary_sensor.cpp b/tests/proxy/test_rotary_sensor.cpp new file mode 100644 index 0000000..f827862 --- /dev/null +++ b/tests/proxy/test_rotary_sensor.cpp @@ -0,0 +1,109 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "host_fixture.hpp" +#include "micras/hal/crc.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/models/as5047u_model.hpp" +#include "micras/proxy/rotary_sensor.hpp" + +namespace micras::test { +namespace { +const proxy::RotarySensor::Registers registers{ + .disable = {{.UVW_off = 1, .ABI_off = 0, .na = 0, .FILTER_disable = 0}}, + .zposm = {{.ZPOSM = 0}}, + .zposl = {{.ZPOSL = 0, .Dia1_en = 0, .Dia2_en = 0}}, + .settings1 = {{.K_max = 0, .K_min = 0, .Dia3_en = 0, .Dia4_en = 0}}, + .settings2 = {{ + .IWIDTH = 0, + .NOISESET = 0, + .DIR = 0, + .UVW_ABI = 0, + .DAECDIS = 0, + .ABI_DEC = 0, + .Data_select = 0, + .PWMon = 0, + }}, + .settings3 = {{.UVWPP = 0, .HYS = 0, .ABIRES = 0b100}}, + .ecc = {{.ECC_chsum = 0, .ECC_en = 0}}, +}; + +const proxy::RotarySensor::Config rotary_sensor_config{ + .spi = + { + .init_function = MX_SPI3_Init, + .handle = &hspi3, + .cs_gpio = spi_config.cs_gpio, + .timeout = 2, + .clock_polarity = SPI_POLARITY_LOW, + .clock_phase = SPI_PHASE_2EDGE, + }, + .encoder = encoder_config, + .crc = crc_config, + .registers = registers, +}; + +void attach(const proxy::RotarySensor::Config& config, hal::host::SpiDevice& device) { + hal::host::Board::spi_device(config.spi.handle, config.spi.cs_gpio.port, config.spi.cs_gpio.pin, device); +} +} // namespace + +TEST_SUITE("rotary_sensor") { + TEST_CASE_FIXTURE(HostBoard, "sets the chip up and reads its resolution back") { + models::As5047uModel chip; + attach(rotary_sensor_config, chip); + + const proxy::RotarySensor sensor{rotary_sensor_config}; + + CHECK(sensor.was_initialized()); + CHECK(sensor.get_resolution() == 16384); + CHECK(chip.peek(models::As5047uModel::settings3_address) == registers.settings3.raw); + CHECK(chip.peek(models::As5047uModel::disable_address) == registers.disable.raw); + CHECK(chip.peek(models::As5047uModel::errfl_address) == 0); + } + + TEST_CASE_FIXTURE(HostBoard, "reads the position the encoder counted") { + models::As5047uModel chip; + attach(rotary_sensor_config, chip); + const proxy::RotarySensor sensor{rotary_sensor_config}; + + hal::host::Board::encoder(rotary_sensor_config.encoder.handle).count = 4096; + + CHECK(sensor.get_position() == doctest::Approx(std::numbers::pi_v / 2.0F)); + } + + TEST_CASE_FIXTURE(HostBoard, "fails when the chip rejects the frames for their CRC") { + CRC_HandleTypeDef wrong_crc = hcrc; + wrong_crc.Init.InitValue = 0x00; + proxy::RotarySensor::Config config = rotary_sensor_config; + config.crc.handle = &wrong_crc; + models::As5047uModel chip; + attach(config, chip); + + const proxy::RotarySensor sensor{config}; + + CHECK_FALSE(sensor.was_initialized()); + CHECK(chip.peek(models::As5047uModel::settings3_address) == 0); + CHECK((chip.peek(models::As5047uModel::errfl_address) & models::As5047uModel::crc_error) != 0); + } + + TEST_CASE_FIXTURE(HostBoard, "computes the frame CRC as the CRC unit does") { + hal::Crc crc{crc_config}; + + for (const std::array bytes : std::vector>{ + {0x00, 0x00}, {0x40, 0x01}, {0x40, 0x1A}, {0x00, 0x80}, {0x3F, 0xFF}, {0xC0, 0x15} + }) { + CAPTURE(bytes); + CHECK(models::As5047uModel::crc(bytes.at(0), bytes.at(1)) == (crc.calculate(bytes) ^ 0xFFU)); + } + } +} +} // namespace micras::test diff --git a/tests/proxy/test_storage.cpp b/tests/proxy/test_storage.cpp index 0ef4ff6..b5d0e78 100644 --- a/tests/proxy/test_storage.cpp +++ b/tests/proxy/test_storage.cpp @@ -2,113 +2,141 @@ * @file */ -#include +#include +#include +#include + +#include "host_fixture.hpp" +#include "micras/core/serializable.hpp" +#include "micras/core/variable_pool.hpp" #include "micras/hal/flash.hpp" +#include "micras/hal/host/board.hpp" #include "micras/proxy/storage.hpp" -#include "test_host.hpp" -using namespace micras; +namespace micras::test { +namespace { +class Blob : public core::ISerializable { +public: + Blob() = default; -struct Blob : core::ISerializable { - std::vector data; + explicit Blob(std::vector data) : data{std::move(data)} { } + + std::vector serialize() const override { return this->data; } - std::vector serialize() const override { return data; } + void deserialize(const uint8_t* buffer, uint16_t size) override { + this->data.assign(buffer, std::next(buffer, size)); + } + + const std::vector& get() const { return this->data; } - void deserialize(const uint8_t* p, uint16_t n) override { data.assign(p, p + n); } +private: + std::vector data; }; -static const proxy::Storage::Config cfg{.start_sector = 0, .number_of_sectors = 1}; +const proxy::Storage::Config storage_config{.start_sector = 0, .number_of_sectors = 1}; -int main() { - // --- round trip --- - { +class StoredPool : public HostBoard { +public: + StoredPool() { core::TVariablePool<8> pool; - float kv = 3.25F; + float gain = 3.25F; uint8_t profile = 7; bool flag = true; float transient = 1.0F; - Blob maze; - maze.data = {9, 8, 7, 6, 5}; - pool.add("model/", "kv", kv, {.persist = true}); + Blob maze{{9, 8, 7, 6, 5}}; + + pool.add("model/", "kv", gain, {.persist = true}); pool.add("", "run_profile", profile, {.persist = true}); pool.add("", "flag", flag, {.persist = true}); pool.add("", "transient", transient, {.stream = true}); pool.add("", "maze", maze, {.persist = true}); - proxy::Storage s{cfg}; - CHECK(!s.is_valid()); - CHECK(s.restore(pool) == 0); - CHECK(s.save(pool)); - CHECK(s.is_valid()); + proxy::Storage storage{storage_config}; + CHECK_FALSE(storage.is_valid()); + CHECK(storage.restore(pool) == 0); + CHECK(storage.save(pool)); + CHECK(storage.is_valid()); } - { +}; +} // namespace + +TEST_SUITE("storage") { + TEST_CASE_FIXTURE(StoredPool, "restores every persistent variable and leaves the others alone") { core::TVariablePool<8> pool; - float kv = 0.0F; + float gain = 0.0F; uint8_t profile = 0; bool flag = false; float transient = 42.0F; Blob maze; - pool.add("model/", "kv", kv, {.persist = true}); + + pool.add("model/", "kv", gain, {.persist = true}); pool.add("", "run_profile", profile, {.persist = true}); pool.add("", "flag", flag, {.persist = true}); pool.add("", "transient", transient, {.stream = true}); pool.add("", "maze", maze, {.persist = true}); - proxy::Storage s{cfg}; - CHECK(s.is_valid()); - CHECK(s.restore(pool) == 4); - CHECK(kv == 3.25F && profile == 7 && flag == true); - CHECK((maze.data == std::vector{9, 8, 7, 6, 5})); + proxy::Storage storage{storage_config}; + CHECK(storage.is_valid()); + CHECK(storage.restore(pool) == 4); + CHECK(gain == 3.25F); + CHECK(profile == 7); + CHECK(flag); + CHECK(maze.get() == std::vector{9, 8, 7, 6, 5}); CHECK(transient == 42.0F); } - // --- a renamed / reordered pool still finds what it knows --- - { + + TEST_CASE_FIXTURE(StoredPool, "finds the variables it knows in a reordered pool") { core::TVariablePool<8> pool; uint8_t profile = 0; - float kv = 0.0F; + float gain = 0.0F; float fresh = 1.0F; + pool.add("", "fresh", fresh, {.persist = true}); pool.add("", "run_profile", profile, {.persist = true}); - pool.add("model/", "kv", kv, {.persist = true}); - proxy::Storage s{cfg}; - CHECK(s.restore(pool) == 2); - CHECK(profile == 7 && kv == 3.25F && fresh == 1.0F); + pool.add("model/", "kv", gain, {.persist = true}); + + proxy::Storage storage{storage_config}; + CHECK(storage.restore(pool) == 2); + CHECK(profile == 7); + CHECK(gain == 3.25F); + CHECK(fresh == 1.0F); } - // --- a type change is refused, not misread --- - { + + TEST_CASE_FIXTURE(StoredPool, "refuses a variable whose type changed instead of misreading it") { core::TVariablePool<8> pool; - uint32_t kv = 0; - pool.add("model/", "kv", kv, {.persist = true}); - proxy::Storage s{cfg}; - CHECK(s.restore(pool) == 0); - CHECK(kv == 0); + uint32_t gain = 0; + pool.add("model/", "kv", gain, {.persist = true}); + + proxy::Storage storage{storage_config}; + CHECK(storage.restore(pool) == 0); + CHECK(gain == 0); } - // --- a torn write leaves the previous image, not a half one --- - { + + TEST_CASE_FIXTURE(StoredPool, "leaves no image to restore after a write the power cut off") { core::TVariablePool<8> pool; - float kv = 99.0F; - pool.add("model/", "kv", kv, {.persist = true}); - proxy::Storage s{cfg}; - hal::Flash::write_budget = 0; - CHECK(!s.save(pool)); - hal::Flash::write_budget = UINT32_MAX; - proxy::Storage after{cfg}; - CHECK(!after.is_valid()); - CHECK(after.restore(pool) == 0); + float gain = 99.0F; + pool.add("model/", "kv", gain, {.persist = true}); + + proxy::Storage storage{storage_config}; + hal::host::Board::flash().write_budget = 0; + CHECK_FALSE(storage.save(pool)); + + const proxy::Storage after{storage_config}; + CHECK_FALSE(after.is_valid()); } - // --- writing the body but dying before the header is the same as never saving --- - { - hal::Flash::erase_sectors(0, 1); + + TEST_CASE_FIXTURE(HostBoard, "treats a body written without its header as never saved") { core::TVariablePool<8> pool; - float kv = 1.0F; - pool.add("model/", "kv", kv, {.persist = true}); - proxy::Storage s{cfg}; - hal::Flash::write_budget = 1; // one flash word: the body goes in, the header does not - CHECK(!s.save(pool)); - hal::Flash::write_budget = UINT32_MAX; - proxy::Storage after{cfg}; - CHECK(!after.is_valid()); + float gain = 1.0F; + pool.add("model/", "kv", gain, {.persist = true}); + + proxy::Storage storage{storage_config}; + hal::host::Board::flash().write_budget = 1; + CHECK_FALSE(storage.save(pool)); + + const proxy::Storage after{storage_config}; + CHECK_FALSE(after.is_valid()); } - std::puts("storage ok"); } +} // namespace micras::test diff --git a/tests/stub/micras/hal/flash.hpp b/tests/stub/micras/hal/flash.hpp deleted file mode 100644 index ab27e0a..0000000 --- a/tests/stub/micras/hal/flash.hpp +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef MICRAS_HAL_FLASH_HPP -#define MICRAS_HAL_FLASH_HPP -#include -#include -#include -#include -#include - -namespace micras::hal { -class FlashWord { -public: - static constexpr uint32_t words{8}; - static constexpr uint32_t size{words * sizeof(uint32_t)}; - static constexpr uint8_t erased_value{0xFF}; -}; - -class Flash { -public: - enum class Status : uint8_t { - OK = 0, - MISALIGNED = 1, - OUT_OF_BOUNDS = 2, - ERROR = 3 - }; - static constexpr uint32_t sector_size{128 * 1024}; - static constexpr uint16_t total_sectors{2}; - static constexpr uint32_t total_size{total_sectors * sector_size}; - - static inline std::vector memory = std::vector(total_size, 0xFF); - static inline uint32_t write_budget = UINT32_MAX; // flash words allowed before a simulated power loss - - static std::span read(uint16_t sector, uint32_t sector_address, uint32_t size) { - uint64_t address = uint64_t(sector) * sector_size + sector_address; - if (address + size > total_size) - return {}; - return std::span{memory.data() + address, size}; - } - - static Status write(uint16_t sector, uint32_t sector_address, std::span data) { - if (sector_address % FlashWord::size != 0) - return Status::MISALIGNED; - uint64_t address = uint64_t(sector) * sector_size + sector_address; - uint32_t padded = (data.size() + FlashWord::size - 1) / FlashWord::size * FlashWord::size; - if (address + padded > total_size) - return Status::OUT_OF_BOUNDS; - for (uint32_t i = 0; i < padded; i += FlashWord::size) { - if (write_budget == 0) - return Status::ERROR; - write_budget--; - for (uint32_t j = 0; j < FlashWord::size; j++) { - uint32_t k = i + j; - memory[address + k] = k < data.size() ? data[k] : FlashWord::erased_value; - } - } - return Status::OK; - } - - static Status erase_sectors(uint16_t start_sector, uint16_t number_of_sectors = 1) { - uint64_t address = uint64_t(start_sector) * sector_size; - if (address + uint64_t(number_of_sectors) * sector_size > total_size) - return Status::OUT_OF_BOUNDS; - std::memset(memory.data() + address, 0xFF, size_t(number_of_sectors) * sector_size); - return Status::OK; - } -}; -} // namespace micras::hal -#endif diff --git a/tests/support/host_board/CMakeLists.txt b/tests/support/host_board/CMakeLists.txt new file mode 100644 index 0000000..8d9617f --- /dev/null +++ b/tests/support/host_board/CMakeLists.txt @@ -0,0 +1,16 @@ +# The fake Cube layer of the reference projects. micras_hal links it as its board target, and it links +# micras_hal back for the host backend's headers and registry, a cycle of two static libraries that +# CMake links by repeating them. +add_library(micras_lib_host_board STATIC + handles.cpp +) + +target_include_directories(micras_lib_host_board PUBLIC + . +) + +target_link_libraries(micras_lib_host_board PUBLIC + micras::hal +) + +micras_apply_warnings(micras_lib_host_board WERROR ${MICRAS_LIB_WERROR}) diff --git a/tests/support/host_board/adc.h b/tests/support/host_board/adc.h new file mode 100644 index 0000000..e4b5114 --- /dev/null +++ b/tests/support/host_board/adc.h @@ -0,0 +1,22 @@ +/** + * @file + * + * @brief Fake STM32CubeMX adc.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_ADC_H +#define MICRAS_LIB_TESTS_HOST_BOARD_ADC_H + +#include "main.h" + +/** + * @brief Handle hadc3, which converts the internal reference and the temperature sensor. + */ +extern ADC_HandleTypeDef hadc3; + +/** + * @brief Configure what MX_ADC3_Init configures. + */ +void MX_ADC3_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_ADC_H diff --git a/tests/support/host_board/crc.h b/tests/support/host_board/crc.h new file mode 100644 index 0000000..c30a1e7 --- /dev/null +++ b/tests/support/host_board/crc.h @@ -0,0 +1,22 @@ +/** + * @file + * + * @brief Fake STM32CubeMX crc.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_CRC_H +#define MICRAS_LIB_TESTS_HOST_BOARD_CRC_H + +#include "main.h" + +/** + * @brief Handle hcrc, set up for the AS5047U frames: 8 bits, polynomial 0x1D, initial value 0xC4. + */ +extern CRC_HandleTypeDef hcrc; + +/** + * @brief Configure what MX_CRC_Init configures. + */ +void MX_CRC_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_CRC_H diff --git a/tests/support/host_board/dma.h b/tests/support/host_board/dma.h new file mode 100644 index 0000000..88d9ebb --- /dev/null +++ b/tests/support/host_board/dma.h @@ -0,0 +1,17 @@ +/** + * @file + * + * @brief Fake STM32CubeMX dma.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_DMA_H +#define MICRAS_LIB_TESTS_HOST_BOARD_DMA_H + +#include "main.h" + +/** + * @brief Configure what MX_DMA_Init configures, which the host needs nothing of. + */ +void MX_DMA_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_DMA_H diff --git a/tests/support/host_board/fmac.h b/tests/support/host_board/fmac.h new file mode 100644 index 0000000..5ac2057 --- /dev/null +++ b/tests/support/host_board/fmac.h @@ -0,0 +1,22 @@ +/** + * @file + * + * @brief Fake STM32CubeMX fmac.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_FMAC_H +#define MICRAS_LIB_TESTS_HOST_BOARD_FMAC_H + +#include "main.h" + +/** + * @brief Handle hfmac. + */ +extern FMAC_HandleTypeDef hfmac; + +/** + * @brief Configure what MX_FMAC_Init configures. + */ +void MX_FMAC_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_FMAC_H diff --git a/tests/support/host_board/gpio.h b/tests/support/host_board/gpio.h new file mode 100644 index 0000000..198fdf2 --- /dev/null +++ b/tests/support/host_board/gpio.h @@ -0,0 +1,17 @@ +/** + * @file + * + * @brief Fake STM32CubeMX gpio.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_GPIO_H +#define MICRAS_LIB_TESTS_HOST_BOARD_GPIO_H + +#include "main.h" + +/** + * @brief Configure what MX_GPIO_Init configures: the labels of the pins. + */ +void MX_GPIO_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_GPIO_H diff --git a/tests/support/host_board/handles.cpp b/tests/support/host_board/handles.cpp new file mode 100644 index 0000000..aedcf0e --- /dev/null +++ b/tests/support/host_board/handles.cpp @@ -0,0 +1,137 @@ +/** + * @file + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/hal/host/board.hpp" + +namespace { +constexpr uint32_t timer_clock{200000000}; + +constexpr uint32_t spi3_kernel_clock{125000000}; + +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) +TIM_TypeDef tim2_registers{}; +TIM_TypeDef tim5_registers{}; +TIM_TypeDef tim8_registers{}; +TIM_TypeDef tim15_registers{}; +SPI_TypeDef spi3_registers{}; + +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) + +void init_timer( + TIM_HandleTypeDef& handle, TIM_TypeDef& registers, const char* name, uint32_t prescaler, uint32_t counter_mode, + uint32_t period +) { + handle.Instance = ®isters; + handle.Init = {.Prescaler = prescaler, .CounterMode = counter_mode, .Period = period}; + registers = {.CR1 = counter_mode, .CCER = 0, .PSC = prescaler, .ARR = period, .kernel_clock = timer_clock}; + handle.State = HAL_TIM_STATE_READY; + micras::hal::host::Board::name_handle(&handle, name); +} +} // namespace + +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) +uint32_t SystemCoreClock{400000000}; + +GPIO_TypeDef GPIOA_instance{'A'}; +GPIO_TypeDef GPIOB_instance{'B'}; +GPIO_TypeDef GPIOD_instance{'D'}; + +ADC_HandleTypeDef hadc3{}; +CRC_HandleTypeDef hcrc{}; +FMAC_HandleTypeDef hfmac{}; +SPI_HandleTypeDef hspi3{}; +TIM_HandleTypeDef htim2{}; +TIM_HandleTypeDef htim5{}; +TIM_HandleTypeDef htim8{}; +TIM_HandleTypeDef htim15{}; +UART_HandleTypeDef huart4{}; + +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) + +extern "C" { +void SystemClock_Config() { } + +void PeriphCommonClock_Config() { } +} + +void MX_GPIO_Init() { + using micras::hal::host::Board; + Board::name_gpio(Button_GPIO_Port, Button_Pin, "Button"); + Board::name_gpio(Status_LED_GPIO_Port, Status_LED_Pin, "Status_LED"); + Board::name_gpio(SPI_CSn_GPIO_Port, SPI_CSn_Pin, "SPI_CSn"); +} + +void MX_DMA_Init() { } + +void MX_ADC3_Init() { + hadc3.Init.NbrOfConversion = 2; + hadc3.State = HAL_ADC_STATE_READY; + micras::hal::host::Board::name_handle(&hadc3, "hadc3"); +} + +void MX_CRC_Init() { + hcrc.Init = { + .DefaultPolynomialUse = DEFAULT_POLYNOMIAL_DISABLE, + .DefaultInitValueUse = DEFAULT_INIT_VALUE_DISABLE, + .GeneratingPolynomial = 0x1D, + .CRCLength = CRC_POLYLENGTH_8B, + .InitValue = 0xC4, + .InputDataInversionMode = CRC_INPUTDATA_INVERSION_NONE, + .OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_DISABLE, + }; + hcrc.InputDataFormat = CRC_INPUTDATA_FORMAT_BYTES; + micras::hal::host::Board::name_handle(&hcrc, "hcrc"); +} + +void MX_FMAC_Init() { + hfmac.State = HAL_FMAC_STATE_READY; + micras::hal::host::Board::name_handle(&hfmac, "hfmac"); +} + +void MX_SPI3_Init() { + spi3_registers.kernel_clock = spi3_kernel_clock; + hspi3.Instance = &spi3_registers; + hspi3.Init = { + .CLKPolarity = SPI_POLARITY_HIGH, + .CLKPhase = SPI_PHASE_2EDGE, + .BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32, + }; + hspi3.State = HAL_SPI_STATE_READY; + micras::hal::host::Board::name_handle(&hspi3, "hspi3"); +} + +void MX_TIM2_Init() { + init_timer(htim2, tim2_registers, "htim2", 0, TIM_COUNTERMODE_UP, 0xFFFFFFFF); +} + +void MX_TIM5_Init() { + init_timer(htim5, tim5_registers, "htim5", 0, TIM_COUNTERMODE_UP, 0xFFFFFFFF); +} + +void MX_TIM8_Init() { + init_timer(htim8, tim8_registers, "htim8", 0, TIM_COUNTERMODE_UP, 249); +} + +void MX_TIM15_Init() { + init_timer(htim15, tim15_registers, "htim15", 199, TIM_COUNTERMODE_UP, 999); +} + +void MX_UART4_Init() { + huart4.Init.BaudRate = 115200; + huart4.gState = HAL_UART_STATE_READY; + huart4.RxState = HAL_UART_STATE_READY; + micras::hal::host::Board::name_handle(&huart4, "huart4"); +} diff --git a/tests/support/host_board/main.h b/tests/support/host_board/main.h new file mode 100644 index 0000000..a991676 --- /dev/null +++ b/tests/support/host_board/main.h @@ -0,0 +1,62 @@ +/** + * @file + * + * @brief Fake STM32CubeMX main.h of the reference projects, for the host tests. + * + * @note The host counterpart of reference/h7: the same peripherals under the + * same handle and pin names, one instance each, with the values its + * CubeMX project generates. The configurations the tests build from them + * compile unchanged against the generated tree of the reference. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_MAIN_H +#define MICRAS_LIB_TESTS_HOST_BOARD_MAIN_H + +#include + +#include "stm32_host.h" + +/***************************************** + * Microcontroller + *****************************************/ + +/** + * @brief Core clock after SystemClock_Config: HSI 64 MHz / PLLM 4 * PLLN 50 / PLLP 2. + */ +extern uint32_t SystemCoreClock; + +/** + * @brief Flash geometry of the STM32H725xG: one bank of eight 128 KB sectors, 256-bit flash words. + */ +///@{ +#define FLASH_NB_32BITWORD_IN_FLASHWORD 8U +inline constexpr uint32_t FLASH_SECTOR_SIZE{0x00020000U}; +inline constexpr uint32_t FLASH_SECTOR_TOTAL{8U}; +///@} + +/***************************************** + * GPIO ports and pins + *****************************************/ + +///@{ +extern GPIO_TypeDef GPIOA_instance; +extern GPIO_TypeDef GPIOB_instance; +extern GPIO_TypeDef GPIOD_instance; +///@} + +///@{ +#define GPIOA (&GPIOA_instance) +#define GPIOB (&GPIOB_instance) +#define GPIOD (&GPIOD_instance) +///@} + +///@{ +#define Button_Pin GPIO_PIN_7 +#define Button_GPIO_Port GPIOA +#define Status_LED_Pin GPIO_PIN_12 +#define Status_LED_GPIO_Port GPIOB +#define SPI_CSn_Pin GPIO_PIN_2 +#define SPI_CSn_GPIO_Port GPIOD +///@} + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_MAIN_H diff --git a/tests/support/host_board/spi.h b/tests/support/host_board/spi.h new file mode 100644 index 0000000..bf4e948 --- /dev/null +++ b/tests/support/host_board/spi.h @@ -0,0 +1,22 @@ +/** + * @file + * + * @brief Fake STM32CubeMX spi.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_SPI_H +#define MICRAS_LIB_TESTS_HOST_BOARD_SPI_H + +#include "main.h" + +/** + * @brief Handle hspi3, a full duplex master in SPI mode 3. + */ +extern SPI_HandleTypeDef hspi3; + +/** + * @brief Configure what MX_SPI3_Init configures. + */ +void MX_SPI3_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_SPI_H diff --git a/tests/support/host_board/tim.h b/tests/support/host_board/tim.h new file mode 100644 index 0000000..cc2ba65 --- /dev/null +++ b/tests/support/host_board/tim.h @@ -0,0 +1,52 @@ +/** + * @file + * + * @brief Fake STM32CubeMX tim.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_TIM_H +#define MICRAS_LIB_TESTS_HOST_BOARD_TIM_H + +#include "main.h" + +/** + * @brief Handle htim2, a free-running 32-bit timer. + */ +extern TIM_HandleTypeDef htim2; + +/** + * @brief Handle htim5, a timer in encoder mode. + */ +extern TIM_HandleTypeDef htim5; + +/** + * @brief Handle htim8, whose first channel a DMA stream feeds. + */ +extern TIM_HandleTypeDef htim8; + +/** + * @brief Handle htim15, whose first channel is a PWM output. + */ +extern TIM_HandleTypeDef htim15; + +/** + * @brief Configure what MX_TIM2_Init configures. + */ +void MX_TIM2_Init(); + +/** + * @brief Configure what MX_TIM5_Init configures. + */ +void MX_TIM5_Init(); + +/** + * @brief Configure what MX_TIM8_Init configures. + */ +void MX_TIM8_Init(); + +/** + * @brief Configure what MX_TIM15_Init configures. + */ +void MX_TIM15_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_TIM_H diff --git a/tests/support/host_board/usart.h b/tests/support/host_board/usart.h new file mode 100644 index 0000000..18f52c5 --- /dev/null +++ b/tests/support/host_board/usart.h @@ -0,0 +1,22 @@ +/** + * @file + * + * @brief Fake STM32CubeMX usart.h of the reference projects: its handles and init functions. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_BOARD_USART_H +#define MICRAS_LIB_TESTS_HOST_BOARD_USART_H + +#include "main.h" + +/** + * @brief Handle huart4, with DMA on receive and transmit. + */ +extern UART_HandleTypeDef huart4; + +/** + * @brief Configure what MX_UART4_Init configures. + */ +void MX_UART4_Init(); + +#endif // MICRAS_LIB_TESTS_HOST_BOARD_USART_H diff --git a/tests/support/host_fixture.hpp b/tests/support/host_fixture.hpp new file mode 100644 index 0000000..62bc947 --- /dev/null +++ b/tests/support/host_fixture.hpp @@ -0,0 +1,186 @@ +/** + * @file + * + * @brief The peripherals of the reference projects as the host tests configure them, and the fixture + * that starts every test from a fresh board. + */ + +#ifndef MICRAS_LIB_TESTS_HOST_FIXTURE_HPP +#define MICRAS_LIB_TESTS_HOST_FIXTURE_HPP + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/crc.hpp" +#include "micras/hal/encoder.hpp" +#include "micras/hal/fmac.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/host/board.hpp" +#include "micras/hal/host/clock.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/pwm_dma.hpp" +#include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" +#include "micras/hal/uart_dma.hpp" + +namespace micras::test { +/** + * @brief The output pin of the reference projects. + */ +inline const hal::Gpio::Config status_led_config{ + .port = Status_LED_GPIO_Port, + .pin = Status_LED_Pin, +}; + +/** + * @brief The input pin of the reference projects. + */ +inline const hal::Gpio::Config button_config{ + .port = Button_GPIO_Port, + .pin = Button_Pin, +}; + +/** + * @brief The SPI bus and its chip select, in SPI mode 3. + */ +inline const hal::Spi::Config spi_config{ + .init_function = MX_SPI3_Init, + .handle = &hspi3, + .cs_gpio = {.port = SPI_CSn_GPIO_Port, .pin = SPI_CSn_Pin}, + .timeout = 2, + .clock_polarity = SPI_POLARITY_HIGH, + .clock_phase = SPI_PHASE_2EDGE, +}; + +/** + * @brief A second device on the same bus, in SPI mode 1, behind the output pin used as a chip select. + */ +inline const hal::Spi::Config second_spi_config{ + .init_function = MX_SPI3_Init, + .handle = &hspi3, + .cs_gpio = status_led_config, + .timeout = 2, + .clock_polarity = SPI_POLARITY_LOW, + .clock_phase = SPI_PHASE_2EDGE, +}; + +/** + * @brief The PWM channel. + */ +inline const hal::Pwm::Config pwm_config{ + .init_function = MX_TIM15_Init, + .handle = &htim15, + .timer_channel = TIM_CHANNEL_1, + .inverted = false, +}; + +/** + * @brief The PWM channel a DMA stream feeds. + */ +inline const hal::PwmDma::Config pwm_dma_config{ + .init_function = MX_TIM8_Init, + .handle = &htim8, + .timer_channel = TIM_CHANNEL_1, +}; + +/** + * @brief The timer in encoder mode. + */ +inline const hal::Encoder::Config encoder_config{ + .init_function = MX_TIM5_Init, + .handle = &htim5, + .timer_channel = TIM_CHANNEL_ALL, +}; + +/** + * @brief The UART. + */ +inline const hal::UartDma::Config uart_config{ + .init_function = MX_UART4_Init, + .handle = &huart4, +}; + +/** + * @brief The ADC, twelve bits referenced to 3.3 V. + */ +inline const hal::AdcDma::Config adc_config{ + .init_function = MX_ADC3_Init, + .handle = &hadc3, + .max_reading = 4095, + .reference_voltage = 3.3F, +}; + +/** + * @brief The CRC unit, set up for the AS5047U frames. + */ +inline const hal::Crc::Config crc_config{ + .handle = &hcrc, +}; + +/** + * @brief The filter math accelerator. + */ +inline const hal::Fmac::Config fmac_config{ + .init_function = MX_FMAC_Init, + .handle = &hfmac, +}; + +/** + * @brief Starts a test from the state the board has after a reset. + * + * @note Forgets every port and rewinds the clock, as the host backend asks of independent runs in one + * process, and returns every handle to its reset state, so that each driver runs its init function + * again. The pins are named, and the SPI bus initialized, before any driver creates a port for them, + * since a port takes its name when it is created. + */ +class HostBoard { +public: + /** + * @brief Reset the board, the clock and the handles. + */ + HostBoard() { + hal::host::Board::reset(); + hal::host::Clock::instance().reset(); + hal::host::Clock::instance().configure(SystemCoreClock / 1000000); + hal::Timer::init(); + + hadc3 = {}; + hcrc = {}; + hfmac = {}; + hspi3 = {}; + htim2 = {}; + htim5 = {}; + htim8 = {}; + htim15 = {}; + huart4 = {}; + + MX_GPIO_Init(); + MX_DMA_Init(); + MX_CRC_Init(); + MX_SPI3_Init(); + } + + /** + * @brief Advance the host clock as a firmware waiting that long does, one timer read at a time. + * + * @param microseconds How long to wait. + */ + static void wait_us(uint32_t microseconds) { + const uint32_t start = hal::Timer::get_counter(); + + while (hal::Timer::to_microseconds(hal::Timer::get_counter() - start) < microseconds) { } + } +}; +} // namespace micras::test + +#endif // MICRAS_LIB_TESTS_HOST_FIXTURE_HPP diff --git a/tests/support/main.cpp b/tests/support/main.cpp new file mode 100644 index 0000000..8791533 --- /dev/null +++ b/tests/support/main.cpp @@ -0,0 +1,6 @@ +/** + * @file + */ + +#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN +#include diff --git a/tests/test_host.hpp b/tests/test_host.hpp deleted file mode 100644 index 8f28101..0000000 --- a/tests/test_host.hpp +++ /dev/null @@ -1,25 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_TEST_HOST_HPP -#define MICRAS_TEST_HOST_HPP - -#include -#include - -/** - * @brief Fail the test with the condition and where it was written. - * - * @note Spelled out instead of using assert so that it holds whatever the build turns NDEBUG into. - */ -// NOLINTNEXTLINE(cppcoreguidelines-macro-usage) it has to capture the text and the line -#define CHECK(condition) \ - do { \ - if (not(condition)) { \ - std::fprintf(stderr, "%s:%d: %s\n", __FILE__, __LINE__, #condition); \ - std::abort(); \ - } \ - } while (false) - -#endif // MICRAS_TEST_HOST_HPP From 66001914f629e181033853b83ae48439574dd231 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 05:39:45 -0300 Subject: [PATCH 534/561] =?UTF-8?q?=E2=9C=85=20Seed=20the=20navigation=20t?= =?UTF-8?q?ests=20on=20the=20contest=20mazes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The planner finds the fastest route of each of the ten contest mazes with golden times and step counts, to regenerate on intended planner changes; the maze round trips through its serialization; the turn tables, designed at runtime for Micras v1's model and designs, fit with both margins and end every turn on its exit node, as the firmware's static_asserts require; and the route compiler chains its segments into the goal. A deserialized wall is decoded from an unsigned byte, which GCC 15 no longer warns about. --- micras_nav/include/micras/nav/impl/maze.tpp | 3 +- .../data/mazes/alljapan-033-2012-exp-fin.txt | 33 ++++ tests/data/mazes/apec2016.txt | 33 ++++ tests/data/mazes/apec2017.txt | 33 ++++ tests/data/mazes/apec2018.txt | 33 ++++ tests/data/mazes/apec2019.txt | 33 ++++ tests/data/mazes/japan2013ef.txt | 33 ++++ tests/data/mazes/japan2017ef.txt | 33 ++++ tests/data/mazes/maze1.txt | 33 ++++ tests/data/mazes/maze2.txt | 33 ++++ tests/data/mazes/uk2016f.txt | 33 ++++ tests/nav/reference_robot.cpp | 116 ++++++++++++ tests/nav/reference_robot.hpp | 169 ++++++++++++++++++ tests/nav/test_maze.cpp | 131 ++++++++++++++ tests/nav/test_planner.cpp | 71 ++++++++ tests/nav/test_route_compiler.cpp | 151 ++++++++++++++++ tests/nav/test_turn_table.cpp | 98 ++++++++++ tests/support/maze_reader.hpp | 136 ++++++++++++++ 18 files changed, 1204 insertions(+), 1 deletion(-) create mode 100644 tests/data/mazes/alljapan-033-2012-exp-fin.txt create mode 100644 tests/data/mazes/apec2016.txt create mode 100644 tests/data/mazes/apec2017.txt create mode 100644 tests/data/mazes/apec2018.txt create mode 100644 tests/data/mazes/apec2019.txt create mode 100644 tests/data/mazes/japan2013ef.txt create mode 100644 tests/data/mazes/japan2017ef.txt create mode 100644 tests/data/mazes/maze1.txt create mode 100644 tests/data/mazes/maze2.txt create mode 100644 tests/data/mazes/uk2016f.txt create mode 100644 tests/nav/reference_robot.cpp create mode 100644 tests/nav/reference_robot.hpp create mode 100644 tests/nav/test_maze.cpp create mode 100644 tests/nav/test_planner.cpp create mode 100644 tests/nav/test_route_compiler.cpp create mode 100644 tests/nav/test_turn_table.cpp create mode 100644 tests/support/maze_reader.hpp diff --git a/micras_nav/include/micras/nav/impl/maze.tpp b/micras_nav/include/micras/nav/impl/maze.tpp index bb38e6d..d241735 100644 --- a/micras_nav/include/micras/nav/impl/maze.tpp +++ b/micras_nav/include/micras/nav/impl/maze.tpp @@ -229,7 +229,8 @@ void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { for (uint8_t row = 0; row < height; row++) { for (uint8_t column = 0; column < width; column++) { const uint16_t index = row * width + column; - const uint8_t packed = (data[header_size + index / 2U] >> (4U * (index % 2U))) & 0x0FU; + const uint8_t packed = + (static_cast(data[header_size + index / 2U]) >> (4U * (index % 2U))) & 0x0FU; const std::array, 2> sides{ {{Side::RIGHT, packed & 0x03U}, {Side::UP, packed >> 2U}} diff --git a/tests/data/mazes/alljapan-033-2012-exp-fin.txt b/tests/data/mazes/alljapan-033-2012-exp-fin.txt new file mode 100644 index 0000000..6b41392 --- /dev/null +++ b/tests/data/mazes/alljapan-033-2012-exp-fin.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | | | +o o o o---o---o---o o---o---o---o---o o o o o o +| | | | | | | | | | | +o o o o o o o---o o---o---o---o---o o o o o +| | | | | | | | | +o o---o---o o o---o---o---o---o---o---o o o---o---o o +| | | | | | +o o---o---o---o o o---o---o---o---o---o o---o---o o o +| | | | | | | | +o o o---o o---o o o o---o o o---o o---o o o +| | | | | | | | | | | | +o o---o o---o o o o o o o---o o o o---o o +| | | | | | | | | | +o o o---o---o o o o o---o o o---o o---o o o +| | | | | G G | | | | | +o o---o o---o o o o o o o o o---o o---o o +| | | | | G G | | | | | +o o o---o o o o o o---o o o---o o---o---o o +| | | | | | | | | | +o o---o o---o o o o o o---o---o o---o---o o o +| | | | | | | | +o---o---o---o---o o o o o o o---o o---o o o o +| | | | | | | | | +o o---o---o---o o o o o o o---o---o o---o o o +| | | | | | | | | +o---o---o o---o o---o---o o---o o o o o o---o o +| | | | | | | | | +o o o o o o---o o---o---o---o o o o o---o---o +| | | | | | | | | +o o o o o o---o---o---o---o---o---o---o---o---o---o o +| S | | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/apec2016.txt b/tests/data/mazes/apec2016.txt new file mode 100644 index 0000000..0b171b8 --- /dev/null +++ b/tests/data/mazes/apec2016.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | | | | | | +o o o o---o o---o o o o o o---o---o---o o o +| | | | | | | | | | | | | +o o o o o---o---o---o o o o o---o o o---o o +| | | | | | | | | | +o o o o o---o---o---o o o o o---o o---o o o +| | | | | | | | | | | | | | +o o o o o o o o---o o---o---o o---o o o o +| | | | | | | | | | +o o o o---o---o o o---o---o---o o---o---o o o o +| | | | | | G G | | | | | | +o o o o o o---o o o o o---o---o o o o o +| | | | | | | G G | | | | +o o o o o---o o o---o---o---o---o o o o o o +| | | | | | | | | | | | | +o o o o o o---o---o---o---o o o o o---o---o o +| | | | | | | | | | | +o o o o o o o o---o o---o o o---o---o o o +| | | | | | | | | | | +o o o o---o---o o---o o---o o o---o---o o o o +| | | | | | | | | | +o o---o o o---o---o o---o o---o o o o o o o +| | | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/apec2017.txt b/tests/data/mazes/apec2017.txt new file mode 100644 index 0000000..b03869e --- /dev/null +++ b/tests/data/mazes/apec2017.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | | | | +o o o---o---o o o o o---o---o o o---o o o o +| | | | | | | | | | | | +o o o o o---o o---o---o o o o---o o o o o +| | | | | | | | | | | | | | +o o o o---o o---o o o---o o o o---o o o o +| | | | | | | | | | | | | +o o o o o---o o o---o o o o---o o o o o +| | | | | | | | | | | | +o o o o o o o---o---o---o---o---o---o o o o o +| | | | | G G | | | | | +o o o---o---o o---o o o o o---o o o o o o +| | | | G G | | | | | | +o o o o---o---o---o---o---o o---o o o---o o o o +| | | | | | | | | | +o o o---o o o---o o---o---o o o---o o---o o o +| | | | | | | | | | | | +o o o o---o o o---o---o o o---o o---o o o o +| | | | | | | | | | | | | +o o o---o o o---o o o---o---o o---o o o o o +| | | | | | | | | | | +o o---o o o---o o o---o o o---o o---o o o o +| | | | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/apec2018.txt b/tests/data/mazes/apec2018.txt new file mode 100644 index 0000000..f38c3ef --- /dev/null +++ b/tests/data/mazes/apec2018.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | | +o o o o o---o---o o---o---o---o---o---o---o---o---o o +| | | | | | | | | +o o o o---o---o o---o---o---o o o o o o o o +| | | | | | | | | | | +o o o o---o o---o o---o---o---o o o o o---o o +| | | | | | | | | | +o o o o o o---o---o---o---o---o o o o---o o o +| | | | | | | | | | | | | +o o o o o---o o o---o---o o o o o o---o o +| | | | | | | G G | | | | | | | +o o o o---o o o o o o o o o o---o o o +| | | | | G G | | | | | | +o o o o o---o o o---o---o o o o---o o o o +| | | | | | | | | | | +o o o o---o o o---o---o o---o---o o o---o o o +| | | | | | | | | | | | | +o o o o o o---o o o---o o o---o---o o---o o +| | | | | | | | | | | | +o o o o o---o o o---o o---o o---o---o---o o o +| | | | | | | | | | +o o o o---o o---o---o---o---o---o o o o o o o +| | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/apec2019.txt b/tests/data/mazes/apec2019.txt new file mode 100644 index 0000000..02cf47b --- /dev/null +++ b/tests/data/mazes/apec2019.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | | | | | | +o o o o---o o---o o---o o---o o---o o---o o o +| | | | | | | | | | | +o o o o o---o o---o o---o o---o o---o o o o +| | | | | | | | | | | +o o o o---o o---o o---o o---o o---o---o---o o o +| | | | | | | | +o o o o---o---o---o---o---o---o---o---o o o---o o o +| | | | | | | | | +o o o o o o o---o---o---o---o---o---o---o o---o o +| | | | | | G G | | | +o o o o o o---o---o o o o---o---o---o---o o o +| | | | | | G G | | | | +o o o o o---o---o o---o---o---o---o o o---o---o o +| | | | | | | | | +o o o o---o o o---o---o---o o o---o---o---o o o +| | | | | | | | | | +o o o o o---o o o---o o---o---o o---o---o o o +| | | | | | | | | | +o o o o---o---o o---o o---o---o o---o---o o o o +| | | | | | | | | | | +o o o o o---o---o---o o o---o---o o o o o o +| | | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/japan2013ef.txt b/tests/data/mazes/japan2013ef.txt new file mode 100644 index 0000000..05d8df4 --- /dev/null +++ b/tests/data/mazes/japan2013ef.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | | +o o---o o---o---o---o---o---o---o---o---o---o o o o o +| | | | | | | +o o---o---o o o---o---o---o---o---o---o o---o o o o +| | | | | | | | +o o---o o---o o o---o o---o o---o---o o---o---o o +| | | | | | | | | +o o---o o---o---o o---o o---o o o o o---o o o +| | | | | | | | | | +o o---o o---o o---o---o o---o---o---o o o---o o o +| | | | | | | | | | +o o o---o o---o o---o o---o o o o---o o o o +| | | | | | | | | | +o o o---o---o o o---o o---o---o o---o o o o o +| | | | | G G | | | | | | | +o o---o---o o o---o o o o o---o o o o o o +| | | | | G G | | | | | | +o o o---o---o o o---o---o---o---o o---o o o o o +| | | | | | | | | | +o o---o---o o---o---o o o o o---o o---o o o o +| | | | | | | | | | | +o o o---o---o o o---o---o---o---o o o---o o o o +| | | | | | | | | +o o o---o---o---o o o o---o o---o o o---o o o +| | | | | | | | | | | +o o o o o o---o---o---o---o o o---o o o o o +| | | | | | | | | | +o o o---o---o o o---o o o---o---o o o o---o o +| | | | | | | | | | +o o o o o---o---o---o---o---o---o---o---o---o o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/japan2017ef.txt b/tests/data/mazes/japan2017ef.txt new file mode 100644 index 0000000..42b70e0 --- /dev/null +++ b/tests/data/mazes/japan2017ef.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | +o o o o---o---o---o---o---o---o---o---o---o---o o o---o +| | | | | | | | +o o---o o o---o---o o o---o---o o o o---o---o o +| | | | | | | | | +o o---o o o o o---o---o o o---o---o---o---o o o +| | | | | | | +o o---o---o---o---o---o---o---o---o---o---o---o o o---o o +| | | | | | | | | +o o o o o o o o o o o o o---o---o o o +| | | | | | | | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o o---o o +| | | | | | | | +o o---o o o o---o o o---o o o o---o o---o o +| | | | | | G G | | | | +o o o---o o---o o o o o---o---o---o---o o---o o +| | | | | | G G | | | | +o o o o---o o---o o---o---o o o o---o---o---o o +| | | | | | | +o o---o---o o---o---o---o---o---o---o---o---o---o---o o o +| | | | | | | | | | +o o o---o---o o o o o o o o o---o o---o o +| | | | | | | | | | +o o---o o o---o---o o---o---o o---o o o---o o o +| | | | | | | | | | | +o o o---o---o o o---o o o---o o---o o---o---o o +| | | | | | | | | +o o o o---o---o o---o---o---o---o---o o o o o o +| | | | | | | | | +o o---o o o o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/maze1.txt b/tests/data/mazes/maze1.txt new file mode 100644 index 0000000..85890fb --- /dev/null +++ b/tests/data/mazes/maze1.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | +o o---o---o---o o---o o---o---o---o---o o---o o o o +| | | | | | | +o o---o---o o---o---o o o o---o o o o---o---o o +| | | | | | | +o o---o o---o---o---o o o---o---o o o---o o o---o +| | | | | +o o---o---o---o---o---o o---o---o---o o o---o---o---o o +| | | | | +o o---o---o---o---o o---o o---o o o o---o---o o o +| | | | | | | | +o o o---o---o---o---o o---o o---o---o o o---o o o +| | | | | | | | | +o o o o---o---o---o o---o---o---o o o o---o o o +| | | | | G G | | | | | | +o o o---o---o---o o o o o o o o---o---o o o +| | | | G G | | | | +o o---o---o---o---o---o o---o---o o o o---o o---o o +| | | | | +o o---o---o---o o o---o---o---o---o---o---o o o o o +| | | | | | | | | | +o o o---o---o o o o o o---o---o o o o o o +| | | | | | | | | | +o---o---o---o o o o o o---o---o---o o o o o o +| | | | | | | | | | +o o---o---o o o o o o o---o o o o o o o +| | | | | | | | | +o o o---o---o o o---o o---o---o---o o o o o o +| | | | | | | | | | +o o---o---o---o o o o o---o---o o o o o o o +| S | | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/maze2.txt b/tests/data/mazes/maze2.txt new file mode 100644 index 0000000..ef6b2a5 --- /dev/null +++ b/tests/data/mazes/maze2.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o o---o o o---o o---o---o o---o o---o o o +| | | | | | | | | | | | | | | +o o---o o o o o---o o o o o---o o o---o o +| | | | | | | | | | | | +o o---o o---o o o o o o o o o o---o o o +| | | | | +o o---o---o---o---o---o---o---o o o o o o---o o o +| | | | | | | | +o o o---o---o---o---o---o o---o---o---o---o---o o---o---o +| | | | | | | +o o o---o---o---o---o---o o o o o o o---o o o +| | | | | | | | | +o o---o---o---o---o---o o---o---o---o---o---o---o o o o +| | | G G | | | | | | | | +o o o---o---o---o---o---o o o o o o o o o o +| | | G G | | | +o o---o---o---o---o---o o---o---o o o o o o o o +| | | | | | | | | +o o o---o---o---o---o---o---o---o o---o---o---o---o o o +| | | | | | | +o o o o---o o---o o---o o---o o---o o---o o o +| | | | | | | | | | +o o o---o o---o o---o o---o o---o o---o o---o o +| | | | | | | | | | | +o o o o---o o---o o---o o---o o---o o---o o o +| | | | | | | | | | | +o o---o o o---o o---o o---o o---o o---o---o o o +| | | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/data/mazes/uk2016f.txt b/tests/data/mazes/uk2016f.txt new file mode 100644 index 0000000..e1123b0 --- /dev/null +++ b/tests/data/mazes/uk2016f.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o o---o---o---o o---o---o---o---o o +| | | | +o---o o o o---o---o---o---o---o o---o o---o o o o +| | | | | | | +o o o o---o o---o---o o---o---o---o o o o o o +| | | | | | | | | +o o o o o o o---o---o---o---o---o o o o o o +| | | | | | | | +o o---o---o o---o o o---o---o---o o---o o o o o +| | | | | | | | | | +o o o o---o o o o---o o o o o o o o o +| | | | | | | | | | | | | +o o o---o o o o o---o---o o o o o---o o o +| | | | | G G | | | | | | | +o o---o o---o---o---o o o o o o o o o---o o +| | | | G G | | | | | +o o o o o---o---o o---o---o---o o o o o o---o +| | | | | | | | | +o o o o---o---o---o---o o---o---o---o o o o o o +| | | | | | | | | +o o o o o---o---o o---o---o---o o o o o o o +| | | | | | | | | | +o o---o o o o o---o---o o---o---o o o o o o +| | | | | | | | +o o o---o o o---o o---o---o---o---o---o o o o o +| | | | | | | +o o---o o o---o---o---o---o---o o o---o---o---o o---o +| | | | | | +o o o---o---o o---o---o---o o---o---o o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/tests/nav/reference_robot.cpp b/tests/nav/reference_robot.cpp new file mode 100644 index 0000000..f4fd7b8 --- /dev/null +++ b/tests/nav/reference_robot.cpp @@ -0,0 +1,116 @@ +/** + * @file + */ + +#include +#include +#include + +#include + +#include "maze_reader.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/turn_table.hpp" +#include "reference_robot.hpp" + +namespace micras::test { +const nav::RobotModel reference_robot{ + .maze = {.cell_size = 0.18F, .wall_thickness = 0.0126F}, + .chassis = + { + .mass = 0.07F, + .yaw_inertia = 2.9e-5F, + .wheel_radius = 0.011F, + .rolling_compliance = 56e-6F, + .track_width = 0.04575F, + .half_width = 0.0251F, + .front_length = 0.0549F, + .rear_length = 0.0365F, + }, + .traction = + { + .friction_coefficient = 1.0F, + .fan_downforce = 3.0F, + .fan_offset = 0.0175F, + .lateral_compliance = 0.0048F, + }, + .drive = + { + .torque_constant = 0.00336F, + .resistance = 12.62F, + .gear_ratio = 5.25F, + .supply_voltage = 19.63F, + .static_friction_voltage = 0.09F, + }, + .wall_sensors = {{ + {.position = {.x = 0.0439F, .y = 0.0215F}, .angle = 0.0F, .half_angle = 0.09F}, + {.position = {.x = 0.0526F, .y = 0.0153F}, .angle = std::numbers::pi_v / 4.0F, .half_angle = 0.09F}, + {.position = {.x = 0.0526F, .y = -0.0153F}, .angle = -std::numbers::pi_v / 4.0F, .half_angle = 0.09F}, + {.position = {.x = 0.0439F, .y = -0.0215F}, .angle = 0.0F, .half_angle = 0.09F}, + }}, + .noise = + { + .gyroscope = 6.5e-5F, + .gyroscope_bias_walk = 1.0e-4F, + .wheel_angle = 2.0F * std::numbers::pi_v / 16384.0F, + .longitudinal_slip = 0.005F, + .longitudinal_slip_per_acceleration = 0.0005F, + .lateral_slip = 0.003F, + .wall_range = 0.001F, + .wall_range_per_meter = 0.017F, + }, + .gyroscope_scale = 1.0F, +}; + +const nav::Dynamics& reference_dynamics() { + static const nav::Dynamics dynamics{{ + .model = reference_robot, + .turns = + nav::TurnTable{ + reference_robot, turn_margin, nav::TurnTable::place(reference_robot, turn_margin, two_bend_designs) + }, + .risky_turns = + nav::TurnTable{ + reference_robot, risky_turn_margin, + nav::TurnTable::place(reference_robot, risky_turn_margin, risky_two_bend_designs) + }, + .max_linear_speed = 3.0F, + .max_angular_speed = 12.0F, + .voltage_reserve = 0.15F, + }}; + + return dynamics; +} + +nav::Route plan_route(const MazeText& maze, const nav::RunProfile& profile) { + using Planner = nav::TPlanner<16, 16>; + + nav::TMaze<16, 16> map{{.start = {.position = maze.start, .orientation = nav::Side::UP}, .goal = maze.goal}}; + fill_maze(maze, map); + + const auto planner = + std::make_unique(reference_dynamics(), Planner::Config{.start_distance = start_distance}); + planner->begin(map, nav::WallAssumption::PESSIMISTIC, profile); + + while (not planner->step(1000)) { } + + CHECK(planner->is_finished()); + CHECK(planner->is_exact()); + REQUIRE(planner->get_number_of_routes() > 0); + + nav::Route route; + planner->get_route(0, route); + + for (uint8_t index = 1; index < planner->get_number_of_routes(); index++) { + nav::Route candidate; + planner->get_route(index, candidate); + CHECK(candidate.time >= route.time); + } + + return route; +} +} // namespace micras::test diff --git a/tests/nav/reference_robot.hpp b/tests/nav/reference_robot.hpp new file mode 100644 index 0000000..257b355 --- /dev/null +++ b/tests/nav/reference_robot.hpp @@ -0,0 +1,169 @@ +/** + * @file + * + * @brief The robot the nav tests plan for: Micras v1 as the firmware describes it at MicrasFirmware + * 4dac6e2 (config/targets/v1/robot.hpp, config/turn_margins.hpp and config/two_bend_turns.hpp). + * + * @note A real robot, so that the planner and the turn tables are tested on the geometry and the + * dynamics they are tuned for. The model is defined in its own source, so that nothing built from it + * here is a constant expression. + */ + +#ifndef MICRAS_LIB_TESTS_NAV_REFERENCE_ROBOT_HPP +#define MICRAS_LIB_TESTS_NAV_REFERENCE_ROBOT_HPP + +#include +#include +#include + +#include "maze_reader.hpp" +#include "micras/nav/lattice.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/robot_model.hpp" +#include "micras/nav/turn_table.hpp" + +namespace micras::test { +/** + * @brief Physical description of the robot and of the classic maze. + */ +extern const nav::RobotModel reference_robot; + +/** + * @brief Distance kept between the outline of the robot and any obstacle, without and with the + * risky switch. + */ +///@{ +inline constexpr float turn_margin{0.015F}; +inline constexpr float risky_turn_margin{0.010F}; +///@} + +/** + * @brief Distance from the start pose of a run to the first wall it crosses. + */ +inline constexpr float start_distance{0.18F - (0.04F + 0.0126F / 2.0F)}; + +/** + * @brief Designs of the turns of two bends, with the normal margin. + */ +inline constexpr std::array two_bend_designs{{ + {.first_angle = -0.436332315F, + .first_curvature = 11.8644524F, + .second_angle = 1.22173047F, + .second_curvature = 11.8644524F, + .pre = 0.0F}, + {.first_angle = 1.26536369F, + .first_curvature = 17.934948F, + .second_angle = -0.479965538F, + .second_curvature = 17.934948F, + .pre = 0.0F}, + {.first_angle = -0.0872664601F, + .first_curvature = 5.19209814F, + .second_angle = 2.44346094F, + .second_curvature = 19.8867226F, + .pre = 0.00999999978F}, + {.first_angle = -0.479965538F, + .first_curvature = 17.934948F, + .second_angle = 1.26536369F, + .second_curvature = 17.934948F, + .pre = 0.0F}, + {.first_angle = -1.39626336F, + .first_curvature = 19.8867226F, + .second_angle = 1.39626336F, + .second_curvature = 19.8867226F, + .pre = 0.0F}, + {.first_angle = -1.13446403F, + .first_curvature = 13.1556044F, + .second_angle = 0.34906584F, + .second_curvature = 9.64987087F, + .pre = 0.0F}, + {.first_angle = -0.741764903F, + .first_curvature = 13.1556044F, + .second_angle = -0.829031408F, + .second_curvature = 13.1556044F, + .pre = 0.0F}, + {.first_angle = -2.44346094F, + .first_curvature = 19.8867226F, + .second_angle = 0.0872664601F, + .second_curvature = 4.22295713F, + .pre = 0.0F}, +}}; + +/** + * @brief Designs of the turns of two bends, with the risky margin. + */ +inline constexpr std::array risky_two_bend_designs{{ + {.first_angle = -0.436332315F, + .first_curvature = 11.8644524F, + .second_angle = 1.22173047F, + .second_curvature = 11.8644524F, + .pre = 0.0F}, + {.first_angle = 1.26536369F, + .first_curvature = 17.934948F, + .second_angle = -0.479965538F, + .second_curvature = 17.934948F, + .pre = 0.0F}, + {.first_angle = -0.0872664601F, + .first_curvature = 5.19209814F, + .second_angle = 2.44346094F, + .second_curvature = 19.8867226F, + .pre = 0.00999999978F}, + {.first_angle = -0.479965538F, + .first_curvature = 17.934948F, + .second_angle = 1.26536369F, + .second_curvature = 17.934948F, + .pre = 0.0F}, + {.first_angle = -1.39626336F, + .first_curvature = 19.8867226F, + .second_angle = 1.39626336F, + .second_curvature = 19.8867226F, + .pre = 0.0F}, + {.first_angle = -1.22173047F, + .first_curvature = 11.8644524F, + .second_angle = 0.436332315F, + .second_curvature = 11.8644524F, + .pre = 0.0F}, + {.first_angle = -0.741764903F, + .first_curvature = 9.64987087F, + .second_angle = -0.829031408F, + .second_curvature = 9.64987087F, + .pre = 0.0F}, + {.first_angle = -2.44346094F, + .first_curvature = 19.8867226F, + .second_angle = 0.0872664601F, + .second_curvature = 4.22295713F, + .pre = 0.0F}, +}}; + +/** + * @brief Profile of a fast run with the fan on, the normal turns and the normal share of the traction. + */ +inline constexpr nav::RunProfile fast_profile{ + .racing_line = false, + .fan = true, + .risky = false, + .utilization = 0.6F, + .max_speed = std::numeric_limits::infinity(), +}; + +/** + * @brief Get the dynamics of the robot, with both turn tables designed when first asked for. + * + * @return The dynamics, shared by every test of the program. + */ +const nav::Dynamics& reference_dynamics(); + +/** + * @brief Plan the fastest route of a maze whose walls are all known, from the start cell facing up. + * + * @note Checks on the way that the search finished, kept every label it needed, and ranked its + * candidates by their time. + * + * @param maze The maze, with its goal. + * @param profile The profile of the run. + * @return The fastest route the search found. + */ +nav::Route plan_route(const MazeText& maze, const nav::RunProfile& profile); +} // namespace micras::test + +#endif // MICRAS_LIB_TESTS_NAV_REFERENCE_ROBOT_HPP diff --git a/tests/nav/test_maze.cpp b/tests/nav/test_maze.cpp new file mode 100644 index 0000000..402f33b --- /dev/null +++ b/tests/nav/test_maze.cpp @@ -0,0 +1,131 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "maze_reader.hpp" +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" + +namespace micras::test { +namespace { +using Maze = nav::TMaze<16, 16>; + +const std::filesystem::path mazes_directory{MICRAS_LIB_TEST_MAZES}; + +constexpr nav::GridPose maze_start{.position = {.x = 0, .y = 0}, .orientation = nav::Side::UP}; + +constexpr std::array maze_goal{ + {{.x = 8, .y = 8}, {.x = 7, .y = 8}, {.x = 8, .y = 7}, {.x = 7, .y = 7}} +}; + +void check_same_walls(const Maze& first, const Maze& second) { + for (uint8_t y = 0; y < 16; y++) { + for (uint8_t x = 0; x < 16; x++) { + for (const nav::Side side : nav::all_sides) { + const nav::GridPose wall{.position = {.x = x, .y = y}, .orientation = side}; + CAPTURE(x); + CAPTURE(y); + CAPTURE(static_cast(side)); + REQUIRE(first.get_wall(wall) == second.get_wall(wall)); + } + } + } +} +} // namespace + +TEST_SUITE("maze") { + TEST_CASE("reads the contest mazes with their start and goal") { + for (const auto& entry : std::filesystem::directory_iterator{mazes_directory}) { + CAPTURE(entry.path().filename().string()); + const MazeText text = read_maze(entry.path()); + + CHECK(text.width == 16); + CHECK(text.height == 16); + CHECK(text.walls.size() == 2 * 16 * 16 + 2 * 16); + CHECK(text.start == maze_start.position); + CHECK(text.goal.size() == 4); + } + } + + TEST_CASE("round trips a whole maze through its serialization") { + const MazeText text = read_maze(mazes_directory / "maze1.txt"); + Maze maze{{.start = maze_start, .goal = maze_goal}}; + fill_maze(text, maze); + + const std::vector bytes = maze.serialize(); + CHECK(bytes.size() == 3 + 16 * 16 / 2); + CHECK(bytes.at(1) == 16); + CHECK(bytes.at(2) == 16); + + Maze copy{{.start = maze_start, .goal = maze_goal}}; + copy.deserialize(bytes.data(), static_cast(bytes.size())); + + check_same_walls(maze, copy); + CHECK(copy.get_cost(maze_start.position) == maze.get_cost(maze_start.position)); + } + + TEST_CASE("keeps the unknown walls unknown through the serialization") { + const MazeText text = read_maze(mazes_directory / "japan2017ef.txt"); + Maze maze{{.start = maze_start, .goal = maze_goal}}; + + for (std::size_t index = 0; index < text.walls.size(); index += 3) { + maze.set_wall(text.walls.at(index).first, text.walls.at(index).second); + } + + const std::vector bytes = maze.serialize(); + Maze copy{{.start = maze_start, .goal = maze_goal}}; + copy.deserialize(bytes.data(), static_cast(bytes.size())); + + check_same_walls(maze, copy); + } + + TEST_CASE("ignores a serialization of another format or size") { + const MazeText text = read_maze(mazes_directory / "maze1.txt"); + Maze maze{{.start = maze_start, .goal = maze_goal}}; + fill_maze(text, maze); + + const Maze fresh{{.start = maze_start, .goal = maze_goal}}; + Maze copy{{.start = maze_start, .goal = maze_goal}}; + + std::vector bytes = maze.serialize(); + bytes.at(0) ^= 0xFFU; + copy.deserialize(bytes.data(), static_cast(bytes.size())); + check_same_walls(copy, fresh); + + bytes = maze.serialize(); + copy.deserialize(bytes.data(), static_cast(bytes.size() - 1)); + check_same_walls(copy, fresh); + } + + TEST_CASE("floods the cost to the goal through the known walls") { + const MazeText text = read_maze(mazes_directory / "maze1.txt"); + Maze maze{{.start = maze_start, .goal = maze_goal}}; + fill_maze(text, maze); + + for (const nav::GridPoint& cell : maze_goal) { + CHECK(maze.get_cost(cell) == 0); + } + + nav::GridPose pose = maze_start; + uint16_t steps = 0; + + while (not maze.is_goal(pose.position) and steps < 256) { + const auto next = maze.get_next(pose); + REQUIRE(next.has_value()); + CHECK(maze.get_cost(next->position) + 1 == maze.get_cost(pose.position)); + pose = next.value(); + steps++; + } + + CHECK(maze.is_goal(pose.position)); + CHECK(steps == maze.get_cost(maze_start.position)); + } +} +} // namespace micras::test diff --git a/tests/nav/test_planner.cpp b/tests/nav/test_planner.cpp new file mode 100644 index 0000000..5ffc87d --- /dev/null +++ b/tests/nav/test_planner.cpp @@ -0,0 +1,71 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include + +#include "maze_reader.hpp" +#include "micras/nav/planner.hpp" +#include "reference_robot.hpp" + +namespace micras::test { +namespace { +struct Golden { + std::string_view maze; + float time; + std::size_t steps; +}; + +constexpr std::array golden_routes{{ + {.maze = "alljapan-033-2012-exp-fin", .time = 5.28452778F, .steps = 19}, + {.maze = "apec2016", .time = 9.0424633F, .steps = 26}, + {.maze = "apec2017", .time = 7.20180178F, .steps = 24}, + {.maze = "apec2018", .time = 5.90276146F, .steps = 17}, + {.maze = "apec2019", .time = 7.93852806F, .steps = 34}, + {.maze = "japan2013ef", .time = 7.11609554F, .steps = 27}, + {.maze = "japan2017ef", .time = 7.34178638F, .steps = 51}, + {.maze = "maze1", .time = 3.97810149F, .steps = 21}, + {.maze = "maze2", .time = 3.71197319F, .steps = 10}, + {.maze = "uk2016f", .time = 4.59111643F, .steps = 23}, +}}; +} // namespace + +TEST_SUITE("planner") { + TEST_CASE("finds the golden route of every contest maze") { + for (const Golden& golden : golden_routes) { + CAPTURE(golden.maze); + const MazeText text = + read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / (std::string{golden.maze} + ".txt")); + const nav::Route route = plan_route(text, fast_profile); + + CHECK(route.time == doctest::Approx(golden.time).epsilon(1e-6)); + CHECK(route.steps.size() == golden.steps); + } + } + + TEST_CASE("drives no slower once walls are taken out of the maze") { + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "maze2.txt"); + MazeText opened = text; + + for (std::size_t index = 0; index < opened.walls.size(); index += 16) { + opened.walls.at(index).second = false; + } + + CHECK(plan_route(opened, fast_profile).time <= plan_route(text, fast_profile).time); + } + + TEST_CASE("drives faster with the fan than without") { + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "maze1.txt"); + nav::RunProfile no_fan = fast_profile; + no_fan.fan = false; + + CHECK(plan_route(text, fast_profile).time < plan_route(text, no_fan).time); + } +} +} // namespace micras::test diff --git a/tests/nav/test_route_compiler.cpp b/tests/nav/test_route_compiler.cpp new file mode 100644 index 0000000..247416a --- /dev/null +++ b/tests/nav/test_route_compiler.cpp @@ -0,0 +1,151 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include + +#include "maze_reader.hpp" +#include "micras/nav/lattice.hpp" +#include "micras/nav/planner.hpp" +#include "micras/nav/route_compiler.hpp" +#include "micras/nav/segment.hpp" +#include "micras/nav/state.hpp" +#include "reference_robot.hpp" + +namespace micras::test { +namespace { +constexpr float tolerance{1e-3F}; + +const nav::LatticePose first_node{.point = {.x = 1, .y = 2}, .heading = 2}; + +nav::Pose end_of(const nav::Segment& segment, const nav::RunProfile& profile) { + if (segment.kind == nav::SegmentKind::STRAIGHT) { + return segment.start.compose({.position = {.x = segment.length, .y = 0.0F}, .orientation = 0.0F}); + } + + const nav::TurnShape& shape = reference_dynamics().get_turn(profile, segment.turn); + const auto end = shape.sample(shape.length()); + const float side = segment.length < 0.0F ? -1.0F : 1.0F; + + return segment.start.compose({.position = {.x = end.x, .y = side * end.y}, .orientation = side * end.heading}); +} + +bool same_pose(const nav::Pose& first, const nav::Pose& second) { + const float heading = std::remainder(first.orientation - second.orientation, 2.0F * std::numbers::pi_v); + + return std::abs(first.position.x - second.position.x) < tolerance and + std::abs(first.position.y - second.position.y) < tolerance and std::abs(heading) < tolerance; +} + +std::vector compile(const nav::Route& route) { + std::vector segments; + nav::RouteCompiler::compile(route, reference_dynamics(), fast_profile, start_distance, segments); + return segments; +} +} // namespace + +TEST_SUITE("route_compiler") { + TEST_CASE("drives a straight route as one straight from the start pose") { + const nav::Route route{ + .start = first_node, + .steps = {{.run = 3, .has_turn = false, .turn = nav::TurnId::SS90S, .side = nav::TurnSide::LEFT}}, + .stop_distance = 0.09F, + .finish_distance = 0.0F, + .time = 1.0F, + }; + const float cell_size = reference_robot.maze.cell_size; + + const std::vector segments = compile(route); + + REQUIRE(segments.size() == 1); + CHECK(segments.front().kind == nav::SegmentKind::STRAIGHT); + CHECK(segments.front().length == doctest::Approx(start_distance + 3 * cell_size + 0.09F)); + CHECK(same_pose( + segments.front().start, {.position = {.x = cell_size / 2.0F, .y = cell_size - start_distance}, + .orientation = std::numbers::pi_v / 2.0F} + )); + } + + TEST_CASE("places a turn between the straights of its nodes") { + const nav::Route route{ + .start = first_node, + .steps = + {{.run = 1, .has_turn = true, .turn = nav::TurnId::SS90S, .side = nav::TurnSide::RIGHT}, + {.run = 2, .has_turn = false, .turn = nav::TurnId::SS90S, .side = nav::TurnSide::LEFT}}, + .stop_distance = 0.09F, + .finish_distance = 0.0F, + .time = 1.0F, + }; + const nav::TurnShape& shape = reference_dynamics().get_turn(fast_profile, nav::TurnId::SS90S); + const float cell_size = reference_robot.maze.cell_size; + + const std::vector segments = compile(route); + + REQUIRE(segments.size() == 3); + CHECK(segments.at(0).kind == nav::SegmentKind::STRAIGHT); + CHECK(segments.at(1).kind == nav::SegmentKind::TURN); + CHECK(segments.at(2).kind == nav::SegmentKind::STRAIGHT); + CHECK(segments.at(0).length == doctest::Approx(start_distance + cell_size + shape.pre)); + CHECK(segments.at(1).length == doctest::Approx(-shape.length())); + CHECK(segments.at(2).length == doctest::Approx(shape.post + 2 * cell_size + 0.09F)); + CHECK(same_pose(end_of(segments.at(0), fast_profile), segments.at(1).start)); + CHECK(same_pose(end_of(segments.at(1), fast_profile), segments.at(2).start)); + CHECK(same_pose( + end_of(segments.at(2), fast_profile), + {.position = {.x = cell_size + 2 * cell_size + 0.09F, .y = 2.5F * cell_size}, .orientation = 0.0F} + )); + } + + TEST_CASE("chains the segments of a planned route into the goal") { + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "japan2017ef.txt"); + const nav::Route route = plan_route(text, fast_profile); + const float cell_size = reference_robot.maze.cell_size; + + const std::vector segments = compile(route); + + std::size_t turns = 0; + + for (std::size_t index = 0; index < segments.size(); index++) { + CAPTURE(index); + const nav::Segment& segment = segments.at(index); + + if (segment.kind == nav::SegmentKind::TURN) { + turns++; + } else { + CHECK(segment.length > 0.0F); + } + + if (index + 1 < segments.size()) { + CHECK(same_pose(end_of(segment, fast_profile), segments.at(index + 1).start)); + } + } + + std::size_t route_turns = 0; + + for (const nav::RouteStep& step : route.steps) { + route_turns += step.has_turn ? 1 : 0; + } + + CHECK(turns == route_turns); + + const nav::Pose stop = end_of(segments.back(), fast_profile); + const nav::GridPoint cell{ + .x = static_cast(stop.position.x / cell_size), + .y = static_cast(stop.position.y / cell_size) + }; + bool in_goal = false; + + for (const nav::GridPoint& goal : text.goal) { + in_goal = in_goal or goal == cell; + } + + CHECK(in_goal); + } +} +} // namespace micras::test diff --git a/tests/nav/test_turn_table.cpp b/tests/nav/test_turn_table.cpp new file mode 100644 index 0000000..76e76d4 --- /dev/null +++ b/tests/nav/test_turn_table.cpp @@ -0,0 +1,98 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include + +#include "micras/nav/lattice.hpp" +#include "micras/nav/motion_limits.hpp" +#include "micras/nav/state.hpp" +#include "micras/nav/turn_table.hpp" +#include "reference_robot.hpp" + +namespace micras::test { +TEST_SUITE("turn_table") { + TEST_CASE("fits every turn in the maze with the normal margin") { + const nav::TurnTable table{ + reference_robot, turn_margin, nav::TurnTable::place(reference_robot, turn_margin, two_bend_designs) + }; + + CHECK(table.is_valid()); + } + + TEST_CASE("fits every turn in the maze with the risky margin") { + const nav::TurnTable table{ + reference_robot, risky_turn_margin, + nav::TurnTable::place(reference_robot, risky_turn_margin, risky_two_bend_designs) + }; + + CHECK(table.is_valid()); + } + + TEST_CASE("clears the walls with every turn of two bends") { + CHECK(nav::TurnTable::clears(reference_robot, turn_margin, two_bend_designs)); + CHECK(nav::TurnTable::clears(reference_robot, risky_turn_margin, risky_two_bend_designs)); + } + + TEST_CASE("rejects the designs of a smaller margin with a larger one") { + CHECK_FALSE(nav::TurnTable::clears(reference_robot, 0.03F, risky_two_bend_designs)); + } + + TEST_CASE("rejects a turn nothing fits") { + std::array designs = two_bend_designs; + designs.front().first_curvature = 0.0F; + + CHECK_FALSE(nav::TurnTable::clears(reference_robot, turn_margin, designs)); + + const nav::TurnTable table{ + reference_robot, turn_margin, nav::TurnTable::place(reference_robot, turn_margin, designs) + }; + CHECK_FALSE(table.is_valid()); + } + + TEST_CASE("ends every turn on the exit node of its primitive") { + const float cell_size = reference_robot.maze.cell_size; + + for (const bool risky : {false, true}) { + nav::RunProfile profile = fast_profile; + profile.risky = risky; + + for (uint8_t index = 0; index < nav::number_of_turns; index++) { + const auto turn = static_cast(index); + CAPTURE(risky); + CAPTURE(index); + + const nav::TurnShape& shape = reference_dynamics().get_turn(profile, turn); + const nav::LatticePose entry{ + .point = {.x = 2, .y = 1}, + .heading = static_cast(nav::get_primitive(turn).diagonal_entry ? 1 : 0), + }; + const nav::LatticePose exit = nav::get_turn_exit(entry, turn, nav::TurnSide::LEFT); + const nav::Pose expected = exit.to_pose(cell_size); + + const auto end = shape.sample(shape.length()); + const nav::Pose curve_start = + entry.to_pose(cell_size).compose({.position = {.x = shape.pre, .y = 0.0F}, .orientation = 0.0F}); + const nav::Pose curve_end = + curve_start.compose({.position = {.x = end.x, .y = end.y}, .orientation = end.heading}); + const nav::Pose reached = + curve_end.compose({.position = {.x = shape.post, .y = 0.0F}, .orientation = 0.0F}); + + CHECK(shape.valid); + CHECK(std::abs(reached.position.x - expected.position.x) < 1e-3F); + CHECK(std::abs(reached.position.y - expected.position.y) < 1e-3F); + CHECK( + std::abs( + std::remainder(reached.orientation - expected.orientation, 2.0F * std::numbers::pi_v) + ) < 1e-3F + ); + } + } + } +} +} // namespace micras::test diff --git a/tests/support/maze_reader.hpp b/tests/support/maze_reader.hpp new file mode 100644 index 0000000..cccedab --- /dev/null +++ b/tests/support/maze_reader.hpp @@ -0,0 +1,136 @@ +/** + * @file + * + * @brief Reader of the text mazes the simulator draws and the nav tests plan on. + */ + +#ifndef MICRAS_LIB_TESTS_MAZE_READER_HPP +#define MICRAS_LIB_TESTS_MAZE_READER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/nav/grid_pose.hpp" +#include "micras/nav/maze.hpp" + +namespace micras::test { +/** + * @brief The walls of a maze, with its start and its goal. + * + * @note The text has a line of posts and horizontal walls (`o---o`) above and below every row of + * cells, and a line of vertical walls (`|`) through every row, the top line first. A cell is four + * characters wide, with an `S` in the start cell and a `G` in every goal cell. + */ +struct MazeText { + /** + * @brief Size of the maze in cells. + */ + ///@{ + uint8_t width{}; + uint8_t height{}; + ///@} + + /** + * @brief Every wall of the maze, once: the right and upper wall of every cell, and the left and + * lower walls of the border, each with whether it is present. + */ + std::vector> walls; + + /** + * @brief Cell marked as the start. + */ + nav::GridPoint start{}; + + /** + * @brief Cells marked as the goal. + */ + std::vector goal; +}; + +/** + * @brief Read a maze from its text. + * + * @param path Path of the text file. + * @return The maze. + */ +inline MazeText read_maze(const std::filesystem::path& path) { + std::ifstream file{path}; + std::vector lines; + + for (std::string line; std::getline(file, line);) { + lines.push_back(line); + } + + if (lines.size() < 3 or lines.size() % 2 == 0 or lines.front().size() < 5 or lines.front().size() % 4 != 1) { + throw std::runtime_error{"not a maze: " + path.string()}; + } + + MazeText maze{ + .width = static_cast(lines.front().size() / 4), + .height = static_cast(lines.size() / 2), + .walls = {}, + .start = {}, + .goal = {}, + }; + + const auto character = [&lines](std::size_t row, std::size_t column) { + const std::string& line = lines.at(row); + return column < line.size() ? line.at(column) : ' '; + }; + + for (uint8_t y = 0; y < maze.height; y++) { + const std::size_t row = 2 * static_cast(maze.height - 1 - y) + 1; + + for (uint8_t x = 0; x < maze.width; x++) { + const std::size_t column = 4 * static_cast(x); + const nav::GridPoint cell{.x = x, .y = y}; + + maze.walls.emplace_back(nav::GridPose{cell, nav::Side::RIGHT}, character(row, column + 4) == '|'); + maze.walls.emplace_back(nav::GridPose{cell, nav::Side::UP}, character(row - 1, column + 2) == '-'); + + if (x == 0) { + maze.walls.emplace_back(nav::GridPose{cell, nav::Side::LEFT}, character(row, column) == '|'); + } + + if (y == 0) { + maze.walls.emplace_back(nav::GridPose{cell, nav::Side::DOWN}, character(row + 1, column + 2) == '-'); + } + + const char mark = character(row, column + 2); + + if (mark == 'S') { + maze.start = cell; + } else if (mark == 'G') { + maze.goal.push_back(cell); + } + } + } + + return maze; +} + +/** + * @brief Make every wall of a maze known to a map of the same size. + * + * @tparam width Width of the maze in cells. + * @tparam height Height of the maze in cells. + * @param text The maze read from its text. + * @param maze The map to fill. + */ +template +void fill_maze(const MazeText& text, nav::TMaze& maze) { + for (const auto& [wall, present] : text.walls) { + maze.set_wall(wall, present); + } + + maze.flood(maze.get_goal()); +} +} // namespace micras::test + +#endif // MICRAS_LIB_TESTS_MAZE_READER_HPP From 995b82ccd7b57fbe5b2afd22696d9559edc1e516 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 06:19:48 -0300 Subject: [PATCH 535/561] =?UTF-8?q?=F0=9F=9A=A8=20Lint=20the=20tpp=20files?= =?UTF-8?q?=20and=20the=20ported=20tests=20with=20clang-tidy=2022?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The header filter now reaches the .tpp files and the tests get their first pass. Helpers of the host backend, the chip models and the tests are static functions, includes name what each file uses, and test values are compared without implicit float promotions. The findings in the navigation's hot paths and in the models' byte loops are suppressed with their reason, since rewriting them moves the robot's image; the deserialized nibble is masked in int, which keeps both the image and a sanitized build without a sign conversion. The lint targets take the library root as an absolute path, so the sources they pass match the compilation database. --- micras_hal/host/src/adc_dma.cpp | 1 + micras_hal/host/src/board.cpp | 16 ++- micras_hal/host/src/clock.cpp | 1 + micras_hal/host/src/crc.cpp | 6 +- micras_hal/host/src/encoder.cpp | 3 +- micras_hal/host/src/flash.cpp | 7 +- micras_hal/host/src/fmac.cpp | 5 +- micras_hal/host/src/gpio.cpp | 1 + micras_hal/host/src/mcu.cpp | 3 + micras_hal/host/src/ports.cpp | 3 + micras_hal/host/src/pwm.cpp | 5 +- micras_hal/host/src/pwm_dma.cpp | 5 +- micras_hal/host/src/spi.cpp | 15 ++- micras_hal/host/src/uart_dma.cpp | 1 + .../include/micras/nav/impl/curve_speed.tpp | 4 + micras_nav/include/micras/nav/impl/maze.tpp | 8 +- .../include/micras/nav/impl/planner.tpp | 2 + .../include/micras/nav/impl/wall_model.tpp | 5 + micras_nav/src/grid_pose.cpp | 2 +- micras_proxy/models/src/as5047u_model.cpp | 4 + micras_proxy/models/src/lsm6dsv_model.cpp | 8 +- micras_proxy/src/imu.cpp | 1 - micras_proxy/src/storage.cpp | 2 +- tests/comm/test_frame.cpp | 105 ++++++++++-------- tests/comm/test_link.cpp | 28 +++-- tests/core/test_cobs.cpp | 14 +-- tests/core/test_variable_pool.cpp | 5 +- tests/hal/test_adc.cpp | 3 +- tests/hal/test_board.cpp | 1 + tests/hal/test_crc.cpp | 4 +- tests/hal/test_flash.cpp | 7 +- tests/hal/test_fmac.cpp | 5 +- tests/hal/test_spi.cpp | 17 +-- tests/hal/test_timers.cpp | 27 +++-- tests/hal/test_uart.cpp | 1 + tests/models/test_as5047u_model.cpp | 22 ++-- tests/models/test_lsm6dsv_model.cpp | 18 +-- tests/nav/test_maze.cpp | 39 ++++--- tests/nav/test_planner.cpp | 3 +- tests/nav/test_route_compiler.cpp | 29 +++-- tests/proxy/test_fmac_filter.cpp | 9 +- tests/proxy/test_imu.cpp | 10 +- tests/proxy/test_rotary_sensor.cpp | 54 +++++---- tests/proxy/test_storage.cpp | 3 +- tests/support/maze_reader.hpp | 17 ++- 45 files changed, 326 insertions(+), 203 deletions(-) diff --git a/micras_hal/host/src/adc_dma.cpp b/micras_hal/host/src/adc_dma.cpp index 3c3bca2..96f4164 100644 --- a/micras_hal/host/src/adc_dma.cpp +++ b/micras_hal/host/src/adc_dma.cpp @@ -9,6 +9,7 @@ #include "micras/hal/adc_dma.hpp" #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" namespace micras::hal { std::array AdcDma::instances{}; diff --git a/micras_hal/host/src/board.cpp b/micras_hal/host/src/board.cpp index e27804f..4d6c9e4 100644 --- a/micras_hal/host/src/board.cpp +++ b/micras_hal/host/src/board.cpp @@ -2,16 +2,20 @@ * @file */ -#include #include #include #include #include #include +#include +#include #include #include +#include #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" +#include "micras/hal/host/spi_device.hpp" namespace micras::hal::host { namespace { @@ -42,13 +46,14 @@ struct Registry { FlashPort flash{{.name = "flash", .touched = false, .bound = false}, {}, std::numeric_limits::max()}; McuPort mcu{{.name = "mcu", .touched = false, .bound = false}, 0, 0, 0, 0}; }; +} // namespace /** * @brief Get the registry. * * @return The process-wide registry. */ -Registry& registry() { +static Registry& registry() { static Registry instance; return instance; } @@ -59,7 +64,7 @@ Registry& registry() { * @param handle Peripheral handle. * @return Its name. */ -std::string handle_name(const void* handle) { +static std::string handle_name(const void* handle) { const auto found = registry().handle_names.find(handle); return found != registry().handle_names.end() ? found->second : std::format("handle@{:#x}", std::bit_cast(handle)); @@ -71,7 +76,7 @@ std::string handle_name(const void* handle) { * @param key GPIO port and pin mask. * @return Its name. */ -std::string gpio_name(const Key& key) { +static std::string gpio_name(const Key& key) { const auto found = registry().gpio_names.find(key); return found != registry().gpio_names.end() ? found->second : std::format("{} pin {:#06x}", handle_name(key.first), key.second); @@ -86,7 +91,7 @@ std::string gpio_name(const Key& key) { * @return The port. */ template -typename Map::mapped_type& find_or_create(Map& ports, const typename Map::key_type& key, const Namer& name) { +static Map::mapped_type& find_or_create(Map& ports, const typename Map::key_type& key, const Namer& name) { const auto [found, created] = ports.try_emplace(key); if (created) { @@ -95,7 +100,6 @@ typename Map::mapped_type& find_or_create(Map& ports, const typename Map::key_ty return found->second; } -} // namespace GpioPort& Board::gpio(const void* port, uint16_t pin) { const Key key{port, pin}; diff --git a/micras_hal/host/src/clock.cpp b/micras_hal/host/src/clock.cpp index 27206dc..b92d9b9 100644 --- a/micras_hal/host/src/clock.cpp +++ b/micras_hal/host/src/clock.cpp @@ -2,6 +2,7 @@ * @file */ +#include #include #include "micras/hal/host/clock.hpp" diff --git a/micras_hal/host/src/crc.cpp b/micras_hal/host/src/crc.cpp index 4dc6923..cb033e0 100644 --- a/micras_hal/host/src/crc.cpp +++ b/micras_hal/host/src/crc.cpp @@ -8,7 +8,6 @@ #include "micras/hal/crc.hpp" namespace micras::hal { -namespace { /** * @brief Reverse the order of the lowest bits of a value. * @@ -16,7 +15,7 @@ namespace { * @param width Number of bits to reflect. * @return The reflected bits. */ -uint32_t reflect(uint32_t value, uint32_t width) { +static uint32_t reflect(uint32_t value, uint32_t width) { uint32_t reflected = 0; for (uint32_t bit = 0; bit < width; bit++) { @@ -32,7 +31,7 @@ uint32_t reflect(uint32_t value, uint32_t width) { * @param length CRCLength field. * @return Width in bits. */ -uint32_t width_of(uint32_t length) { +static uint32_t width_of(uint32_t length) { if (length == CRC_POLYLENGTH_7B) { return 7; } @@ -47,7 +46,6 @@ uint32_t width_of(uint32_t length) { return 32; } -} // namespace Crc::Crc(const Config& config) : handle{config.handle} { } diff --git a/micras_hal/host/src/encoder.cpp b/micras_hal/host/src/encoder.cpp index 562b1e4..7d9b939 100644 --- a/micras_hal/host/src/encoder.cpp +++ b/micras_hal/host/src/encoder.cpp @@ -6,9 +6,10 @@ #include "micras/hal/encoder.hpp" #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" namespace micras::hal { -Encoder::Encoder(const Config& config) : handle{config.handle}, start_count{0} { +Encoder::Encoder(const Config& config) : handle{config.handle} { if (this->handle->State == HAL_TIM_STATE_RESET) { config.init_function(); } diff --git a/micras_hal/host/src/flash.cpp b/micras_hal/host/src/flash.cpp index 26422e5..2e2388d 100644 --- a/micras_hal/host/src/flash.cpp +++ b/micras_hal/host/src/flash.cpp @@ -10,16 +10,16 @@ #include "micras/hal/flash.hpp" #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" namespace micras::hal { -namespace { /** * @brief Round a size up to whole flash words. * * @param size Size in bytes. * @return The rounded size. */ -constexpr uint32_t align_size(uint32_t size) { +static constexpr uint32_t align_size(uint32_t size) { return (size + FlashWord::size - 1) / FlashWord::size * FlashWord::size; } @@ -28,7 +28,7 @@ constexpr uint32_t align_size(uint32_t size) { * * @return The port. */ -host::FlashPort& storage() { +static host::FlashPort& storage() { host::FlashPort& port = host::Board::flash(); if (port.bytes.size() != Flash::total_size) { @@ -38,7 +38,6 @@ host::FlashPort& storage() { port.touched = true; return port; } -} // namespace FlashWord::FlashWord(std::span data) { const auto data_address = std::bit_cast(data.data()); diff --git a/micras_hal/host/src/fmac.cpp b/micras_hal/host/src/fmac.cpp index 90679fc..5f71b66 100644 --- a/micras_hal/host/src/fmac.cpp +++ b/micras_hal/host/src/fmac.cpp @@ -7,6 +7,7 @@ #include "micras/hal/fmac.hpp" #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" namespace micras::hal { Fmac::Fmac(const Config& config) : handle{config.handle} { @@ -26,13 +27,15 @@ bool Fmac::configure_iir(std::span feed_forward, std::spanhandle); port.touched = true; port.configure(feed_forward, feedback); + this->output_buffer_size = static_cast(this->output_buffer.size()); this->initialized = true; return true; } int16_t Fmac::update(int16_t sample) { - return host::Board::fmac(this->handle).filter(sample); + this->output_buffer.front() = host::Board::fmac(this->handle).filter(sample); + return this->output_buffer.front(); } bool Fmac::was_initialized() const { diff --git a/micras_hal/host/src/gpio.cpp b/micras_hal/host/src/gpio.cpp index a340c94..9c868c5 100644 --- a/micras_hal/host/src/gpio.cpp +++ b/micras_hal/host/src/gpio.cpp @@ -4,6 +4,7 @@ #include "micras/hal/gpio.hpp" #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" namespace micras::hal { Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { } diff --git a/micras_hal/host/src/mcu.cpp b/micras_hal/host/src/mcu.cpp index 00d48dd..d896680 100644 --- a/micras_hal/host/src/mcu.cpp +++ b/micras_hal/host/src/mcu.cpp @@ -9,11 +9,14 @@ #include #include +#include #include +#include "micras/hal/gpio.hpp" #include "micras/hal/host/board.hpp" #include "micras/hal/host/clock.hpp" #include "micras/hal/mcu.hpp" +#include "micras/hal/pwm.hpp" #include "micras/hal/timer.hpp" namespace micras::hal { diff --git a/micras_hal/host/src/ports.cpp b/micras_hal/host/src/ports.cpp index 53260ea..71c4453 100644 --- a/micras_hal/host/src/ports.cpp +++ b/micras_hal/host/src/ports.cpp @@ -12,11 +12,13 @@ namespace micras::hal::host { void AdcPort::write(std::size_t index, uint32_t counts) const { + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) bounded just above if (index < this->buffer16.size()) { this->buffer16[index] = static_cast(counts); } else if (index < this->buffer32.size()) { this->buffer32[index] = counts; } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) } void AdcPort::finish_sequence() const { @@ -30,6 +32,7 @@ void UartPort::receive(uint8_t byte) { return; } + // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) the head wraps below the size this->rx_buffer[this->rx_head] = byte; this->rx_head = (this->rx_head + 1) % this->rx_buffer.size(); } diff --git a/micras_hal/host/src/pwm.cpp b/micras_hal/host/src/pwm.cpp index 8d21122..b3fb92b 100644 --- a/micras_hal/host/src/pwm.cpp +++ b/micras_hal/host/src/pwm.cpp @@ -6,6 +6,7 @@ #include #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" #include "micras/hal/pwm.hpp" namespace micras::hal { @@ -14,6 +15,7 @@ namespace { * @brief Mask of the channel offset, which the vendor HAL uses as a bit shift. */ constexpr uint32_t channel_shift_mask{0x1F}; +} // namespace /** * @brief Check whether a timer counts up and down. @@ -21,10 +23,9 @@ constexpr uint32_t channel_shift_mask{0x1F}; * @param handle Timer handle. * @return True in a center-aligned mode. */ -bool is_center_aligned(const TIM_HandleTypeDef* handle) { +static bool is_center_aligned(const TIM_HandleTypeDef* handle) { return (handle->Instance->CR1 & TIM_CR1_CMS) != 0; } -} // namespace Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel}, inverted{config.inverted} { if (this->handle->State == HAL_TIM_STATE_RESET) { diff --git a/micras_hal/host/src/pwm_dma.cpp b/micras_hal/host/src/pwm_dma.cpp index c96863a..d60b547 100644 --- a/micras_hal/host/src/pwm_dma.cpp +++ b/micras_hal/host/src/pwm_dma.cpp @@ -7,10 +7,10 @@ #include "micras/hal/host/board.hpp" #include "micras/hal/host/clock.hpp" +#include "micras/hal/host/ports.hpp" #include "micras/hal/pwm_dma.hpp" namespace micras::hal { -namespace { /** * @brief Start a transfer of compare values. * @@ -19,7 +19,7 @@ namespace { * @param compares Values the DMA feeds the compare register with. */ template -void start(TIM_HandleTypeDef* handle, uint32_t channel, std::span compares) { +static void start(TIM_HandleTypeDef* handle, uint32_t channel, std::span compares) { host::PwmDmaPort& port = host::Board::pwm_dma(handle, channel); port.touched = true; port.compares.assign(compares.begin(), compares.end()); @@ -32,7 +32,6 @@ void start(TIM_HandleTypeDef* handle, uint32_t channel, std::span compa port.transfer_end = host::Clock::instance().now() + compares.size() * port.period * cycles_per_count; } -} // namespace PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.timer_channel} { if (this->handle->State == HAL_TIM_STATE_RESET) { diff --git a/micras_hal/host/src/spi.cpp b/micras_hal/host/src/spi.cpp index 009b308..9c5d1fb 100644 --- a/micras_hal/host/src/spi.cpp +++ b/micras_hal/host/src/spi.cpp @@ -4,13 +4,16 @@ #include #include +#include #include #include #include #include +#include "micras/hal/gpio.hpp" #include "micras/hal/host/board.hpp" #include "micras/hal/host/clock.hpp" +#include "micras/hal/host/ports.hpp" #include "micras/hal/host/spi_device.hpp" #include "micras/hal/spi.hpp" #include "micras/hal/timer.hpp" @@ -31,13 +34,14 @@ struct HostState { */ uint64_t transfer_end{0}; }; +} // namespace /** * @brief Get the host state of every Spi alive. * * @return The states, keyed by the object. */ -std::map& host_states() { +static std::map& host_states() { static std::map states; return states; } @@ -49,7 +53,7 @@ std::map& host_states() { * @param handle Its handle. * @return The port. */ -host::SpiPort& port_of(const Spi* spi, const SPI_HandleTypeDef* handle) { +static host::SpiPort& port_of(const Spi* spi, const SPI_HandleTypeDef* handle) { const Gpio::Config& cs = host_states().at(spi).cs; return host::Board::spi(handle, cs.port, cs.pin); } @@ -60,7 +64,7 @@ host::SpiPort& port_of(const Spi* spi, const SPI_HandleTypeDef* handle) { * @param handle Handle of the bus. * @return The mode. */ -host::SpiDevice::Mode mode_of(const SPI_HandleTypeDef& handle) { +static host::SpiDevice::Mode mode_of(const SPI_HandleTypeDef& handle) { const bool polarity = handle.Init.CLKPolarity == SPI_POLARITY_HIGH; const bool phase = handle.Init.CLKPhase == SPI_PHASE_2EDGE; return static_cast((polarity ? 2 : 0) + (phase ? 1 : 0)); @@ -74,7 +78,7 @@ host::SpiDevice::Mode mode_of(const SPI_HandleTypeDef& handle) { * @param transmitted Bytes sent. * @param received Bytes answered, as many as were sent; all ones when no device drives the bus in its mode. */ -void exchange( +static void exchange( const SPI_HandleTypeDef& handle, host::SpiPort& port, std::span transmitted, std::span received ) { @@ -93,7 +97,7 @@ void exchange( * @param bytes Bytes transferred. * @return Host clock cycles, zero when the bus has no clock to time it by. */ -uint64_t transfer_cycles(const SPI_HandleTypeDef& handle, std::size_t bytes) { +static uint64_t transfer_cycles(const SPI_HandleTypeDef& handle, std::size_t bytes) { if (handle.Instance == nullptr or handle.Instance->kernel_clock == 0) { return 0; } @@ -104,7 +108,6 @@ uint64_t transfer_cycles(const SPI_HandleTypeDef& handle, std::size_t bytes) { return ((bytes * 8 * divider * cycles_per_second) + kernel_clock - 1) / kernel_clock; } -} // namespace std::array Spi::transferring{}; diff --git a/micras_hal/host/src/uart_dma.cpp b/micras_hal/host/src/uart_dma.cpp index 6e4f9b3..d29a883 100644 --- a/micras_hal/host/src/uart_dma.cpp +++ b/micras_hal/host/src/uart_dma.cpp @@ -9,6 +9,7 @@ #include #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" #include "micras/hal/uart_dma.hpp" namespace micras::hal { diff --git a/micras_nav/include/micras/nav/impl/curve_speed.tpp b/micras_nav/include/micras/nav/impl/curve_speed.tpp index a0935f6..c45f658 100644 --- a/micras_nav/include/micras/nav/impl/curve_speed.tpp +++ b/micras_nav/include/micras/nav/impl/curve_speed.tpp @@ -16,6 +16,7 @@ namespace micras::nav { template void CurveSpeed::plan( + // NOLINTNEXTLINE(cppcoreguidelines-missing-std-forward) called once per sample, so never forwarded F&& bending, std::span speeds, float spacing, float start_speed, float end_speed, const CurveLimits& limits ) { if (speeds.empty()) { @@ -24,6 +25,8 @@ void CurveSpeed::plan( const std::size_t last = speeds.size() - 1; + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) every index is at most last + Bending before = bending(0); for (std::size_t i = 0; i <= last; i++) { @@ -46,6 +49,7 @@ void CurveSpeed::plan( const float deceleration = limits.get_deceleration(speeds[i], at); speeds[i - 1] = std::min(speeds[i - 1], std::sqrt(speeds[i] * speeds[i] + 2.0F * spacing * deceleration)); } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) } } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/impl/maze.tpp b/micras_nav/include/micras/nav/impl/maze.tpp index d241735..3972e4d 100644 --- a/micras_nav/include/micras/nav/impl/maze.tpp +++ b/micras_nav/include/micras/nav/impl/maze.tpp @@ -98,6 +98,7 @@ bool TMaze::is_known_open(const GridPose& pose) const { template bool TMaze::is_goal(const GridPoint& position) const { + // NOLINTNEXTLINE(readability-use-anyofallof) the loop the robot's image was verified with for (const GridPoint& cell : this->goal) { if (cell == position) { return true; @@ -220,6 +221,8 @@ template void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { const std::span data{buffer, size}; + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) the size is checked first + if (size != header_size + walls_size or data[0] != format_version or data[1] != width or data[2] != height) { return; } @@ -229,8 +232,7 @@ void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { for (uint8_t row = 0; row < height; row++) { for (uint8_t column = 0; column < width; column++) { const uint16_t index = row * width + column; - const uint8_t packed = - (static_cast(data[header_size + index / 2U]) >> (4U * (index % 2U))) & 0x0FU; + const uint8_t packed = (data[header_size + index / 2U] >> (4U * (index % 2U))) & 0x0F; const std::array, 2> sides{ {{Side::RIGHT, packed & 0x03U}, {Side::UP, packed >> 2U}} @@ -247,6 +249,8 @@ void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { } } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) + this->flood(this->goal); } diff --git a/micras_nav/include/micras/nav/impl/planner.tpp b/micras_nav/include/micras/nav/impl/planner.tpp index 417c522..e432a4f 100644 --- a/micras_nav/include/micras/nav/impl/planner.tpp +++ b/micras_nav/include/micras/nav/impl/planner.tpp @@ -197,6 +197,7 @@ void TPlanner::get_route(uint8_t index, Route& route) const { template template +// NOLINTNEXTLINE(cppcoreguidelines-missing-std-forward) called for every wall, so never forwarded void TPlanner::for_each_wall(const Route& route, F&& function) { LatticePose node = route.start; function(node.wall()); @@ -587,6 +588,7 @@ void TPlanner::advance_expansion() { } template +// NOLINTNEXTLINE(readability-function-cognitive-complexity) one edge of the search, both sides at once void TPlanner::relax_turn(uint16_t index, const LatticePose& entry, uint8_t run, TurnId turn) { const TurnPrimitive& primitive = get_primitive(turn); diff --git a/micras_nav/include/micras/nav/impl/wall_model.tpp b/micras_nav/include/micras/nav/impl/wall_model.tpp index 160666b..db45844 100644 --- a/micras_nav/include/micras/nav/impl/wall_model.tpp +++ b/micras_nav/include/micras/nav/impl/wall_model.tpp @@ -15,6 +15,7 @@ namespace micras::nav { template +// NOLINTNEXTLINE(readability-function-cognitive-complexity) one ray, crossing up to four walls RayHit WallModel::cast(const Pose& pose, uint8_t sensor, const TMaze& maze) const { const RobotModel::WallSensor& mounting = this->sensors.at(sensor); @@ -31,6 +32,7 @@ RayHit WallModel::cast(const Pose& pose, uint8_t sensor, const TMaze= width or row >= height) { break; } @@ -55,6 +57,7 @@ RayHit WallModel::cast(const Pose& pose, uint8_t sensor, const TMaze(column), .y = static_cast(row)}, + // NOLINTNEXTLINE(readability-avoid-nested-conditional-operator) one of the four sides .orientation = vertical ? (direction_x > 0.0F ? Side::RIGHT : Side::LEFT) : (direction_y > 0.0F ? Side::UP : Side::DOWN), }; @@ -75,8 +78,10 @@ RayHit WallModel::cast(const Pose& pose, uint8_t sensor, const TMaze(column + (vertical ? (direction_x > 0.0F ? 1 : -1) : 0)); row = static_cast(row + (vertical ? 0 : (direction_y > 0.0F ? 1 : -1))); + // NOLINTEND(readability-avoid-nested-conditional-operator) continue; } diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index ff8501f..3242504 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -13,7 +13,7 @@ namespace micras::nav { Side angle_to_grid(float angle) { - const int32_t grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); + const auto grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); return static_cast(grid_angle < 0 ? (4 + (grid_angle % 4)) % 4 : grid_angle % 4); } diff --git a/micras_proxy/models/src/as5047u_model.cpp b/micras_proxy/models/src/as5047u_model.cpp index 9903a07..989820f 100644 --- a/micras_proxy/models/src/as5047u_model.cpp +++ b/micras_proxy/models/src/as5047u_model.cpp @@ -4,9 +4,11 @@ #include #include +#include #include #include +#include "micras/hal/host/spi_device.hpp" #include "micras/models/as5047u_model.hpp" namespace micras::models { @@ -51,6 +53,7 @@ void As5047uModel::select() { void As5047uModel::exchange(std::span transmitted, std::span received) { const std::size_t size = std::min(transmitted.size(), received.size()); + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) below the size of both spans for (std::size_t index = 0; index < size; index++) { const uint32_t shift = 8U * static_cast(frame_size - 1 - this->received_bytes); received[index] = static_cast(this->answer >> shift); @@ -62,6 +65,7 @@ void As5047uModel::exchange(std::span transmitted, std::spanprocess(this->frame); } } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) } void As5047uModel::deselect() { diff --git a/micras_proxy/models/src/lsm6dsv_model.cpp b/micras_proxy/models/src/lsm6dsv_model.cpp index cd5c289..c794723 100644 --- a/micras_proxy/models/src/lsm6dsv_model.cpp +++ b/micras_proxy/models/src/lsm6dsv_model.cpp @@ -5,11 +5,13 @@ #include #include #include +#include #include #include #include #include +#include "micras/hal/host/spi_device.hpp" #include "micras/models/lsm6dsv_model.hpp" namespace micras::models { @@ -113,6 +115,7 @@ constexpr double mg_to_mps2{0.00980665}; * @brief Rate of ODR code 3 for each high accuracy set of HAODR_CFG, in hertz; each code above doubles it. */ constexpr std::array base_rates{15.0, 15.625, 12.5, 15.0}; +} // namespace /** * @brief Check whether the SPI interface can write a register. @@ -120,12 +123,11 @@ constexpr std::array base_rates{15.0, 15.625, 12.5, 15.0}; * @param address Register address. * @return True for a read-write register. */ -bool writable(uint8_t address) { +static bool writable(uint8_t address) { return std::ranges::any_of(writable_ranges, [address](const auto& range) { return address >= range.first and address <= range.second; }); } -} // namespace Lsm6dsvModel::Lsm6dsvModel() : SpiDevice{Mode::MODE_3} { this->reset(); @@ -139,6 +141,7 @@ void Lsm6dsvModel::select() { void Lsm6dsvModel::exchange(std::span transmitted, std::span received) { const std::size_t size = std::min(transmitted.size(), received.size()); + // NOLINTBEGIN(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) below the size of both spans for (std::size_t index = 0; index < size; index++) { received[index] = 0; @@ -163,6 +166,7 @@ void Lsm6dsvModel::exchange(std::span transmitted, std::spanaddress = (this->address + 1) & address_mask; } } + // NOLINTEND(cppcoreguidelines-pro-bounds-avoid-unchecked-container-access) } void Lsm6dsvModel::deselect() { diff --git a/micras_proxy/src/imu.cpp b/micras_proxy/src/imu.cpp index 9d6654f..1b3d028 100644 --- a/micras_proxy/src/imu.cpp +++ b/micras_proxy/src/imu.cpp @@ -4,7 +4,6 @@ #include #include -#include #include #include "lsm6dsv_reg.h" diff --git a/micras_proxy/src/storage.cpp b/micras_proxy/src/storage.cpp index 7c8afb7..ba79c80 100644 --- a/micras_proxy/src/storage.cpp +++ b/micras_proxy/src/storage.cpp @@ -179,7 +179,7 @@ bool Storage::save(const core::VariablePool& pool) { body.resize(align_size(body.size()), hal::FlashWord::erased_value); if (body.size() / hal::FlashWord::size > UINT16_MAX or - header_size + body.size() > this->number_of_sectors * hal::Flash::sector_size) { + header_size + body.size() > static_cast(this->number_of_sectors) * hal::Flash::sector_size) { return false; } diff --git a/tests/comm/test_frame.cpp b/tests/comm/test_frame.cpp index a80ff86..d4dc09a 100644 --- a/tests/comm/test_frame.cpp +++ b/tests/comm/test_frame.cpp @@ -20,53 +20,68 @@ struct Vector { std::vector payload; std::vector frame; }; - -const std::vector vectors{ - {"hello", MessageType{0x01}, {}, {4, 1, 1, 1, 0}}, - {"hello_ack", - MessageType{0x81}, - {1, 133, 114, 115, 247, 4, 0, 125, 0, 0, 0, 0, 1}, - {8, 129, 1, 133, 114, 115, 247, 4, 2, 125, 1, 1, 1, 4, 1, 104, 155, 0}}, - {"schema_request", MessageType{0x02}, {0, 0}, {2, 2, 1, 3, 2, 6, 0}}, - {"group_define", MessageType{0x03}, {0, 80, 0, 2, 0, 0, 1, 0}, {2, 3, 2, 80, 2, 2, 1, 2, 1, 3, 86, 89, 0}}, - {"credit", MessageType{0x05}, {0, 1}, {2, 5, 4, 1, 6, 16, 0}}, - {"write", MessageType{0x06}, {2, 0, 0, 0, 128, 63}, {3, 6, 2, 1, 1, 5, 128, 63, 199, 118, 0}}, - {"zeros", MessageType{0x85}, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {2, 133, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1, 1, - 1, 1, 1, 3, 133, 221, 0}}, - {"ramp", - MessageType{0x8B}, - {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, - 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, - 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, - 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, - 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, - 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, - 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, - 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, - 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199}, - {2, 139, 202, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, - 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, - 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, - 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, - 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, - 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, - 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, - 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, - 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 149, 49, 0}}, - {"high", - MessageType{0x89}, - {200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, - 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239}, - {44, 137, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, - 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 247, 247, 0}}, -}; - } // namespace +static std::vector monitor_vectors() { + return { + {.name = "hello", .type = MessageType{0x01}, .payload = {}, .frame = {4, 1, 1, 1, 0}}, + {.name = "hello_ack", + .type = MessageType{0x81}, + .payload = {1, 133, 114, 115, 247, 4, 0, 125, 0, 0, 0, 0, 1}, + .frame = {8, 129, 1, 133, 114, 115, 247, 4, 2, 125, 1, 1, 1, 4, 1, 104, 155, 0}}, + {.name = "schema_request", .type = MessageType{0x02}, .payload = {0, 0}, .frame = {2, 2, 1, 3, 2, 6, 0}}, + {.name = "group_define", + .type = MessageType{0x03}, + .payload = {0, 80, 0, 2, 0, 0, 1, 0}, + .frame = {2, 3, 2, 80, 2, 2, 1, 2, 1, 3, 86, 89, 0}}, + {.name = "credit", .type = MessageType{0x05}, .payload = {0, 1}, .frame = {2, 5, 4, 1, 6, 16, 0}}, + {.name = "write", + .type = MessageType{0x06}, + .payload = {2, 0, 0, 0, 128, 63}, + .frame = {3, 6, 2, 1, 1, 5, 128, 63, 199, 118, 0}}, + {.name = "zeros", + .type = MessageType{0x85}, + .payload = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + .frame = {2, 133, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3, 133, 221, 0}}, + {.name = "ramp", + // NOLINTNEXTLINE(clang-analyzer-optin.core.EnumCastOutOfRange) the framing carries any type byte + .type = MessageType{0x8B}, + .payload = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, + 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, + 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, + 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, + 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, + 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, + 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, + 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, + 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, + 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, + 190, 191, 192, 193, 194, 195, 196, 197, 198, 199}, + .frame = {2, 139, 202, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, + 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, + 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, + 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, + 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, + 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, + 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, + 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, + 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, + 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, + 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 149, 49, 0}}, + {.name = "high", + .type = MessageType{0x89}, + .payload = {200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, + 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, + 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239}, + .frame = {44, 137, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, + 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, + 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 247, 247, 0}}, + }; +} + TEST_SUITE("frame") { TEST_CASE("builds and reads the golden frames of micras-monitor's src/lib/comm/__tests__/frameVectors.ts") { - for (const Vector& vector : vectors) { + for (const Vector& vector : monitor_vectors()) { CAPTURE(vector.name); std::vector built(max_frame_size); @@ -89,8 +104,8 @@ TEST_SUITE("frame") { } TEST_CASE("discards a frame that lost a byte and reads the next one") { - const std::vector& good = vectors.at(3).frame; - FrameReader reader; + const std::vector good = monitor_vectors().at(3).frame; + FrameReader reader; for (std::size_t index = 0; index + 3 < good.size(); index++) { CHECK_FALSE(reader.push(good.at(index))); diff --git a/tests/comm/test_link.cpp b/tests/comm/test_link.cpp index 6c5cc2b..ea88a5a 100644 --- a/tests/comm/test_link.cpp +++ b/tests/comm/test_link.cpp @@ -3,10 +3,12 @@ */ #include +#include #include #include #include #include +#include #include #include #include @@ -29,11 +31,10 @@ class Loopback : public core::IByteStream { public: std::size_t read(std::span into) override { const std::size_t count = std::min(into.size(), this->to_robot.size()); + const auto end = std::next(this->to_robot.begin(), static_cast(count)); - for (std::size_t index = 0; index < count; index++) { - into[index] = this->to_robot.front(); - this->to_robot.pop_front(); - } + std::copy(this->to_robot.begin(), end, into.begin()); + this->to_robot.erase(this->to_robot.begin(), end); return count; } @@ -77,12 +78,13 @@ struct Message { MessageType type; Bytes payload; }; +} // namespace -float to_float(uint32_t bits) { +static float to_float(uint32_t bits) { return std::bit_cast(bits); } -std::vector of_type(const std::vector& messages, MessageType type) { +static std::vector of_type(const std::vector& messages, MessageType type) { std::vector matching; std::ranges::copy_if(messages, std::back_inserter(matching), [type](const Message& message) { return message.type == type; @@ -90,6 +92,7 @@ std::vector of_type(const std::vector& messages, MessageType t return matching; } +namespace { class Session { public: static constexpr uint32_t loop_time_us{125}; @@ -126,8 +129,8 @@ class Session { if (this->application.push(byte)) { messages.push_back( - {this->application.type(), - Bytes(this->application.payload().begin(), this->application.payload().end())} + {.type = this->application.type(), + .payload = Bytes(this->application.payload().begin(), this->application.payload().end())} ); } } @@ -150,7 +153,14 @@ class Session { this->link.register_variables(probe, "link/"); std::array buffer{}; - probe.read(probe.find("link/dropped_samples").value(), buffer); + const auto id = probe.find("link/dropped_samples"); + + if (not id.has_value()) { + FAIL_CHECK("the link registers no dropped samples counter"); + return 0; + } + + probe.read(*id, buffer); return Reader{buffer}.u32(); } diff --git a/tests/core/test_cobs.cpp b/tests/core/test_cobs.cpp index 5499dba..f4bc781 100644 --- a/tests/core/test_cobs.cpp +++ b/tests/core/test_cobs.cpp @@ -2,7 +2,6 @@ * @file */ -#include #include #include #include @@ -14,20 +13,21 @@ namespace micras::core { namespace { using Bytes = std::vector; +} // namespace -Bytes encode(const Bytes& raw) { +static Bytes encode(const Bytes& raw) { Bytes encoded(cobs_encoded_size(raw.size())); encoded.resize(cobs_encode(raw, encoded)); return encoded; } -Bytes decode(const Bytes& encoded) { +static Bytes decode(const Bytes& encoded) { Bytes decoded(encoded.size() + 1); decoded.resize(cobs_decode(encoded, decoded)); return decoded; } -Bytes count(int first, int last) { +static Bytes count(int first, int last) { Bytes bytes; for (int value = first; value <= last; value++) { @@ -37,13 +37,13 @@ Bytes count(int first, int last) { return bytes; } -Bytes join(const Bytes& head, const Bytes& tail) { +static Bytes join(const Bytes& head, const Bytes& tail) { Bytes joined = head; joined.insert(joined.end(), tail.begin(), tail.end()); return joined; } -void check_vector(const Bytes& raw, const Bytes& expected) { +static void check_vector(const Bytes& raw, const Bytes& expected) { CAPTURE(raw.size()); const Bytes encoded = encode(raw); @@ -51,7 +51,6 @@ void check_vector(const Bytes& raw, const Bytes& expected) { CHECK(encoded == expected); CHECK(decode(encoded) == raw); } -} // namespace TEST_SUITE("cobs") { TEST_CASE("encodes the reference vectors and decodes them back") { @@ -70,6 +69,7 @@ TEST_SUITE("cobs") { } TEST_CASE("round trips random frames without a delimiter in the encoding") { + // NOLINTNEXTLINE(bugprone-random-generator-seed) a fixed seed keeps the test reproducible std::mt19937 generator{7}; for (int trial = 0; trial < 200000; trial++) { diff --git a/tests/core/test_variable_pool.cpp b/tests/core/test_variable_pool.cpp index 6e907a5..df03139 100644 --- a/tests/core/test_variable_pool.cpp +++ b/tests/core/test_variable_pool.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -32,11 +33,11 @@ enum class Profile : uint8_t { A, B }; +} // namespace -std::span bytes_of(const float& value) { +static std::span bytes_of(const float& value) { return {std::bit_cast(&value), sizeof(value)}; } -} // namespace TEST_SUITE("variable_pool") { TEST_CASE("registers finds reads and writes variables with their access") { diff --git a/tests/hal/test_adc.cpp b/tests/hal/test_adc.cpp index 9c6fcee..d9111d4 100644 --- a/tests/hal/test_adc.cpp +++ b/tests/hal/test_adc.cpp @@ -10,6 +10,7 @@ #include "host_fixture.hpp" #include "micras/hal/adc_dma.hpp" #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" namespace micras::test { TEST_SUITE("adc") { @@ -35,7 +36,7 @@ TEST_SUITE("adc") { CHECK(adc.read_snapshot(read) == 1); CHECK(read == std::array{1500, 900}); CHECK(adc.get_max_reading() == 4095); - CHECK(adc.get_reference_voltage() == doctest::Approx(3.3F)); + CHECK(static_cast(adc.get_reference_voltage()) == doctest::Approx(3.3)); } TEST_CASE_FIXTURE(HostBoard, "stops updating after an error until it recovers") { diff --git a/tests/hal/test_board.cpp b/tests/hal/test_board.cpp index 9a50001..ee4b6bc 100644 --- a/tests/hal/test_board.cpp +++ b/tests/hal/test_board.cpp @@ -10,6 +10,7 @@ #include "host_fixture.hpp" #include "micras/hal/gpio.hpp" #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" namespace micras::test { TEST_SUITE("board") { diff --git a/tests/hal/test_crc.cpp b/tests/hal/test_crc.cpp index 5a09783..08f05d8 100644 --- a/tests/hal/test_crc.cpp +++ b/tests/hal/test_crc.cpp @@ -14,8 +14,7 @@ #include "micras/hal/crc.hpp" namespace micras::test { -namespace { -uint8_t reference_crc8(std::span data, uint8_t polynomial, uint8_t initial) { +static uint8_t reference_crc8(std::span data, uint8_t polynomial, uint8_t initial) { uint8_t crc = initial; for (const uint8_t byte : data) { @@ -28,7 +27,6 @@ uint8_t reference_crc8(std::span data, uint8_t polynomial, uint8_ return crc; } -} // namespace TEST_SUITE("crc") { TEST_CASE_FIXTURE(HostBoard, "computes the check value of CRC-8/SAE-J1850") { diff --git a/tests/hal/test_flash.cpp b/tests/hal/test_flash.cpp index 490a4fa..3d17cab 100644 --- a/tests/hal/test_flash.cpp +++ b/tests/hal/test_flash.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -16,11 +17,11 @@ namespace micras::test { namespace { constexpr std::array data{1, 2, 3, 4, 5}; +} // namespace -bool is_erased(std::span bytes) { +static bool is_erased(std::span bytes) { return std::ranges::all_of(bytes, [](uint8_t byte) { return byte == hal::FlashWord::erased_value; }); } -} // namespace TEST_SUITE("flash") { TEST_CASE_FIXTURE(HostBoard, "reserves the upper half of the sectors") { @@ -58,7 +59,7 @@ TEST_SUITE("flash") { } TEST_CASE_FIXTURE(HostBoard, "stops a write where the simulated power loss cuts it off") { - std::array long_data{}; + std::array(3 * hal::FlashWord::size)> long_data{}; long_data.fill(0x42); hal::host::Board::flash().write_budget = 2; diff --git a/tests/hal/test_fmac.cpp b/tests/hal/test_fmac.cpp index 6e81a4b..a9bb93f 100644 --- a/tests/hal/test_fmac.cpp +++ b/tests/hal/test_fmac.cpp @@ -20,8 +20,9 @@ constexpr double q15_scale{32768.0}; constexpr std::array feed_forward{2048, 4096, 2048}; constexpr std::array feedback{29491, -13107}; +} // namespace -std::vector direct_form(const std::vector& inputs) { +static std::vector direct_form(const std::vector& inputs) { std::vector outputs; std::array input_history{}; std::array output_history{}; @@ -45,7 +46,6 @@ std::vector direct_form(const std::vector& inputs) { return outputs; } -} // namespace TEST_SUITE("fmac") { TEST_CASE_FIXTURE(HostBoard, "runs the configured filter as a direct form reference does") { @@ -55,6 +55,7 @@ TEST_SUITE("fmac") { std::vector inputs; + inputs.reserve(200); for (int sample = 0; sample < 200; sample++) { inputs.push_back(0.5 * std::sin(0.1 * sample) + (sample > 100 ? 0.25 : 0.0)); } diff --git a/tests/hal/test_spi.cpp b/tests/hal/test_spi.cpp index b5d4584..efd5d37 100644 --- a/tests/hal/test_spi.cpp +++ b/tests/hal/test_spi.cpp @@ -2,8 +2,8 @@ * @file */ +#include #include -#include #include #include #include @@ -14,6 +14,7 @@ #include "host_fixture.hpp" #include "micras/hal/host/board.hpp" #include "micras/hal/host/clock.hpp" +#include "micras/hal/host/ports.hpp" #include "micras/hal/host/spi_device.hpp" #include "micras/hal/spi.hpp" #include "micras/hal/timer.hpp" @@ -32,10 +33,10 @@ class RecordingDevice : public SpiDevice { void exchange(std::span transmitted, std::span received) override { this->log.push_back("exchange " + std::to_string(transmitted.size())); - for (std::size_t index = 0; index < transmitted.size(); index++) { - this->received_bytes.push_back(transmitted[index]); - received[index] = static_cast(transmitted[index] + this->offset); - } + this->received_bytes.insert(this->received_bytes.end(), transmitted.begin(), transmitted.end()); + std::ranges::transform(transmitted, received.begin(), [this](uint8_t byte) { + return static_cast(byte + this->offset); + }); } void deselect() override { this->log.emplace_back("deselect"); } @@ -49,12 +50,13 @@ class RecordingDevice : public SpiDevice { std::vector log; std::vector received_bytes; }; +} // namespace -void attach(const hal::Spi::Config& config, SpiDevice& device) { +static void attach(const hal::Spi::Config& config, SpiDevice& device) { Board::spi_device(config.handle, config.cs_gpio.port, config.cs_gpio.pin, device); } -uint32_t wait_for_transfer(const hal::Spi& spi) { +static uint32_t wait_for_transfer(const hal::Spi& spi) { const uint64_t start = hal::host::Clock::instance().now(); while (spi.get_transfer() == hal::Spi::Transfer::RUNNING) { @@ -63,7 +65,6 @@ uint32_t wait_for_transfer(const hal::Spi& spi) { return static_cast((hal::host::Clock::instance().now() - start) / (SystemCoreClock / 1000000)); } -} // namespace TEST_SUITE("spi") { TEST_CASE_FIXTURE(HostBoard, "routes each transfer to the device its chip select selects") { diff --git a/tests/hal/test_timers.cpp b/tests/hal/test_timers.cpp index 2ffe027..2a3339e 100644 --- a/tests/hal/test_timers.cpp +++ b/tests/hal/test_timers.cpp @@ -10,8 +10,10 @@ #include "host_fixture.hpp" #include "micras/hal/encoder.hpp" #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" #include "micras/hal/pwm.hpp" #include "micras/hal/pwm_dma.hpp" +#include "tim.h" namespace micras::test { TEST_SUITE("timers") { @@ -19,8 +21,11 @@ TEST_SUITE("timers") { const hal::Pwm pwm{pwm_config}; CHECK(pwm.was_initialized()); - CHECK(pwm.get_frequency() == doctest::Approx(1000.0F)); - CHECK(hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel).frequency == doctest::Approx(1000.0F)); + CHECK(static_cast(pwm.get_frequency()) == doctest::Approx(1000.0)); + CHECK( + static_cast(hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel).frequency) == + doctest::Approx(1000.0) + ); } TEST_CASE_FIXTURE(HostBoard, "reports the duty cycle the compare register gives") { @@ -28,14 +33,14 @@ TEST_SUITE("timers") { const hal::host::PwmPort& port = hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel); pwm.set_duty_cycle(25.0F); - CHECK(port.duty_cycle == doctest::Approx(25.0F)); + CHECK(static_cast(port.duty_cycle) == doctest::Approx(25.0)); CHECK(port.touched); pwm.set_duty_cycle(150.0F); - CHECK(port.duty_cycle == doctest::Approx(100.0F)); + CHECK(static_cast(port.duty_cycle) == doctest::Approx(100.0)); pwm.set_duty_cycle(-10.0F); - CHECK(port.duty_cycle == doctest::Approx(0.0F)); + CHECK(static_cast(port.duty_cycle) == doctest::Approx(0.0)); } TEST_CASE_FIXTURE(HostBoard, "reports the active fraction of an inverted channel") { @@ -45,7 +50,10 @@ TEST_SUITE("timers") { pwm.set_duty_cycle(30.0F); - CHECK(hal::host::Board::pwm(config.handle, config.timer_channel).duty_cycle == doctest::Approx(30.0F)); + CHECK( + static_cast(hal::host::Board::pwm(config.handle, config.timer_channel).duty_cycle) == + doctest::Approx(30.0) + ); CHECK((htim15.Instance->CCER & TIM_CCER_CC1P) != 0); } @@ -55,8 +63,11 @@ TEST_SUITE("timers") { pwm.set_frequency(2000); CHECK(htim15.Instance->ARR == 499); - CHECK(pwm.get_frequency() == doctest::Approx(2000.0F)); - CHECK(hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel).frequency == doctest::Approx(2000.0F)); + CHECK(static_cast(pwm.get_frequency()) == doctest::Approx(2000.0)); + CHECK( + static_cast(hal::host::Board::pwm(pwm_config.handle, pwm_config.timer_channel).frequency) == + doctest::Approx(2000.0) + ); } TEST_CASE_FIXTURE(HostBoard, "keeps a DMA transfer of compare values busy for one period per value") { diff --git a/tests/hal/test_uart.cpp b/tests/hal/test_uart.cpp index 678867c..4cd9787 100644 --- a/tests/hal/test_uart.cpp +++ b/tests/hal/test_uart.cpp @@ -11,6 +11,7 @@ #include "host_fixture.hpp" #include "micras/hal/host/board.hpp" +#include "micras/hal/host/ports.hpp" #include "micras/hal/uart_dma.hpp" namespace micras::test { diff --git a/tests/models/test_as5047u_model.cpp b/tests/models/test_as5047u_model.cpp index fa85660..9282131 100644 --- a/tests/models/test_as5047u_model.cpp +++ b/tests/models/test_as5047u_model.cpp @@ -7,6 +7,7 @@ #include +#include "micras/hal/host/spi_device.hpp" #include "micras/models/as5047u_model.hpp" namespace micras::models { @@ -15,26 +16,27 @@ using Frame = std::array; constexpr uint32_t read_flag{1U << 22U}; constexpr uint32_t error_flag{1U << 22U}; +} // namespace -Frame frame_of(uint32_t upper) { +static Frame frame_of(uint32_t upper) { const auto high = static_cast(upper >> 16U); const auto low = static_cast(upper >> 8U); return {high, low, As5047uModel::crc(high, low)}; } -Frame read_command(uint16_t address) { +static Frame read_command(uint16_t address) { return frame_of(read_flag | static_cast(address) << 8U); } -Frame write_command(uint16_t address) { +static Frame write_command(uint16_t address) { return frame_of(static_cast(address) << 8U); } -Frame data_frame(uint16_t data) { +static Frame data_frame(uint16_t data) { return frame_of(static_cast(data) << 8U); } -uint32_t transfer(As5047uModel& chip, const Frame& frame) { +static uint32_t transfer(As5047uModel& chip, const Frame& frame) { Frame received{}; chip.select(); chip.exchange(frame, received); @@ -43,27 +45,25 @@ uint32_t transfer(As5047uModel& chip, const Frame& frame) { std::get<2>(received); } -uint16_t data_of(uint32_t answer) { +static uint16_t data_of(uint32_t answer) { return static_cast((answer >> 8U) & 0x3FFFU); } -bool valid(uint32_t answer) { +static bool valid(uint32_t answer) { return As5047uModel::crc(static_cast(answer >> 16U), static_cast(answer >> 8U)) == static_cast(answer); } -uint16_t read_register(As5047uModel& chip, uint16_t address) { +static uint16_t read_register(As5047uModel& chip, uint16_t address) { transfer(chip, read_command(address)); return data_of(transfer(chip, read_command(As5047uModel::nop_address))); } -void write_register(As5047uModel& chip, uint16_t address, uint16_t data) { +static void write_register(As5047uModel& chip, uint16_t address, uint16_t data) { transfer(chip, write_command(address)); transfer(chip, data_frame(data)); } -} // namespace - TEST_SUITE("as5047u_model") { TEST_CASE("answers in SPI mode 1") { const As5047uModel chip; diff --git a/tests/models/test_lsm6dsv_model.cpp b/tests/models/test_lsm6dsv_model.cpp index f12ed47..e2d61ce 100644 --- a/tests/models/test_lsm6dsv_model.cpp +++ b/tests/models/test_lsm6dsv_model.cpp @@ -3,20 +3,24 @@ */ #include +#include #include #include +#include #include #include #include "lsm6dsv_reg.h" +#include "micras/hal/host/spi_device.hpp" #include "micras/models/lsm6dsv_model.hpp" namespace micras::models { namespace { constexpr uint8_t read_flag{0x80}; +} // namespace -std::vector transact(Lsm6dsvModel& chip, std::vector transmitted) { +static std::vector transact(Lsm6dsvModel& chip, std::vector transmitted) { std::vector received(transmitted.size()); chip.select(); chip.exchange(transmitted, received); @@ -24,19 +28,19 @@ std::vector transact(Lsm6dsvModel& chip, std::vector transmitt return received; } -void write(Lsm6dsvModel& chip, uint8_t address, uint8_t value) { +static void write(Lsm6dsvModel& chip, uint8_t address, uint8_t value) { transact(chip, {address, value}); } -uint8_t read(Lsm6dsvModel& chip, uint8_t address) { +static uint8_t read(Lsm6dsvModel& chip, uint8_t address) { return transact(chip, {static_cast(address | read_flag), 0}).at(1); } -int16_t word(const std::vector& bytes, std::size_t low) { +static int16_t word(const std::vector& bytes, std::size_t low) { return static_cast(bytes.at(low) | (bytes.at(low + 1) << 8U)); } -void configure(Lsm6dsvModel& chip, uint8_t gyroscope_scale, uint8_t accelerometer_scale) { +static void configure(Lsm6dsvModel& chip, uint8_t gyroscope_scale, uint8_t accelerometer_scale) { write(chip, LSM6DSV_HAODR_CFG, 0x01); write(chip, LSM6DSV_CTRL1, 0x1C); write(chip, LSM6DSV_CTRL2, 0x1C); @@ -44,14 +48,12 @@ void configure(Lsm6dsvModel& chip, uint8_t gyroscope_scale, uint8_t acceleromete write(chip, LSM6DSV_CTRL8, accelerometer_scale); } -std::vector read_burst(Lsm6dsvModel& chip) { +static std::vector read_burst(Lsm6dsvModel& chip) { std::vector command(17); command.at(0) = LSM6DSV_STATUS_REG | read_flag; return transact(chip, command); } -} // namespace - TEST_SUITE("lsm6dsv_model") { TEST_CASE("answers in SPI mode 3") { const Lsm6dsvModel chip; diff --git a/tests/nav/test_maze.cpp b/tests/nav/test_maze.cpp index 402f33b..603b6ff 100644 --- a/tests/nav/test_maze.cpp +++ b/tests/nav/test_maze.cpp @@ -3,8 +3,10 @@ */ #include +#include #include #include +#include #include #include @@ -17,32 +19,30 @@ namespace micras::test { namespace { using Maze = nav::TMaze<16, 16>; -const std::filesystem::path mazes_directory{MICRAS_LIB_TEST_MAZES}; - constexpr nav::GridPose maze_start{.position = {.x = 0, .y = 0}, .orientation = nav::Side::UP}; constexpr std::array maze_goal{ {{.x = 8, .y = 8}, {.x = 7, .y = 8}, {.x = 8, .y = 7}, {.x = 7, .y = 7}} }; +} // namespace -void check_same_walls(const Maze& first, const Maze& second) { - for (uint8_t y = 0; y < 16; y++) { - for (uint8_t x = 0; x < 16; x++) { +static void check_same_walls(const Maze& first, const Maze& second) { + for (uint8_t row = 0; row < 16; row++) { + for (uint8_t column = 0; column < 16; column++) { for (const nav::Side side : nav::all_sides) { - const nav::GridPose wall{.position = {.x = x, .y = y}, .orientation = side}; - CAPTURE(x); - CAPTURE(y); + const nav::GridPose wall{.position = {.x = column, .y = row}, .orientation = side}; + CAPTURE(column); + CAPTURE(row); CAPTURE(static_cast(side)); REQUIRE(first.get_wall(wall) == second.get_wall(wall)); } } } } -} // namespace TEST_SUITE("maze") { TEST_CASE("reads the contest mazes with their start and goal") { - for (const auto& entry : std::filesystem::directory_iterator{mazes_directory}) { + for (const auto& entry : std::filesystem::directory_iterator{MICRAS_LIB_TEST_MAZES}) { CAPTURE(entry.path().filename().string()); const MazeText text = read_maze(entry.path()); @@ -55,7 +55,7 @@ TEST_SUITE("maze") { } TEST_CASE("round trips a whole maze through its serialization") { - const MazeText text = read_maze(mazes_directory / "maze1.txt"); + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "maze1.txt"); Maze maze{{.start = maze_start, .goal = maze_goal}}; fill_maze(text, maze); @@ -72,7 +72,7 @@ TEST_SUITE("maze") { } TEST_CASE("keeps the unknown walls unknown through the serialization") { - const MazeText text = read_maze(mazes_directory / "japan2017ef.txt"); + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "japan2017ef.txt"); Maze maze{{.start = maze_start, .goal = maze_goal}}; for (std::size_t index = 0; index < text.walls.size(); index += 3) { @@ -87,7 +87,7 @@ TEST_SUITE("maze") { } TEST_CASE("ignores a serialization of another format or size") { - const MazeText text = read_maze(mazes_directory / "maze1.txt"); + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "maze1.txt"); Maze maze{{.start = maze_start, .goal = maze_goal}}; fill_maze(text, maze); @@ -105,7 +105,7 @@ TEST_SUITE("maze") { } TEST_CASE("floods the cost to the goal through the known walls") { - const MazeText text = read_maze(mazes_directory / "maze1.txt"); + const MazeText text = read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / "maze1.txt"); Maze maze{{.start = maze_start, .goal = maze_goal}}; fill_maze(text, maze); @@ -117,10 +117,15 @@ TEST_SUITE("maze") { uint16_t steps = 0; while (not maze.is_goal(pose.position) and steps < 256) { - const auto next = maze.get_next(pose); - REQUIRE(next.has_value()); + const std::optional next = maze.get_next(pose); + + if (not next.has_value()) { + FAIL_CHECK("no neighbor leads to the goal"); + break; + } + CHECK(maze.get_cost(next->position) + 1 == maze.get_cost(pose.position)); - pose = next.value(); + pose = *next; steps++; } diff --git a/tests/nav/test_planner.cpp b/tests/nav/test_planner.cpp index 5ffc87d..91d4a13 100644 --- a/tests/nav/test_planner.cpp +++ b/tests/nav/test_planner.cpp @@ -11,6 +11,7 @@ #include #include "maze_reader.hpp" +#include "micras/nav/motion_limits.hpp" #include "micras/nav/planner.hpp" #include "reference_robot.hpp" @@ -44,7 +45,7 @@ TEST_SUITE("planner") { read_maze(std::filesystem::path{MICRAS_LIB_TEST_MAZES} / (std::string{golden.maze} + ".txt")); const nav::Route route = plan_route(text, fast_profile); - CHECK(route.time == doctest::Approx(golden.time).epsilon(1e-6)); + CHECK(static_cast(route.time) == doctest::Approx(static_cast(golden.time)).epsilon(1e-6)); CHECK(route.steps.size() == golden.steps); } } diff --git a/tests/nav/test_route_compiler.cpp b/tests/nav/test_route_compiler.cpp index 247416a..137b7cb 100644 --- a/tests/nav/test_route_compiler.cpp +++ b/tests/nav/test_route_compiler.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -11,11 +12,14 @@ #include #include "maze_reader.hpp" +#include "micras/nav/grid_pose.hpp" #include "micras/nav/lattice.hpp" +#include "micras/nav/motion_limits.hpp" #include "micras/nav/planner.hpp" #include "micras/nav/route_compiler.hpp" #include "micras/nav/segment.hpp" #include "micras/nav/state.hpp" +#include "micras/nav/turn_table.hpp" #include "reference_robot.hpp" namespace micras::test { @@ -23,8 +27,9 @@ namespace { constexpr float tolerance{1e-3F}; const nav::LatticePose first_node{.point = {.x = 1, .y = 2}, .heading = 2}; +} // namespace -nav::Pose end_of(const nav::Segment& segment, const nav::RunProfile& profile) { +static nav::Pose end_of(const nav::Segment& segment, const nav::RunProfile& profile) { if (segment.kind == nav::SegmentKind::STRAIGHT) { return segment.start.compose({.position = {.x = segment.length, .y = 0.0F}, .orientation = 0.0F}); } @@ -36,19 +41,18 @@ nav::Pose end_of(const nav::Segment& segment, const nav::RunProfile& profile) { return segment.start.compose({.position = {.x = end.x, .y = side * end.y}, .orientation = side * end.heading}); } -bool same_pose(const nav::Pose& first, const nav::Pose& second) { +static bool same_pose(const nav::Pose& first, const nav::Pose& second) { const float heading = std::remainder(first.orientation - second.orientation, 2.0F * std::numbers::pi_v); return std::abs(first.position.x - second.position.x) < tolerance and std::abs(first.position.y - second.position.y) < tolerance and std::abs(heading) < tolerance; } -std::vector compile(const nav::Route& route) { +static std::vector compile(const nav::Route& route) { std::vector segments; nav::RouteCompiler::compile(route, reference_dynamics(), fast_profile, start_distance, segments); return segments; } -} // namespace TEST_SUITE("route_compiler") { TEST_CASE("drives a straight route as one straight from the start pose") { @@ -65,7 +69,10 @@ TEST_SUITE("route_compiler") { REQUIRE(segments.size() == 1); CHECK(segments.front().kind == nav::SegmentKind::STRAIGHT); - CHECK(segments.front().length == doctest::Approx(start_distance + 3 * cell_size + 0.09F)); + CHECK( + static_cast(segments.front().length) == + doctest::Approx(static_cast(start_distance + 3 * cell_size + 0.09F)) + ); CHECK(same_pose( segments.front().start, {.position = {.x = cell_size / 2.0F, .y = cell_size - start_distance}, .orientation = std::numbers::pi_v / 2.0F} @@ -91,9 +98,15 @@ TEST_SUITE("route_compiler") { CHECK(segments.at(0).kind == nav::SegmentKind::STRAIGHT); CHECK(segments.at(1).kind == nav::SegmentKind::TURN); CHECK(segments.at(2).kind == nav::SegmentKind::STRAIGHT); - CHECK(segments.at(0).length == doctest::Approx(start_distance + cell_size + shape.pre)); - CHECK(segments.at(1).length == doctest::Approx(-shape.length())); - CHECK(segments.at(2).length == doctest::Approx(shape.post + 2 * cell_size + 0.09F)); + CHECK( + static_cast(segments.at(0).length) == + doctest::Approx(static_cast(start_distance + cell_size + shape.pre)) + ); + CHECK(static_cast(segments.at(1).length) == doctest::Approx(static_cast(-shape.length()))); + CHECK( + static_cast(segments.at(2).length) == + doctest::Approx(static_cast(shape.post + 2 * cell_size + 0.09F)) + ); CHECK(same_pose(end_of(segments.at(0), fast_profile), segments.at(1).start)); CHECK(same_pose(end_of(segments.at(1), fast_profile), segments.at(2).start)); CHECK(same_pose( diff --git a/tests/proxy/test_fmac_filter.cpp b/tests/proxy/test_fmac_filter.cpp index 5d36d3c..a168e49 100644 --- a/tests/proxy/test_fmac_filter.cpp +++ b/tests/proxy/test_fmac_filter.cpp @@ -19,8 +19,9 @@ const core::ButterworthFilter::Config filter_config{ .cutoff_frequency = 200.0F, .sampling_frequency = 1000.0F, }; +} // namespace -std::vector direct_form(const std::vector& inputs) { +static std::vector direct_form(const std::vector& inputs) { const core::ButterworthFilter::Coefficients coefficients = core::ButterworthFilter::compute_coefficients(filter_config); std::array input_history{}; @@ -46,7 +47,6 @@ std::vector direct_form(const std::vector& inputs) { return outputs; } -} // namespace TEST_SUITE("fmac_filter") { TEST_CASE_FIXTURE(HostBoard, "filters as the Butterworth filter it was designed as") { @@ -55,6 +55,7 @@ TEST_SUITE("fmac_filter") { std::vector inputs; + inputs.reserve(300); for (int sample = 0; sample < 300; sample++) { inputs.push_back((sample < 150 ? 0.5 : -0.25) + 0.1 * std::sin(0.9 * sample)); } @@ -63,10 +64,10 @@ TEST_SUITE("fmac_filter") { for (std::size_t sample = 0; sample < inputs.size(); sample++) { CAPTURE(sample); - const double output = filter.update(static_cast(inputs.at(sample))); + const auto output = static_cast(filter.update(static_cast(inputs.at(sample)))); CHECK(std::abs(output - expected.at(sample)) < 2e-3); - CHECK(filter.get_last() == static_cast(output)); + CHECK(static_cast(filter.get_last()) == output); } } diff --git a/tests/proxy/test_imu.cpp b/tests/proxy/test_imu.cpp index 07dae91..fe30f2d 100644 --- a/tests/proxy/test_imu.cpp +++ b/tests/proxy/test_imu.cpp @@ -9,6 +9,7 @@ #include "host_fixture.hpp" #include "micras/hal/host/board.hpp" #include "micras/hal/host/clock.hpp" +#include "micras/hal/host/spi_device.hpp" #include "micras/models/lsm6dsv_model.hpp" #include "micras/proxy/imu.hpp" @@ -25,15 +26,16 @@ const proxy::Imu::Config imu_config{ .gyroscope_filter = LSM6DSV_GY_ULTRA_LIGHT, .accelerometer_filter = LSM6DSV_XL_MEDIUM, }; +} // namespace -void attach(const proxy::Imu::Config& config, hal::host::SpiDevice& device) { +static void attach(const proxy::Imu::Config& config, hal::host::SpiDevice& device) { hal::host::Board::spi_device(config.spi.handle, config.spi.cs_gpio.port, config.spi.cs_gpio.pin, device); } -bool near(double value, double expected, double tolerance) { - return value >= expected - tolerance and value <= expected + tolerance; +static bool near(float value, double expected, double tolerance) { + const auto actual = static_cast(value); + return actual >= expected - tolerance and actual <= expected + tolerance; } -} // namespace TEST_SUITE("imu") { TEST_CASE_FIXTURE(HostBoard, "sets the chip up as its configuration says") { diff --git a/tests/proxy/test_rotary_sensor.cpp b/tests/proxy/test_rotary_sensor.cpp index f827862..1dcada3 100644 --- a/tests/proxy/test_rotary_sensor.cpp +++ b/tests/proxy/test_rotary_sensor.cpp @@ -9,11 +9,14 @@ #include +#include "crc.h" #include "host_fixture.hpp" #include "micras/hal/crc.hpp" #include "micras/hal/host/board.hpp" +#include "micras/hal/host/spi_device.hpp" #include "micras/models/as5047u_model.hpp" #include "micras/proxy/rotary_sensor.hpp" +#include "spi.h" namespace micras::test { namespace { @@ -35,33 +38,35 @@ const proxy::RotarySensor::Registers registers{ .settings3 = {{.UVWPP = 0, .HYS = 0, .ABIRES = 0b100}}, .ecc = {{.ECC_chsum = 0, .ECC_en = 0}}, }; +} // namespace -const proxy::RotarySensor::Config rotary_sensor_config{ - .spi = - { - .init_function = MX_SPI3_Init, - .handle = &hspi3, - .cs_gpio = spi_config.cs_gpio, - .timeout = 2, - .clock_polarity = SPI_POLARITY_LOW, - .clock_phase = SPI_PHASE_2EDGE, - }, - .encoder = encoder_config, - .crc = crc_config, - .registers = registers, -}; +static proxy::RotarySensor::Config rotary_sensor_config() { + return { + .spi = + { + .init_function = MX_SPI3_Init, + .handle = &hspi3, + .cs_gpio = spi_config.cs_gpio, + .timeout = 2, + .clock_polarity = SPI_POLARITY_LOW, + .clock_phase = SPI_PHASE_2EDGE, + }, + .encoder = encoder_config, + .crc = crc_config, + .registers = registers, + }; +} -void attach(const proxy::RotarySensor::Config& config, hal::host::SpiDevice& device) { +static void attach(const proxy::RotarySensor::Config& config, hal::host::SpiDevice& device) { hal::host::Board::spi_device(config.spi.handle, config.spi.cs_gpio.port, config.spi.cs_gpio.pin, device); } -} // namespace TEST_SUITE("rotary_sensor") { TEST_CASE_FIXTURE(HostBoard, "sets the chip up and reads its resolution back") { models::As5047uModel chip; - attach(rotary_sensor_config, chip); + attach(rotary_sensor_config(), chip); - const proxy::RotarySensor sensor{rotary_sensor_config}; + const proxy::RotarySensor sensor{rotary_sensor_config()}; CHECK(sensor.was_initialized()); CHECK(sensor.get_resolution() == 16384); @@ -72,18 +77,21 @@ TEST_SUITE("rotary_sensor") { TEST_CASE_FIXTURE(HostBoard, "reads the position the encoder counted") { models::As5047uModel chip; - attach(rotary_sensor_config, chip); - const proxy::RotarySensor sensor{rotary_sensor_config}; + attach(rotary_sensor_config(), chip); + const proxy::RotarySensor sensor{rotary_sensor_config()}; - hal::host::Board::encoder(rotary_sensor_config.encoder.handle).count = 4096; + hal::host::Board::encoder(rotary_sensor_config().encoder.handle).count = 4096; - CHECK(sensor.get_position() == doctest::Approx(std::numbers::pi_v / 2.0F)); + CHECK( + static_cast(sensor.get_position()) == + doctest::Approx(static_cast(std::numbers::pi_v / 2.0F)) + ); } TEST_CASE_FIXTURE(HostBoard, "fails when the chip rejects the frames for their CRC") { CRC_HandleTypeDef wrong_crc = hcrc; wrong_crc.Init.InitValue = 0x00; - proxy::RotarySensor::Config config = rotary_sensor_config; + proxy::RotarySensor::Config config = rotary_sensor_config(); config.crc.handle = &wrong_crc; models::As5047uModel chip; attach(config, chip); diff --git a/tests/proxy/test_storage.cpp b/tests/proxy/test_storage.cpp index b5d0e78..1555f53 100644 --- a/tests/proxy/test_storage.cpp +++ b/tests/proxy/test_storage.cpp @@ -3,6 +3,8 @@ */ #include +#include +#include #include #include @@ -10,7 +12,6 @@ #include "host_fixture.hpp" #include "micras/core/serializable.hpp" #include "micras/core/variable_pool.hpp" -#include "micras/hal/flash.hpp" #include "micras/hal/host/board.hpp" #include "micras/proxy/storage.hpp" diff --git a/tests/support/maze_reader.hpp b/tests/support/maze_reader.hpp index cccedab..e67770a 100644 --- a/tests/support/maze_reader.hpp +++ b/tests/support/maze_reader.hpp @@ -91,15 +91,24 @@ inline MazeText read_maze(const std::filesystem::path& path) { const std::size_t column = 4 * static_cast(x); const nav::GridPoint cell{.x = x, .y = y}; - maze.walls.emplace_back(nav::GridPose{cell, nav::Side::RIGHT}, character(row, column + 4) == '|'); - maze.walls.emplace_back(nav::GridPose{cell, nav::Side::UP}, character(row - 1, column + 2) == '-'); + maze.walls.emplace_back( + nav::GridPose{.position = cell, .orientation = nav::Side::RIGHT}, character(row, column + 4) == '|' + ); + maze.walls.emplace_back( + nav::GridPose{.position = cell, .orientation = nav::Side::UP}, character(row - 1, column + 2) == '-' + ); if (x == 0) { - maze.walls.emplace_back(nav::GridPose{cell, nav::Side::LEFT}, character(row, column) == '|'); + maze.walls.emplace_back( + nav::GridPose{.position = cell, .orientation = nav::Side::LEFT}, character(row, column) == '|' + ); } if (y == 0) { - maze.walls.emplace_back(nav::GridPose{cell, nav::Side::DOWN}, character(row + 1, column + 2) == '-'); + maze.walls.emplace_back( + nav::GridPose{.position = cell, .orientation = nav::Side::DOWN}, + character(row + 1, column + 2) == '-' + ); } const char mark = character(row, column + 2); From c6c958ce67db4c209b32a235ec482d975c8198f7 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 06:19:54 -0300 Subject: [PATCH 536/561] =?UTF-8?q?=F0=9F=94=97=20Keep=20the=20core's=20ar?= =?UTF-8?q?chive=20after=20the=20HAL's=20on=20a=20consumer's=20link=20line?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The HAL uses nothing of the core, but without a link between them CMake puts the core's archive before the HAL's, and the linker then places the robot's objects in another order. A private link keeps the order the firmware's image was built with, without giving the HAL's users the core's include directories. --- micras_hal/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/micras_hal/CMakeLists.txt b/micras_hal/CMakeLists.txt index 17fe667..ccbd7db 100644 --- a/micras_hal/CMakeLists.txt +++ b/micras_hal/CMakeLists.txt @@ -20,6 +20,12 @@ target_link_libraries(micras_hal PUBLIC ${MICRAS_LIB_CUBE_TARGET} ) +# The HAL uses nothing of the core. The link only keeps the core's archive after the HAL's on a +# consumer's link line, the order the robot's image was verified with. +target_link_libraries(micras_hal PRIVATE + micras::core +) + if(MICRAS_LIB_HAL_BACKEND STREQUAL "stm32") # The family is read from the device macro CubeMX puts on its interface target if(MICRAS_LIB_STM32_FAMILY) From 2cba86931e68bf44b69465d8cbc81daa5d101f63 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 06:19:54 -0300 Subject: [PATCH 537/561] =?UTF-8?q?=F0=9F=94=A5=20Leave=20the=20robot's=20?= =?UTF-8?q?wall=20sensor=20layout=20and=20radio=20wiring=20to=20the=20robo?= =?UTF-8?q?t?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit WallSensorsIndex names the wall sensors of one robot, so the robot that has them defines it. The communication README keeps the protocol; the HM-19 module, its wiring and its AT setup belong to the firmware of the board that carries them. --- micras_comm/README.md | 55 +++++------------------ micras_core/include/micras/core/types.hpp | 10 ----- 2 files changed, 10 insertions(+), 55 deletions(-) diff --git a/micras_comm/README.md b/micras_comm/README.md index 56d9a17..8455463 100644 --- a/micras_comm/README.md +++ b/micras_comm/README.md @@ -1,50 +1,15 @@ -# Bluetooth link +# Communication protocol -The robot exposes its variables over an HM-19 BLE module wired to `UART4` (`PA11` = RX, -`PA12` = TX) through the USB-C receptacle. The companion application is -[micras-monitor](https://github.com/Team-Micras/micras-monitor). +The session layer the robot runs over a byte stream to expose its variables to an application such +as [micras-monitor](https://github.com/Team-Micras/micras-monitor): a schema, streamed groups of +samples, writes, reads and commands. It knows nothing of the transport below the stream; the project +that uses it documents the radio and the wiring it runs over. -## Wiring +The protocol assumes a transport with no flow control, which drops bytes silently when its buffer +fills, as a serial Bluetooth module without `RTS`/`CTS` does. The credit window of the session layer +exists to keep it from filling. -The module's serial port *is* the USB-C receptacle. `D-` reaches `PA11` and `D+` reaches `PA12`, -each through a 33 ฮฉ series resistor, and `SBU1`/`SBU2` supply 3V3 through a 300 mA resettable fuse. -`CC1` and `CC2` terminate in 5.1 kฮฉ resistors and never reach the microcontroller. - -One consequence of that shapes the protocol: **there is no hardware flow control.** The CC2640R2F -inside the module has `RTS`/`CTS`, but the board cannot reach them, and every other conductor of the -receptacle is already used. The module therefore drops bytes silently when its buffer fills, which -is what the credit window in the session layer exists to prevent. - -`PA11` and `PA12` are also `USB_OTG_HS_DM` and `USB_OTG_HS_DP`, and the board carries the 5.1 kฮฉ -`CC` terminations and the 1.5 kฮฉ `D+` pull-up of a full speed USB device, so the same receptacle -can be a USB port instead. It cannot be both at once, and the radio owns it. - -## Configuring the module - -The module needs these once, over AT commands, with no application connected. Send them without a -line ending and wait for each reply. - -| Command | Reply | Why | -| --- | --- | --- | -| `AT` | `OK` | Check that the module is in command mode. | -| `AT+ROLE0` | `OK+Set:0` | Peripheral. The application is the central. | -| `AT+NAMEMicras` | `OK+Set:Micras` | What the application looks for. | -| `AT+BAUD4` | `OK+Set:4` | 115 200, matching `MX_UART4_Init`. | -| `AT+COMI0` | `OK+Set:0` | Ask for a 7.5 ms minimum connection interval. | -| `AT+COMA0` | `OK+Set:0` | Ask for a 7.5 ms maximum connection interval. | -| `AT+RESET` | `OK+RESET` | Apply. | - -`AT+COMI` and `AT+COMA` are a request: the central decides the interval it actually uses, and a -browser gives the page no way to read or influence it. The throughput is therefore only knowable by -measuring it, which the `link/dropped_samples` counter and the sample sequence numbers make easy. - -Raising the serial speed to 230 400 is `AT+BAUD8` here and `Dma.UART4_RX` plus the baud rate in -`cube/micras_v1.ioc` on the firmware side. It is only worth doing once a measurement shows the -serial port, rather than the radio, is the limit. - -The application connects to service `FFE0`, characteristic `FFE1`, with notifications enabled. - -## Protocol +## Framing Frames are [COBS](https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing) encoded and delimited by a zero byte: @@ -67,7 +32,7 @@ Message types, their payloads and the meaning of every field are in `micras_comm/include/micras/comm/protocol.hpp`. Everything on the wire is little endian, which is what both the microcontroller and `DataView` in the browser already are. -### The parts worth knowing before reading the code +## The parts worth knowing before reading the code - **The schema is fetched once per firmware build, not once per connection.** `HELLO_ACK` carries a `schema_hash` over every name, type and access flag in order. Identifiers are registration order, diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index 081c05c..3d5ef17 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -16,16 +16,6 @@ enum class Objective : uint8_t { RETURN = 1, SOLVE = 2 }; - -/** - * @brief Struct to store the index of each of the wall sensors. - */ -struct WallSensorsIndex { - uint8_t left_front{}; - uint8_t left{}; - uint8_t right{}; - uint8_t right_front{}; -}; } // namespace micras::core #endif // MICRAS_CORE_TYPES_HPP From bcaa4d6f1d40b39b84fb7a15ff51e3f4464d33cb Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 06:23:50 -0300 Subject: [PATCH 538/561] =?UTF-8?q?=E2=9C=A8=20Build=20and=20link=20a=20be?= =?UTF-8?q?nch=20on=20a=20reference=20STM32H725=20project?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit reference/h7 is the smallest CubeMX project that exercises every peripheral the HAL and the proxies use, derived from Micras v1's with nothing of Micras left: the internal oscillator through the PLL at 400 MHz, no main() and no callback registration. It glues the generated tree in as a consumer does, adds micras-lib with warnings as errors, and links micras_lib_bench_h7, which proves that the vendor callbacks micras-lib defines win over the weak ones and checks, on any board with the part, a storage restored across a watchdog reset, the reset cause, the timer clock against the cycle counter, the CRC unit, the FMAC and the internal reference over DMA. Its lint target covers the stm32 backend against the ARM headers. The Dockerfile generates its Cube files and adds the ARM toolchain to the host stage, the reference-h7 preset builds it from the root, and the tests' host board mirrors its pins and clock functions. --- .docker/Dockerfile | 44 +++ .docker/Dockerfile.dockerignore | 6 + CMakePresets.json | 14 + reference/h7/.gitignore | 6 + reference/h7/CMakeLists.txt | 156 +++++++++ reference/h7/bench.cpp | 307 +++++++++++++++++ reference/h7/reference.ioc | 494 +++++++++++++++++++++++++++ tests/hal/test_mcu.cpp | 6 +- tests/support/host_board/handles.cpp | 11 +- tests/support/host_board/main.h | 16 +- 10 files changed, 1051 insertions(+), 9 deletions(-) create mode 100644 .docker/Dockerfile.dockerignore create mode 100644 reference/h7/.gitignore create mode 100644 reference/h7/CMakeLists.txt create mode 100644 reference/h7/bench.cpp create mode 100644 reference/h7/reference.ioc diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 37af454..4f43eb5 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -37,3 +37,47 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ WORKDIR /project # <<< host stage <<< + +############################################################# +# Reference projects' Cube files, generated by STM32CubeMX # +############################################################# +FROM thunderatz/stm32cubemx:6.18.1-h7 AS generator-h7 + +WORKDIR /project +COPY reference/h7/reference.ioc reference/h7/ + +# Without network, CubeMX skips its online update check, which can hang the project generation. The +# CMakeLists.txt and CMakePresets.json CubeMX writes next to the .ioc are its own suggestion of a +# project, which the reference replaces, so they are left out of the generated tree. +RUN --network=none < /tmp/cube_script + xvfb-run --auto-servernum "${CUBE_PATH}/STM32CubeMX" -q /tmp/cube_script + rm /tmp/cube_script + test -f reference/h7/cmake/stm32cubemx/CMakeLists.txt || { echo "STM32CubeMX failed to generate the project" >&2; exit 1; } + rm -f reference/h7/CMakeLists.txt reference/h7/CMakePresets.json reference/h7/reference.ioc +EOT + +# Only the generated files, can be replaced by already generated ones with --build-context generated-h7= +FROM scratch AS generated-h7 + +COPY --from=generator-h7 /project/reference/h7 / + +########################################################## +# ARM toolchain for the reference projects and benches # +########################################################## +FROM host AS stm32 + +# The ARM libraries come prebuilt for every ARM core, only the Cortex-M4F and Cortex-M7 double +# precision ones are kept +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + apt-get update && \ + apt-get install -y --no-install-recommends \ + gcc-arm-none-eabi \ + libnewlib-arm-none-eabi \ + libstdc++-arm-none-eabi-newlib \ + make && \ + find /usr/lib/arm-none-eabi/lib/thumb /usr/lib/gcc/arm-none-eabi/*/thumb -mindepth 1 -maxdepth 1 \ + -not -name "v7e-m+fp" -not -name "v7e-m+dp" -exec rm -rf {} + && \ + rm -rf /usr/lib/arm-none-eabi/lib/arm /usr/lib/gcc/arm-none-eabi/*/arm diff --git a/.docker/Dockerfile.dockerignore b/.docker/Dockerfile.dockerignore new file mode 100644 index 0000000..fd65b4d --- /dev/null +++ b/.docker/Dockerfile.dockerignore @@ -0,0 +1,6 @@ +# The images need no source: the CI mounts the checkout into the containers it runs +.git/ +build/ +docs/ +reference/*/* +!reference/*/reference.ioc diff --git a/CMakePresets.json b/CMakePresets.json index ff9d9c5..afc1bad 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -36,6 +36,16 @@ "cacheVariables": { "CMAKE_BUILD_TYPE": "Release" } + }, + { + "name": "reference-h7", + "displayName": "Reference STM32H725 project and its bench program", + "generator": "Unix Makefiles", + "binaryDir": "${sourceDir}/build/${presetName}", + "cacheVariables": { + "MICRAS_LIB_REFERENCE": "h7", + "CMAKE_BUILD_TYPE": "Release" + } } ], "buildPresets": [ @@ -46,6 +56,10 @@ { "name": "host-release", "configurePreset": "host-release" + }, + { + "name": "reference-h7", + "configurePreset": "reference-h7" } ], "testPresets": [ diff --git a/reference/h7/.gitignore b/reference/h7/.gitignore new file mode 100644 index 0000000..765b11c --- /dev/null +++ b/reference/h7/.gitignore @@ -0,0 +1,6 @@ +# The Cube files STM32CubeMX generates from reference.ioc +* +!.gitignore +!CMakeLists.txt +!bench.cpp +!reference.ioc diff --git a/reference/h7/CMakeLists.txt b/reference/h7/CMakeLists.txt new file mode 100644 index 0000000..00302c3 --- /dev/null +++ b/reference/h7/CMakeLists.txt @@ -0,0 +1,156 @@ +cmake_minimum_required(VERSION 3.28...4.4) + +############################################################################### +## Reference STM32H725 project +############################################################################### + +# The smallest CubeMX project that exercises every peripheral micras-lib uses, built and linked with +# its bench program the way a consumer of the stm32 backend builds its firmware. The Cube files are +# generated from reference.ioc (the CI generates them with .docker/Dockerfile) and never committed. + +set(CMAKE_C_STANDARD 17) +set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_C_EXTENSIONS ON) + +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_CXX_SCAN_FOR_MODULES OFF) + +set(MICRAS_REFERENCE_DIR ${CMAKE_CURRENT_LIST_DIR}) + +if(NOT EXISTS ${MICRAS_REFERENCE_DIR}/cmake/stm32cubemx/CMakeLists.txt) + message(FATAL_ERROR + "micras-lib reference/h7: the Cube files are not generated. Generate them into reference/h7 with " + "'docker build --file .docker/Dockerfile --target generated-h7 --output type=local,dest=reference/h7 .' " + "from the root of micras-lib, or with STM32CubeMX from reference.ioc.") +endif() + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +############################################################################### +## Toolchain +############################################################################### + +# The generated toolchain file names the linker script relative to CMAKE_SOURCE_DIR, so it is pointed +# at the generated tree while the file is read +set(MICRAS_REFERENCE_SOURCE_DIR ${CMAKE_SOURCE_DIR}) +set(CMAKE_SOURCE_DIR ${MICRAS_REFERENCE_DIR}) +include(${MICRAS_REFERENCE_DIR}/cmake/gcc-arm-none-eabi.cmake) +set(CMAKE_SOURCE_DIR ${MICRAS_REFERENCE_SOURCE_DIR}) + +# One map file per target instead of the one name the generated file gives every target +string(REGEX REPLACE " *-Wl,-Map=[^ ]*" "" CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS}") + +# The flags a consumer builds its firmware with: the same as the Micras firmware's +set(MICRAS_MATH_FLAGS "-fno-math-errno -ffp-contract=fast") + +set(CMAKE_C_FLAGS_DEBUG "-Og -g3") +set(CMAKE_CXX_FLAGS_DEBUG "-Og -g3") +set(CMAKE_C_FLAGS_RELEASE "-O2 -g3 ${MICRAS_MATH_FLAGS}") +set(CMAKE_CXX_FLAGS_RELEASE "-O2 -g3 ${MICRAS_MATH_FLAGS}") + +# Link time optimization through the property, so that CMake archives with gcc-ar +set(CMAKE_INTERPROCEDURAL_OPTIMIZATION_RELEASE TRUE) +set(CMAKE_EXE_LINKER_FLAGS_RELEASE "-O2") + +project(micras_lib_reference_h7 C CXX ASM) + +############################################################################### +## Generated tree +############################################################################### + +# The STM32CubeMX CMake generator writes its sources into ${CMAKE_PROJECT_NAME} instead of a target of +# its own, so the variable is pointed at a target created to hold them +add_library(cube_app OBJECT) +set(MICRAS_REFERENCE_PROJECT_NAME ${CMAKE_PROJECT_NAME}) +set(CMAKE_PROJECT_NAME cube_app) +add_subdirectory(${MICRAS_REFERENCE_DIR}/cmake/stm32cubemx stm32cubemx) +set(CMAKE_PROJECT_NAME ${MICRAS_REFERENCE_PROJECT_NAME}) + +# The generated headers are not the project's to warn about +set_target_properties(stm32cubemx PROPERTIES SYSTEM TRUE) + +foreach(CUBE_OBJECT_LIBRARY cube_app STM32_Drivers) + target_compile_options(${CUBE_OBJECT_LIBRARY} PRIVATE -w) +endforeach() + +############################################################################### +## micras-lib +############################################################################### + +set(MICRAS_LIB_WERROR ON) +add_subdirectory(${MICRAS_REFERENCE_DIR}/../.. micras-lib EXCLUDE_FROM_ALL) + +############################################################################### +## Bench program +############################################################################### + +# The vector table, the interrupt handlers and the MX_*_Init functions are linked into the executable, +# which is where micras-lib's callbacks have to win over the vendor's weak ones +add_executable(micras_lib_bench_h7 + ${MICRAS_REFERENCE_DIR}/bench.cpp +) + +target_link_libraries(micras_lib_bench_h7 PRIVATE + micras::proxy + micras::nav + micras::comm + cube_app + STM32_Drivers +) + +micras_apply_warnings(micras_lib_bench_h7 WERROR ON) + +target_link_options(micras_lib_bench_h7 PRIVATE -Wl,-Map=$.map) + +add_custom_command(TARGET micras_lib_bench_h7 POST_BUILD + COMMAND ${CMAKE_OBJCOPY} -O ihex $ $.hex + COMMAND ${CMAKE_SIZE} $ +) + +############################################################################### +## Lint +############################################################################### + +# The stm32 backend and the bench are linted here, against the ARM headers, since no host build +# compiles them +include(${MICRAS_REFERENCE_DIR}/../../cmake/micras_lint.cmake) + +# clang finds the ARM target from the compiler's name, but not the headers of its toolchain +execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -print-search-dirs + OUTPUT_VARIABLE MICRAS_ARM_SEARCH_DIRS + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine + OUTPUT_VARIABLE MICRAS_ARM_TRIPLE + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +string(REGEX MATCH "install: ([^\n]+)/" MICRAS_ARM_INSTALL_MATCH ${MICRAS_ARM_SEARCH_DIRS}) +set(MICRAS_ARM_INSTALL_DIR ${CMAKE_MATCH_1}) +get_filename_component(MICRAS_ARM_GCC_VERSION ${MICRAS_ARM_INSTALL_DIR} NAME) +get_filename_component(MICRAS_ARM_PREFIX "${MICRAS_ARM_INSTALL_DIR}/../../../" REALPATH) +set(MICRAS_ARM_SYSROOT ${MICRAS_ARM_PREFIX}/${MICRAS_ARM_TRIPLE}) +set(MICRAS_ARM_CXX_INCLUDE_DIR ${MICRAS_ARM_SYSROOT}/include/c++/${MICRAS_ARM_GCC_VERSION}) + +file(GLOB MICRAS_REFERENCE_LINT_SOURCES CONFIGURE_DEPENDS + ${MICRAS_LIB_ROOT}/micras_hal/stm32/src/*.cpp + ${MICRAS_LIB_ROOT}/micras_hal/stm32/h7/src/*.cpp + ${MICRAS_LIB_ROOT}/reference/h7/bench.cpp +) + +micras_add_lint_targets( + SOURCES ${MICRAS_REFERENCE_LINT_SOURCES} + EXTRA_ARGS + --sysroot=${MICRAS_ARM_SYSROOT}/ + -I${MICRAS_ARM_CXX_INCLUDE_DIR}/ + -I${MICRAS_ARM_CXX_INCLUDE_DIR}/${MICRAS_ARM_TRIPLE}/ +) diff --git a/reference/h7/bench.cpp b/reference/h7/bench.cpp new file mode 100644 index 0000000..b9dd51d --- /dev/null +++ b/reference/h7/bench.cpp @@ -0,0 +1,307 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/core/butterworth_filter.hpp" +#include "micras/core/variable_pool.hpp" +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/crc.hpp" +#include "micras/hal/encoder.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/mcu.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/pwm_dma.hpp" +#include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" +#include "micras/hal/uart_dma.hpp" +#include "micras/proxy/fmac_filter.hpp" +#include "micras/proxy/storage.hpp" +#include "micras/proxy/watchdog.hpp" + +extern "C" { +void SystemClock_Config(); +} + +namespace { +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) +volatile bool storage_passed{}; +volatile bool watchdog_passed{}; +volatile float timer_clock_error{}; +volatile bool timer_passed{}; +volatile uint32_t crc_value{}; +volatile bool crc_passed{}; +volatile float fmac_error{}; +volatile bool fmac_passed{}; +volatile uint32_t vdda_mv{}; +volatile bool adc_passed{}; +volatile bool bench_done{}; +volatile bool start_pin_drivers{}; + +std::array adc_buffer{}; +std::array adc_snapshot{}; +std::array pwm_dma_compares{}; +std::array spi_transmitted{}; +std::array spi_received{}; +std::array uart_ring{}; + +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) + +constexpr uint32_t storage_magic{0x5EED1E55}; + +constexpr micras::proxy::Storage::Config storage_config{.start_sector = 0, .number_of_sectors = 1}; + +constexpr uint32_t watchdog_timeout_ms{50}; + +constexpr uint32_t timer_window_us{10000}; + +constexpr float timer_tolerance{1e-3F}; + +constexpr uint8_t crc_polynomial{0x1D}; + +constexpr uint8_t crc_initial_value{0xC4}; + +constexpr micras::core::ButterworthFilter::Config filter_config{ + .cutoff_frequency = 200.0F, + .sampling_frequency = 1000.0F, +}; + +constexpr float fmac_tolerance{2e-3F}; + +constexpr uint32_t adc_timeout_us{10000}; + +constexpr uint32_t min_vdda_mv{1620}; + +constexpr uint32_t max_vdda_mv{3600}; +} // namespace + +static void save_before_reset() { + micras::core::TVariablePool<1> pool; + uint32_t magic = storage_magic; + pool.add("bench/", "magic", magic, {.persist = true}); + + micras::proxy::Storage storage{storage_config}; + storage.save(pool); +} + +static bool restored_after_reset() { + micras::core::TVariablePool<1> pool; + uint32_t magic{}; + pool.add("bench/", "magic", magic, {.persist = true}); + + micras::proxy::Storage storage{storage_config}; + return storage.restore(pool) == 1 and magic == storage_magic; +} + +[[noreturn]] static void expire_watchdog() { + const micras::proxy::Watchdog watchdog{{.timeout_ms = watchdog_timeout_ms}}; + + while (true) { + bench_done = false; + } +} + +static bool check_timer_clock() { + MX_TIM2_Init(); + HAL_TIM_Base_Start(&htim2); + + const uint32_t start_cycles = micras::hal::Timer::get_counter(); + const uint32_t start_counts = __HAL_TIM_GET_COUNTER(&htim2); + const uint32_t window = micras::hal::Timer::to_cycles(timer_window_us); + + while (micras::hal::Timer::get_counter() - start_cycles < window) { } + + const uint32_t cycles = micras::hal::Timer::get_counter() - start_cycles; + const uint32_t counts = __HAL_TIM_GET_COUNTER(&htim2) - start_counts; + + RCC_ClkInitTypeDef clocks{}; + uint32_t latency{}; + HAL_RCC_GetClockConfig(&clocks, &latency); + + const uint32_t bus_clock = HAL_RCC_GetPCLK1Freq(); + const uint32_t timer_clock = clocks.APB1CLKDivider == RCC_APB1_DIV1 ? bus_clock : 2 * bus_clock; + const float measured = + static_cast(counts) * static_cast(SystemCoreClock) / static_cast(cycles); + const float error = measured / static_cast(timer_clock) - 1.0F; + + timer_clock_error = error; + return std::abs(error) < timer_tolerance; +} + +static uint8_t software_crc(std::span data) { + uint8_t crc = crc_initial_value; + + for (const uint8_t byte : data) { + crc ^= byte; + + for (uint8_t bit = 0; bit < 8; bit++) { + crc = (crc & 0x80U) != 0 ? static_cast((crc << 1U) ^ crc_polynomial) : + static_cast(crc << 1U); + } + } + + return crc; +} + +static bool check_crc() { + constexpr std::string_view check{"123456789"}; + const std::vector bytes(check.begin(), check.end()); + + micras::hal::Crc crc{{.handle = &hcrc}}; + const uint32_t value = crc.calculate(bytes); + + crc_value = value; + return value == software_crc(bytes); +} + +static bool check_fmac() { + micras::proxy::FmacFilter filter{{ + .fmac = {.init_function = MX_FMAC_Init, .handle = &hfmac}, + .filter = filter_config, + }}; + + if (not filter.was_initialized()) { + return false; + } + + const micras::core::ButterworthFilter::Coefficients coefficients = + micras::core::ButterworthFilter::compute_coefficients(filter_config); + std::array inputs{}; + std::array outputs{}; + float largest_error{}; + + for (uint32_t sample = 0; sample < 300; sample++) { + const float input = (sample < 150 ? 0.5F : -0.25F) + 0.1F * std::sin(0.9F * static_cast(sample)); + + inputs = {input, inputs.at(0), inputs.at(1)}; + + const float expected = + coefficients.feed_forward.at(0) * inputs.at(0) + coefficients.feed_forward.at(1) * inputs.at(1) + + coefficients.feed_forward.at(2) * inputs.at(2) - coefficients.feedback.at(0) * outputs.at(0) - + coefficients.feedback.at(1) * outputs.at(1); + + outputs = {expected, outputs.at(0)}; + largest_error = std::max(largest_error, std::abs(filter.update(input) - expected)); + } + + fmac_error = largest_error; + return largest_error < fmac_tolerance; +} + +static bool check_adc() { + micras::hal::AdcDma adc{{ + .init_function = MX_ADC3_Init, + .handle = &hadc3, + .max_reading = 4095, + .reference_voltage = 3.3F, + }}; + + if (not adc.start_dma(adc_buffer, adc_snapshot)) { + return false; + } + + std::array readings{}; + const uint32_t start = micras::hal::Timer::get_counter(); + const uint32_t timeout = micras::hal::Timer::to_cycles(adc_timeout_us); + + while (adc.read_snapshot(readings) == 0) { + if (micras::hal::Timer::get_counter() - start > timeout) { + adc.stop_dma(); + return false; + } + } + + adc.stop_dma(); + + const uint32_t vdda = + __LL_ADC_CALC_VREFANALOG_VOLTAGE(static_cast(readings.at(0)), LL_ADC_RESOLUTION_12B); + + vdda_mv = vdda; + return vdda >= min_vdda_mv and vdda <= max_vdda_mv; +} + +static void run_pin_drivers() { + MX_GPIO_Init(); + + micras::hal::Gpio led{{.port = Status_LED_GPIO_Port, .pin = Status_LED_Pin}}; + const micras::hal::Gpio button{{.port = Button_GPIO_Port, .pin = Button_Pin}}; + led.write(button.read()); + + micras::hal::Pwm pwm{ + {.init_function = MX_TIM15_Init, .handle = &htim15, .timer_channel = TIM_CHANNEL_1, .inverted = false} + }; + pwm.set_duty_cycle(50.0F); + + micras::hal::PwmDma pwm_dma{{.init_function = MX_TIM8_Init, .handle = &htim8, .timer_channel = TIM_CHANNEL_1}}; + pwm_dma_compares.fill(static_cast(pwm_dma.get_compare(50.0F))); + pwm_dma.start_dma(pwm_dma_compares); + + const micras::hal::Encoder encoder{ + {.init_function = MX_TIM5_Init, .handle = &htim5, .timer_channel = TIM_CHANNEL_ALL} + }; + led.write(encoder.get_counter() > 0); + + micras::hal::Spi spi{{ + .init_function = MX_SPI3_Init, + .handle = &hspi3, + .cs_gpio = {.port = SPI_CSn_GPIO_Port, .pin = SPI_CSn_Pin}, + .timeout = 2, + .clock_polarity = SPI_POLARITY_HIGH, + .clock_phase = SPI_PHASE_2EDGE, + }}; + spi.start_transfer(spi_transmitted, spi_received); + + micras::hal::UartDma uart{{.init_function = MX_UART4_Init, .handle = &huart4}}; + uart.start_rx(uart_ring); + uart.start_tx(spi_received); +} + +int main() { + constexpr std::array peripheral_inits{MX_DMA_Init, MX_CRC_Init}; + + micras::hal::Mcu::init({ + .clock_init = SystemClock_Config, + .peripheral_clock_init = nullptr, + .peripheral_inits = peripheral_inits, + .cpu_frequency_boost = false, + }); + + if (not micras::hal::Mcu::was_reset_by_watchdog()) { + save_before_reset(); + expire_watchdog(); + } + + watchdog_passed = true; + storage_passed = restored_after_reset(); + timer_passed = check_timer_clock(); + crc_passed = check_crc(); + fmac_passed = check_fmac(); + adc_passed = check_adc(); + bench_done = true; + + if (start_pin_drivers) { + run_pin_drivers(); + } + + while (true) { + bench_done = true; + } +} diff --git a/reference/h7/reference.ioc b/reference/h7/reference.ioc new file mode 100644 index 0000000..b55858d --- /dev/null +++ b/reference/h7/reference.ioc @@ -0,0 +1,494 @@ +#MicroXplorer Configuration settings - do not modify +ADC3.Channel-0\#ChannelRegularConversion=ADC_CHANNEL_VREFINT +ADC3.Channel-1\#ChannelRegularConversion=ADC_CHANNEL_VBAT +ADC3.ClockPrescaler=ADC_CLOCK_ASYNC_DIV4 +ADC3.ClockPrescalerADC3=ADC_CLOCK_ASYNC_DIV8 +ADC3.ContinuousConvMode=ENABLE +ADC3.ConversionDataManagement=ADC_CONVERSIONDATA_DMA_CIRCULAR +ADC3.DMAContinuousRequests=ENABLE +ADC3.IPParameters=Channel-0#ChannelRegularConversion,Rank-0#ChannelRegularConversion,SamplingTime-0#ChannelRegularConversion,OffsetNumber-0#ChannelRegularConversion,OffsetSign-0#ChannelRegularConversion,Channel-1#ChannelRegularConversion,Rank-1#ChannelRegularConversion,SamplingTime-1#ChannelRegularConversion,OffsetNumber-1#ChannelRegularConversion,OffsetSign-1#ChannelRegularConversion,NbrOfConversion,NbrOfConversionFlag,ScanConvMode,ClockPrescaler,ClockPrescalerADC3,ContinuousConvMode,ConversionDataManagement,DMAContinuousRequests,Overrun +ADC3.NbrOfConversion=2 +ADC3.NbrOfConversionFlag=1 +ADC3.OffsetNumber-0\#ChannelRegularConversion=ADC_OFFSET_NONE +ADC3.OffsetNumber-1\#ChannelRegularConversion=ADC_OFFSET_NONE +ADC3.OffsetSign-0\#ChannelRegularConversion=ADC3_OFFSET_SIGN_NEGATIVE +ADC3.OffsetSign-1\#ChannelRegularConversion=ADC3_OFFSET_SIGN_NEGATIVE +ADC3.Overrun=ADC_OVR_DATA_OVERWRITTEN +ADC3.Rank-0\#ChannelRegularConversion=1 +ADC3.Rank-1\#ChannelRegularConversion=2 +ADC3.SamplingTime-0\#ChannelRegularConversion=ADC3_SAMPLETIME_640CYCLES_5 +ADC3.SamplingTime-1\#ChannelRegularConversion=ADC3_SAMPLETIME_640CYCLES_5 +ADC3.ScanConvMode=ADC_SCAN_ENABLE +CAD.formats= +CAD.pinconfig= +CAD.provider= +CORTEX_M7.CPU_DCache=Disabled +CORTEX_M7.CPU_ICache=Enabled +CORTEX_M7.IPParameters=default_mode_Activation,CPU_ICache,CPU_DCache +CORTEX_M7.default_mode_Activation=0 +CRC.CRCLength=CRC_POLYLENGTH_8B +CRC.DefaultInitValueUse=DEFAULT_INIT_VALUE_DISABLE +CRC.DefaultPolynomialUse=DEFAULT_POLYNOMIAL_DISABLE +CRC.GeneratingPolynomial=X4+X3+X2+X0 +CRC.IPParameters=DefaultPolynomialUse,DefaultInitValueUse,CRCLength,GeneratingPolynomial,InitValue +CRC.InitValue=0xC4 +Dma.ADC3.3.Direction=DMA_PERIPH_TO_MEMORY +Dma.ADC3.3.EventEnable=DISABLE +Dma.ADC3.3.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.ADC3.3.Instance=DMA1_Stream5 +Dma.ADC3.3.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.ADC3.3.MemInc=DMA_MINC_ENABLE +Dma.ADC3.3.Mode=DMA_CIRCULAR +Dma.ADC3.3.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.ADC3.3.PeriphInc=DMA_PINC_DISABLE +Dma.ADC3.3.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.ADC3.3.Priority=DMA_PRIORITY_LOW +Dma.ADC3.3.RequestNumber=1 +Dma.ADC3.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.ADC3.3.SignalID=NONE +Dma.ADC3.3.SyncEnable=DISABLE +Dma.ADC3.3.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.ADC3.3.SyncRequestNumber=1 +Dma.ADC3.3.SyncSignalID=NONE +Dma.Request0=TIM8_CH1 +Dma.Request1=UART4_RX +Dma.Request2=UART4_TX +Dma.Request3=ADC3 +Dma.Request4=SPI3_RX +Dma.Request5=SPI3_TX +Dma.RequestsNb=6 +Dma.SPI3_RX.4.Direction=DMA_PERIPH_TO_MEMORY +Dma.SPI3_RX.4.EventEnable=DISABLE +Dma.SPI3_RX.4.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.SPI3_RX.4.Instance=DMA1_Stream6 +Dma.SPI3_RX.4.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.SPI3_RX.4.MemInc=DMA_MINC_ENABLE +Dma.SPI3_RX.4.Mode=DMA_NORMAL +Dma.SPI3_RX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.SPI3_RX.4.PeriphInc=DMA_PINC_DISABLE +Dma.SPI3_RX.4.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.SPI3_RX.4.Priority=DMA_PRIORITY_LOW +Dma.SPI3_RX.4.RequestNumber=1 +Dma.SPI3_RX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.SPI3_RX.4.SignalID=NONE +Dma.SPI3_RX.4.SyncEnable=DISABLE +Dma.SPI3_RX.4.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.SPI3_RX.4.SyncRequestNumber=1 +Dma.SPI3_RX.4.SyncSignalID=NONE +Dma.SPI3_TX.5.Direction=DMA_MEMORY_TO_PERIPH +Dma.SPI3_TX.5.EventEnable=DISABLE +Dma.SPI3_TX.5.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.SPI3_TX.5.Instance=DMA1_Stream7 +Dma.SPI3_TX.5.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.SPI3_TX.5.MemInc=DMA_MINC_ENABLE +Dma.SPI3_TX.5.Mode=DMA_NORMAL +Dma.SPI3_TX.5.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.SPI3_TX.5.PeriphInc=DMA_PINC_DISABLE +Dma.SPI3_TX.5.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.SPI3_TX.5.Priority=DMA_PRIORITY_LOW +Dma.SPI3_TX.5.RequestNumber=1 +Dma.SPI3_TX.5.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.SPI3_TX.5.SignalID=NONE +Dma.SPI3_TX.5.SyncEnable=DISABLE +Dma.SPI3_TX.5.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.SPI3_TX.5.SyncRequestNumber=1 +Dma.SPI3_TX.5.SyncSignalID=NONE +Dma.TIM8_CH1.0.Direction=DMA_MEMORY_TO_PERIPH +Dma.TIM8_CH1.0.EventEnable=DISABLE +Dma.TIM8_CH1.0.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.TIM8_CH1.0.Instance=DMA1_Stream2 +Dma.TIM8_CH1.0.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.TIM8_CH1.0.MemInc=DMA_MINC_ENABLE +Dma.TIM8_CH1.0.Mode=DMA_NORMAL +Dma.TIM8_CH1.0.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.TIM8_CH1.0.PeriphInc=DMA_PINC_DISABLE +Dma.TIM8_CH1.0.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.TIM8_CH1.0.Priority=DMA_PRIORITY_LOW +Dma.TIM8_CH1.0.RequestNumber=1 +Dma.TIM8_CH1.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.TIM8_CH1.0.SignalID=NONE +Dma.TIM8_CH1.0.SyncEnable=DISABLE +Dma.TIM8_CH1.0.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.TIM8_CH1.0.SyncRequestNumber=1 +Dma.TIM8_CH1.0.SyncSignalID=NONE +Dma.UART4_RX.1.Direction=DMA_PERIPH_TO_MEMORY +Dma.UART4_RX.1.EventEnable=DISABLE +Dma.UART4_RX.1.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.UART4_RX.1.Instance=DMA1_Stream3 +Dma.UART4_RX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.UART4_RX.1.MemInc=DMA_MINC_ENABLE +Dma.UART4_RX.1.Mode=DMA_CIRCULAR +Dma.UART4_RX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.UART4_RX.1.PeriphInc=DMA_PINC_DISABLE +Dma.UART4_RX.1.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.UART4_RX.1.Priority=DMA_PRIORITY_LOW +Dma.UART4_RX.1.RequestNumber=1 +Dma.UART4_RX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.UART4_RX.1.SignalID=NONE +Dma.UART4_RX.1.SyncEnable=DISABLE +Dma.UART4_RX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.UART4_RX.1.SyncRequestNumber=1 +Dma.UART4_RX.1.SyncSignalID=NONE +Dma.UART4_TX.2.Direction=DMA_MEMORY_TO_PERIPH +Dma.UART4_TX.2.EventEnable=DISABLE +Dma.UART4_TX.2.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.UART4_TX.2.Instance=DMA1_Stream4 +Dma.UART4_TX.2.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.UART4_TX.2.MemInc=DMA_MINC_ENABLE +Dma.UART4_TX.2.Mode=DMA_NORMAL +Dma.UART4_TX.2.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.UART4_TX.2.PeriphInc=DMA_PINC_DISABLE +Dma.UART4_TX.2.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.UART4_TX.2.Priority=DMA_PRIORITY_LOW +Dma.UART4_TX.2.RequestNumber=1 +Dma.UART4_TX.2.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.UART4_TX.2.SignalID=NONE +Dma.UART4_TX.2.SyncEnable=DISABLE +Dma.UART4_TX.2.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.UART4_TX.2.SyncRequestNumber=1 +Dma.UART4_TX.2.SyncSignalID=NONE +File.Version=6 +GPIO.groupedBy=Group By Peripherals +KeepUserPlacement=false +MMTAppReg1.MEMORYMAP.AppRegionName=RAM +MMTAppReg1.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg1.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg1.MEMORYMAP.DefaultDataRegion=true +MMTAppReg1.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,ContextName,Name,DefaultDataRegion +MMTAppReg1.MEMORYMAP.Name=RAM +MMTAppReg1.MEMORYMAP.Size=327680 +MMTAppReg1.MEMORYMAP.StartAddress=0x24000000 +MMTAppReg2.MEMORYMAP.AppRegionName=RAM_D2 +MMTAppReg2.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg2.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg2.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,ContextName,Name +MMTAppReg2.MEMORYMAP.Name=RAM_D2 +MMTAppReg2.MEMORYMAP.Size=32768 +MMTAppReg2.MEMORYMAP.StartAddress=0x30000000 +MMTAppReg3.MEMORYMAP.AppRegionName=RAM_D3 +MMTAppReg3.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg3.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg3.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,ContextName,Name +MMTAppReg3.MEMORYMAP.Name=RAM_D3 +MMTAppReg3.MEMORYMAP.Size=16384 +MMTAppReg3.MEMORYMAP.StartAddress=0x38000000 +MMTAppReg4.MEMORYMAP.AppRegionName=ITCMRAM +MMTAppReg4.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg4.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg4.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,ContextName,Name +MMTAppReg4.MEMORYMAP.Name=ITCMRAM +MMTAppReg4.MEMORYMAP.Size=65536 +MMTAppReg4.MEMORYMAP.StartAddress=0x00000000 +MMTAppReg5.MEMORYMAP.AppRegionName=FLASH +MMTAppReg5.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg5.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg5.MEMORYMAP.DefaultCodeRegion=true +MMTAppReg5.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,MemType,ContextName,Name,DefaultCodeRegion,ISRRegion,RootBootRegion +MMTAppReg5.MEMORYMAP.ISRRegion=true +MMTAppReg5.MEMORYMAP.MemType=ROM +MMTAppReg5.MEMORYMAP.Name=FLASH +MMTAppReg5.MEMORYMAP.RootBootRegion=true +MMTAppReg5.MEMORYMAP.Size=1048576 +MMTAppReg5.MEMORYMAP.StartAddress=0x08000000 +MMTAppReg6.MEMORYMAP.AppRegionName=DTCMRAM +MMTAppReg6.MEMORYMAP.ContextName=Cortex-M7NS +MMTAppReg6.MEMORYMAP.CoreName=Arm Cortex-M7 +MMTAppReg6.MEMORYMAP.IPParameters=StartAddress,Size,CoreName,ContextName,Name +MMTAppReg6.MEMORYMAP.Name=DTCMRAM +MMTAppReg6.MEMORYMAP.Size=131072 +MMTAppReg6.MEMORYMAP.StartAddress=0x20000000 +MMTAppRegionsCount=6 +MMTConfigApplied=false +MMTProjectManagerApplied=true +MMTSectionSuffix=_Section +Mcu.CPN=STM32H725RGV3 +Mcu.Family=STM32H7 +Mcu.IP0=ADC3 +Mcu.IP1=CORTEX_M7 +Mcu.IP10=SPI3 +Mcu.IP11=SYS +Mcu.IP12=TIM15 +Mcu.IP13=TIM2 +Mcu.IP14=TIM5 +Mcu.IP15=TIM8 +Mcu.IP16=UART4 +Mcu.IP2=CRC +Mcu.IP3=DEBUG +Mcu.IP4=DMA +Mcu.IP5=FMAC +Mcu.IP6=IWDG1 +Mcu.IP7=MEMORYMAP +Mcu.IP8=NVIC +Mcu.IP9=RCC +Mcu.IPNb=17 +Mcu.Name=STM32H725RGVx +Mcu.Package=VFQFPN68 +Mcu.Pin0=PA0 +Mcu.Pin1=PA1 +Mcu.Pin10=PC10 +Mcu.Pin11=PC11 +Mcu.Pin12=PC12 +Mcu.Pin13=PD2 +Mcu.Pin14=VP_ADC3_Vbat_Input +Mcu.Pin15=VP_ADC3_Vref_Input +Mcu.Pin16=VP_CRC_VS_CRC +Mcu.Pin17=VP_FMAC_VS_FMAC +Mcu.Pin18=VP_IWDG1_VS_IWDG +Mcu.Pin19=VP_SYS_VS_Systick +Mcu.Pin2=PA2 +Mcu.Pin20=VP_MEMORYMAP_VS_MEMORYMAP +Mcu.Pin21=VP_TIM2_VS_ClockSourceINT +Mcu.Pin3=PA7 +Mcu.Pin4=PA11 +Mcu.Pin5=PA12 +Mcu.Pin6=PA13(JTMS/SWDIO) +Mcu.Pin7=PA14(JTCK/SWCLK) +Mcu.Pin8=PB12 +Mcu.Pin9=PC6 +Mcu.PinsNb=22 +Mcu.ThirdPartyNb=0 +Mcu.UserConstants= +Mcu.UserName=STM32H725RGVx +MxCube.Version=6.18.1 +MxDb.Version=DB.6.0.181 +NVIC.ADC3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream7_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.ForceEnableDMAVector=true +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:false\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 +NVIC.SPI3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +PA0.GPIOParameters=GPIO_PuPd,GPIO_Label +PA0.GPIO_Label=Encoder_A +PA0.GPIO_PuPd=GPIO_PULLUP +PA0.Locked=true +PA0.Signal=S_TIM5_CH1 +PA1.GPIOParameters=GPIO_PuPd,GPIO_Label +PA1.GPIO_Label=Encoder_B +PA1.GPIO_PuPd=GPIO_PULLUP +PA1.Locked=true +PA1.Signal=S_TIM5_CH2 +PA11.Locked=true +PA11.Mode=Asynchronous +PA11.Signal=UART4_RX +PA12.Locked=true +PA12.Mode=Asynchronous +PA12.Signal=UART4_TX +PA13(JTMS/SWDIO).Locked=true +PA13(JTMS/SWDIO).Mode=Serial_Wire +PA13(JTMS/SWDIO).Signal=DEBUG_JTMS-SWDIO +PA14(JTCK/SWCLK).Locked=true +PA14(JTCK/SWCLK).Mode=Serial_Wire +PA14(JTCK/SWCLK).Signal=DEBUG_JTCK-SWCLK +PA2.GPIOParameters=GPIO_Label +PA2.GPIO_Label=PWM +PA2.Locked=true +PA2.Signal=S_TIM15_CH1 +PA7.GPIOParameters=GPIO_PuPd,GPIO_Label +PA7.GPIO_Label=Button +PA7.GPIO_PuPd=GPIO_PULLUP +PA7.Locked=true +PA7.Signal=GPIO_Input +PB12.GPIOParameters=GPIO_Label +PB12.GPIO_Label=Status_LED +PB12.Locked=true +PB12.Signal=GPIO_Output +PC10.Locked=true +PC10.Mode=Full_Duplex_Master +PC10.Signal=SPI3_SCK +PC11.Locked=true +PC11.Mode=Full_Duplex_Master +PC11.Signal=SPI3_MISO +PC12.Locked=true +PC12.Mode=Full_Duplex_Master +PC12.Signal=SPI3_MOSI +PC6.GPIOParameters=GPIO_Label +PC6.GPIO_Label=PWM_DMA +PC6.Locked=true +PC6.Signal=S_TIM8_CH1 +PD2.GPIOParameters=PinState,GPIO_Label +PD2.GPIO_Label=SPI_CSn +PD2.Locked=true +PD2.PinState=GPIO_PIN_SET +PD2.Signal=GPIO_Output +PinOutPanel.RotationAngle=0 +ProjectManager.AskForMigrate=true +ProjectManager.BackupPrevious=false +ProjectManager.CompilerLinker=GCC +ProjectManager.CompilerOptimize=6 +ProjectManager.ComputerToolchain=false +ProjectManager.CoupleFile=true +ProjectManager.CustomerFirmwarePackage= +ProjectManager.DefaultFWLocation=true +ProjectManager.DeletePrevious=true +ProjectManager.DeviceId=STM32H725RGVx +ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.13.0 +ProjectManager.FreePins=false +ProjectManager.FreePinsContext= +ProjectManager.HalAssertFull=false +ProjectManager.HeapSize=0x1000 +ProjectManager.KeepUserCode=false +ProjectManager.LastFirmware=true +ProjectManager.LibraryCopy=1 +ProjectManager.MainLocation=Src +ProjectManager.NoMain=true +ProjectManager.PreviousToolchain= +ProjectManager.ProjectBuild=false +ProjectManager.ProjectFileName=reference.ioc +ProjectManager.ProjectName=reference +ProjectManager.ProjectStructure= +ProjectManager.RegisterCallBack= +ProjectManager.StackSize=0x4000 +ProjectManager.TargetToolchain=CMake +ProjectManager.ToolChainLocation= +ProjectManager.UAScriptAfterPath= +ProjectManager.UAScriptBeforePath= +ProjectManager.UnderRoot=false +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_SPI3_Init-SPI3-false-HAL-true,5-MX_TIM2_Init-TIM2-false-HAL-true,6-MX_TIM5_Init-TIM5-false-HAL-true,7-MX_UART4_Init-UART4-false-HAL-true,8-MX_TIM8_Init-TIM8-false-HAL-true,9-MX_TIM15_Init-TIM15-false-HAL-true,10-MX_ADC3_Init-ADC3-false-HAL-true,11-MX_CRC_Init-CRC-false-HAL-true,12-MX_FMAC_Init-FMAC-false-HAL-true,13-MX_IWDG1_Init-IWDG1-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true +RCC.ADCFreq_Value=160000000 +RCC.AHB12Freq_Value=275000000 +RCC.AHB4Freq_Value=275000000 +RCC.APB1Freq_Value=137500000 +RCC.APB2Freq_Value=137500000 +RCC.APB3Freq_Value=137500000 +RCC.APB4Freq_Value=137500000 +RCC.AXIClockFreq_Value=275000000 +RCC.CECFreq_Value=32000 +RCC.CKPERFreq_Value=64000000 +RCC.CortexFreq_Value=550000000 +RCC.CpuClockFreq_Value=550000000 +RCC.D1CPREFreq_Value=550000000 +RCC.D1PPRE=RCC_APB3_DIV2 +RCC.D2PPRE1=RCC_APB1_DIV2 +RCC.D2PPRE2=RCC_APB2_DIV2 +RCC.D3PPRE=RCC_APB4_DIV2 +RCC.DFSDMACLkFreq_Value=275000000 +RCC.DFSDMFreq_Value=137500000 +RCC.DIVM1=4 +RCC.DIVM2=63 +RCC.DIVN1=50 +RCC.DIVN2=315 +RCC.DIVN3=125 +RCC.DIVP1=2 +RCC.DIVP1Freq_Value=550000000 +RCC.DIVP2Freq_Value=160000000 +RCC.DIVP3=2 +RCC.DIVP3Freq_Value=125000000 +RCC.DIVQ1Freq_Value=275000000 +RCC.DIVQ2=1 +RCC.DIVQ2Freq_Value=320000000 +RCC.DIVQ3=1 +RCC.DIVQ3Freq_Value=250000000 +RCC.DIVR1Freq_Value=275000000 +RCC.DIVR2Freq_Value=160000000 +RCC.DIVR3=1 +RCC.DIVR3Freq_Value=250000000 +RCC.FDCANFreq_Value=275000000 +RCC.FMCFreq_Value=275000000 +RCC.FamilyName=M +RCC.HCLK3ClockFreq_Value=275000000 +RCC.HCLKFreq_Value=275000000 +RCC.HPRE=RCC_HCLK_DIV2 +RCC.I2C123Freq_Value=137500000 +RCC.I2C4Freq_Value=137500000 +RCC.IPParameters=ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVN1,DIVN2,DIVN3,DIVP1,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,I2C123Freq_Value,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL1_VCI_Range-AdvancedSettings,PLL2FRACN,PLL2_VCO_SEL-AdvancedSettings,PLL3FRACN,PLLFRACN,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123CLockSelection,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,SupplySource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.LPTIM1Freq_Value=137500000 +RCC.LPTIM2Freq_Value=137500000 +RCC.LPTIM345Freq_Value=137500000 +RCC.LPUART1Freq_Value=137500000 +RCC.LTDCFreq_Value=250000000 +RCC.MCO1PinFreq_Value=64000000 +RCC.MCO2PinFreq_Value=550000000 +RCC.PLL1_VCI_Range-AdvancedSettings=RCC_PLL1VCIRANGE_1 +RCC.PLL2FRACN=0 +RCC.PLL2_VCO_SEL-AdvancedSettings=RCC_PLL2VCOWIDE +RCC.PLL3FRACN=0 +RCC.PLLFRACN=0 +RCC.QSPIFreq_Value=275000000 +RCC.RNGFreq_Value=48000000 +RCC.RTCFreq_Value=32000 +RCC.SAI1Freq_Value=275000000 +RCC.SAI4AFreq_Value=275000000 +RCC.SAI4BFreq_Value=275000000 +RCC.SDMMCFreq_Value=275000000 +RCC.SPDIFRXFreq_Value=275000000 +RCC.SPI123CLockSelection=RCC_SPI123CLKSOURCE_PLL3 +RCC.SPI123Freq_Value=125000000 +RCC.SPI45Freq_Value=137500000 +RCC.SPI6Freq_Value=137500000 +RCC.SWPMI1Freq_Value=137500000 +RCC.SYSCLKFreq_VALUE=550000000 +RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.SupplySource=PWR_DIRECT_SMPS_SUPPLY +RCC.Tim1OutputFreq_Value=275000000 +RCC.Tim2OutputFreq_Value=275000000 +RCC.TraceFreq_Value=275000000 +RCC.USART16Freq_Value=137500000 +RCC.USART234578Freq_Value=137500000 +RCC.USBFreq_Value=275000000 +RCC.VCO1OutputFreq_Value=550000000 +RCC.VCO2OutputFreq_Value=320000000 +RCC.VCO3OutputFreq_Value=250000000 +RCC.VCOInput1Freq_Value=16000000 +RCC.VCOInput2Freq_Value=1015873.0158730159 +RCC.VCOInput3Freq_Value=2000000 +SH.S_TIM15_CH1.0=TIM15_CH1,PWM Generation1 CH1 +SH.S_TIM15_CH1.ConfNb=1 +SH.S_TIM5_CH1.0=TIM5_CH1,Encoder_Interface +SH.S_TIM5_CH1.ConfNb=1 +SH.S_TIM5_CH2.0=TIM5_CH2,Encoder_Interface +SH.S_TIM5_CH2.ConfNb=1 +SH.S_TIM8_CH1.0=TIM8_CH1,PWM Generation1 CH1 +SH.S_TIM8_CH1.ConfNb=1 +SPI3.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_32 +SPI3.CLKPhase=SPI_PHASE_2EDGE +SPI3.CLKPolarity=SPI_POLARITY_HIGH +SPI3.CalculateBaudRate=3.90625 MBits/s +SPI3.DataSize=SPI_DATASIZE_8BIT +SPI3.Direction=SPI_DIRECTION_2LINES +SPI3.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,CLKPolarity,CLKPhase,BaudRatePrescaler,MasterKeepIOState +SPI3.MasterKeepIOState=SPI_MASTER_KEEP_IO_STATE_ENABLE +SPI3.Mode=SPI_MODE_MASTER +SPI3.VirtualType=VM_MASTER +TIM15.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM15.IPParameters=Channel-PWM Generation1 CH1,Prescaler,Period +TIM15.Period=999 +TIM15.Prescaler=199 +TIM2.IPParameters=Prescaler,Period +TIM2.Period=4294967295 +TIM2.Prescaler=0 +TIM5.EncoderMode=TIM_ENCODERMODE_TI12 +TIM5.IPParameters=EncoderMode +TIM8.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM8.IPParameters=Channel-PWM Generation1 CH1,Prescaler,Period +TIM8.Period=249 +TIM8.Prescaler=0 +VP_ADC3_Vbat_Input.Mode=IN-Vbat +VP_ADC3_Vbat_Input.Signal=ADC3_Vbat_Input +VP_ADC3_Vref_Input.Mode=IN-Vrefint +VP_ADC3_Vref_Input.Signal=ADC3_Vref_Input +VP_CRC_VS_CRC.Mode=CRC_Activate +VP_CRC_VS_CRC.Signal=CRC_VS_CRC +VP_FMAC_VS_FMAC.Mode=FMAC_Activate +VP_FMAC_VS_FMAC.Signal=FMAC_VS_FMAC +VP_IWDG1_VS_IWDG.Mode=IWDG_Activate +VP_IWDG1_VS_IWDG.Signal=IWDG1_VS_IWDG +VP_MEMORYMAP_VS_MEMORYMAP.Mode=CurAppReg +VP_MEMORYMAP_VS_MEMORYMAP.Signal=MEMORYMAP_VS_MEMORYMAP +VP_SYS_VS_Systick.Mode=SysTick +VP_SYS_VS_Systick.Signal=SYS_VS_Systick +VP_TIM2_VS_ClockSourceINT.Mode=Internal +VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT +board=custom diff --git a/tests/hal/test_mcu.cpp b/tests/hal/test_mcu.cpp index 48b9d1e..6cb785d 100644 --- a/tests/hal/test_mcu.cpp +++ b/tests/hal/test_mcu.cpp @@ -3,7 +3,6 @@ */ #include -#include #include @@ -12,10 +11,11 @@ #include "micras/hal/host/board.hpp" #include "micras/hal/mcu.hpp" #include "micras/hal/pwm.hpp" +#include "tim.h" +#include "usart.h" extern "C" { void SystemClock_Config(); -void PeriphCommonClock_Config(); } namespace micras::test { @@ -25,7 +25,7 @@ TEST_SUITE("mcu") { hal::Mcu::init({ .clock_init = SystemClock_Config, - .peripheral_clock_init = PeriphCommonClock_Config, + .peripheral_clock_init = nullptr, .peripheral_inits = inits, .cpu_frequency_boost = false, }); diff --git a/tests/support/host_board/handles.cpp b/tests/support/host_board/handles.cpp index aedcf0e..890f2e1 100644 --- a/tests/support/host_board/handles.cpp +++ b/tests/support/host_board/handles.cpp @@ -27,10 +27,11 @@ TIM_TypeDef tim5_registers{}; TIM_TypeDef tim8_registers{}; TIM_TypeDef tim15_registers{}; SPI_TypeDef spi3_registers{}; +} // namespace // NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) -void init_timer( +static void init_timer( TIM_HandleTypeDef& handle, TIM_TypeDef& registers, const char* name, uint32_t prescaler, uint32_t counter_mode, uint32_t period ) { @@ -40,13 +41,13 @@ void init_timer( handle.State = HAL_TIM_STATE_READY; micras::hal::host::Board::name_handle(&handle, name); } -} // namespace // NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) uint32_t SystemCoreClock{400000000}; GPIO_TypeDef GPIOA_instance{'A'}; GPIO_TypeDef GPIOB_instance{'B'}; +GPIO_TypeDef GPIOC_instance{'C'}; GPIO_TypeDef GPIOD_instance{'D'}; ADC_HandleTypeDef hadc3{}; @@ -63,14 +64,16 @@ UART_HandleTypeDef huart4{}; extern "C" { void SystemClock_Config() { } - -void PeriphCommonClock_Config() { } } void MX_GPIO_Init() { using micras::hal::host::Board; + Board::name_gpio(Encoder_A_GPIO_Port, Encoder_A_Pin, "Encoder_A"); + Board::name_gpio(Encoder_B_GPIO_Port, Encoder_B_Pin, "Encoder_B"); + Board::name_gpio(PWM_GPIO_Port, PWM_Pin, "PWM"); Board::name_gpio(Button_GPIO_Port, Button_Pin, "Button"); Board::name_gpio(Status_LED_GPIO_Port, Status_LED_Pin, "Status_LED"); + Board::name_gpio(PWM_DMA_GPIO_Port, PWM_DMA_Pin, "PWM_DMA"); Board::name_gpio(SPI_CSn_GPIO_Port, SPI_CSn_Pin, "SPI_CSn"); } diff --git a/tests/support/host_board/main.h b/tests/support/host_board/main.h index a991676..e6336c8 100644 --- a/tests/support/host_board/main.h +++ b/tests/support/host_board/main.h @@ -5,8 +5,10 @@ * * @note The host counterpart of reference/h7: the same peripherals under the * same handle and pin names, one instance each, with the values its - * CubeMX project generates. The configurations the tests build from them - * compile unchanged against the generated tree of the reference. + * CubeMX project generates. The independent watchdog has no handle here, + * since the HAL drives its registers directly. The configurations the + * tests build from them compile unchanged against the generated tree of + * the reference. */ #ifndef MICRAS_LIB_TESTS_HOST_BOARD_MAIN_H @@ -41,20 +43,30 @@ inline constexpr uint32_t FLASH_SECTOR_TOTAL{8U}; ///@{ extern GPIO_TypeDef GPIOA_instance; extern GPIO_TypeDef GPIOB_instance; +extern GPIO_TypeDef GPIOC_instance; extern GPIO_TypeDef GPIOD_instance; ///@} ///@{ #define GPIOA (&GPIOA_instance) #define GPIOB (&GPIOB_instance) +#define GPIOC (&GPIOC_instance) #define GPIOD (&GPIOD_instance) ///@} ///@{ +#define Encoder_A_Pin GPIO_PIN_0 +#define Encoder_A_GPIO_Port GPIOA +#define Encoder_B_Pin GPIO_PIN_1 +#define Encoder_B_GPIO_Port GPIOA +#define PWM_Pin GPIO_PIN_2 +#define PWM_GPIO_Port GPIOA #define Button_Pin GPIO_PIN_7 #define Button_GPIO_Port GPIOA #define Status_LED_Pin GPIO_PIN_12 #define Status_LED_GPIO_Port GPIOB +#define PWM_DMA_Pin GPIO_PIN_6 +#define PWM_DMA_GPIO_Port GPIOC #define SPI_CSn_Pin GPIO_PIN_2 #define SPI_CSn_GPIO_Port GPIOD ///@} From 8145cf5d90d5067c95a058d2f590ebf824abdb09 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 06:23:50 -0300 Subject: [PATCH 539/561] =?UTF-8?q?=F0=9F=93=9D=20Document=20the=20librari?= =?UTF-8?q?es=20for=20their=20consumers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The README is the consumers' guide: how a project adds micras-lib, its options, the contracts of the stm32 and host backends, the reference project and its bench, the design of the host backend with its SPI device slot and chip models, and how to build, test, lint and document it. The Doxyfile covers the five libraries and both READMEs, and the docs target passes it the version, the theme and the output directory through the environment. --- Doxyfile | 45 ++++++++ README.md | 240 ++++++++++++++++++++++++++++++++++++++++ cmake/micras_docs.cmake | 8 +- 3 files changed, 289 insertions(+), 4 deletions(-) create mode 100644 Doxyfile create mode 100644 README.md diff --git a/Doxyfile b/Doxyfile new file mode 100644 index 0000000..c4e11a7 --- /dev/null +++ b/Doxyfile @@ -0,0 +1,45 @@ +# Doxyfile for micras-lib; built by the docs target, which fills in the $(...) variables + +PROJECT_NAME = "micras-lib" +PROJECT_NUMBER = $(MICRAS_LIB_VERSION) +PROJECT_BRIEF = "Core, navigation, communication, HAL and proxies of the Micras micromouse, for STM32 and the host" +OUTPUT_DIRECTORY = $(MICRAS_LIB_DOCS_DIR) +DISTRIBUTE_GROUP_DOC = YES +NUM_PROC_THREADS = 0 +DOT_NUM_THREADS = 0 +WARN_IF_UNDOC_ENUM_VAL = YES +EXTRACT_PRIVATE = YES +EXTRACT_PRIV_VIRTUAL = YES +EXTRACT_STATIC = YES +CASE_SENSE_NAMES = YES +SHOW_INCLUDE_FILES = NO +SHOW_HEADERFILE = NO +INPUT = README.md \ + micras_core/ \ + micras_nav/ \ + micras_comm/ \ + micras_hal/ \ + micras_proxy/ +RECURSIVE = YES +FILE_PATTERNS = *.c *.cpp *.h *.hpp *.tpp *.md +EXTENSION_MAPPING = tpp=C++ +USE_MDFILE_AS_MAINPAGE = README.md +VERBATIM_HEADERS = NO +GENERATE_HTML = YES +GENERATE_LATEX = NO +GENERATE_TREEVIEW = YES +DISABLE_INDEX = NO +FULL_SIDEBAR = NO +HTML_COLORSTYLE = AUTO_DARK +HTML_EXTRA_STYLESHEET = $(DOXYGEN_AWESOME_DIR)/doxygen-awesome.css \ + $(DOXYGEN_AWESOME_DIR)/doxygen-awesome-sidebar-only.css +UML_LOOK = YES +UML_LIMIT_NUM_FIELDS = 50 +DOT_WRAP_THRESHOLD = 100 +TEMPLATE_RELATIONS = YES +INCLUDE_GRAPH = NO +INCLUDED_BY_GRAPH = NO +CALL_GRAPH = YES +CALLER_GRAPH = YES +DOT_GRAPH_MAX_NODES = 100 +MAX_DOT_GRAPH_DEPTH = 1 diff --git a/README.md b/README.md new file mode 100644 index 0000000..40610b4 --- /dev/null +++ b/README.md @@ -0,0 +1,240 @@ +# micras-lib + +The libraries of the [Micras](https://github.com/Team-Micras/MicrasFirmware) micromouse, for any +robot that wants them: the core utilities, the navigation, the communication protocol, the hardware +abstraction layer and the proxies of the devices behind it. The HAL has one set of headers and two +backends, selected by CMake: **stm32**, for the robot, built against the files STM32CubeMX generates, +and **host**, which runs the same code on a PC for tests and simulations. + +| Library | Target | What it holds | Needs | +|---|---|---|---| +| `micras_core` | `micras::core` | math, filters, PID, COBS, the variable pool, the serializable interface | nothing | +| `micras_nav` | `micras::nav` | maze, planner, turn tables, route compiler, velocity planner, localizer, controller, mission | core | +| `micras_comm` | `micras::comm` | the framing and the session layer of the [communication protocol](micras_comm/README.md) | core | +| `micras_hal` | `micras::hal` | GPIO, PWM, DMA-fed PWM, encoder, ADC over DMA, SPI, UART over DMA, CRC, FMAC, flash, timer, MCU | the board target | +| `micras_proxy` | `micras::proxy` | IMU, rotary sensors, wall and torque sensors, motors, storage, watchdog and the rest | hal, core, ST's `lsm6dsv-pid` | +| `micras_proxy_models` | `micras::proxy_models` | LSM6DSV and AS5047U chip models (host backend only) | hal | + +## Using it + +A project adds micras-lib as a git submodule, under `external/` by convention, and adds it to its +build after defining its board target: + +```bash +git submodule add -b refactor/restructure https://github.com/Team-Micras/micras-lib.git external/micras-lib +``` + +```cmake +include(cmake/cube.cmake) # defines stm32cubemx, the board target +add_subdirectory(external/micras-lib EXCLUDE_FROM_ALL) +include(external/micras-lib/cmake/micras_warnings.cmake) + +add_executable(robot src/main.cpp) +target_link_libraries(robot PRIVATE micras::nav micras::proxy cube_app STM32_Drivers) +micras_apply_warnings(robot WERROR ${ROBOT_WERROR}) +``` + +- `EXCLUDE_FROM_ALL` builds only the libraries something links. What is fetched at configure time is + chosen by the options below: a project that needs only the navigation sets `MICRAS_LIB_HAL=OFF`, + links `micras::nav`, and builds core and nav with nothing downloaded. +- Adding micras-lib a second time, as a project that reaches it through two submodules does, is a + no-op: the first add wins. +- As a subproject micras-lib sets no language standard and no flag: the consumer's reach its + libraries by inheritance, and each library requires C++23 without extensions on its own. +- `cmake/micras_warnings.cmake` is the warning set of every Micras repository. + `micras_apply_warnings(... [WERROR ])` applies it to the consumer's own targets; the + libraries get it from `MICRAS_LIB_WARNINGS`. + +| Option | Default | Effect | +|---|---|---| +| `MICRAS_LIB_NAV`, `MICRAS_LIB_COMM` | `ON` | Add the library; core is always added | +| `MICRAS_LIB_HAL` | `ON` | Add `micras_hal` | +| `MICRAS_LIB_PROXY` | `ON` | Add `micras_proxy`, which fetches `lsm6dsv-pid`; forced off without the HAL | +| `MICRAS_LIB_HAL_BACKEND` | `stm32` | `stm32` or `host` | +| `MICRAS_LIB_STM32_FAMILY` | empty | `h7`; read from the board target's device macro when empty | +| `MICRAS_LIB_CUBE_TARGET` | `stm32cubemx` | The board target the HAL links, on every backend | +| `MICRAS_LIB_WARNINGS` | `ON` | Apply the project warnings to every library target | +| `MICRAS_LIB_WERROR` | `OFF` | Treat them as errors | +| `MICRAS_LIB_TESTS`, `MICRAS_LIB_DOCS` | on when top level | The tests (host backend, fetches doctest) and the `docs` target | + +Third-party code is fetched with FetchContent and pinned by commit: `lsm6dsv-pid` (v5.1.1) by +`micras_proxy`, doctest (v2.5.3) by the tests and doxygen-awesome-css (v2.5.0) by the docs. + +## The stm32 backend + +A consumer of the stm32 backend: + +1. Defines the board target (`stm32cubemx` by default) **before** adding micras-lib. It carries + `main.h`, the HAL and CMSIS include directories and the device macro that selects the family + (`STM32H725xx`, for instance). The vendor driver and application objects (`cube_app` and + `STM32_Drivers` in the reference project) are linked into every executable: they hold the vector + table, the interrupt handlers and the `MX_*_Init` functions. +2. Enables in `stm32xx_hal_conf.h` every module the headers name: ADC, CRC, FMAC, SPI, TIM, UART, + FLASH, DMA, RCC and GPIO. +3. Keeps `USE_HAL_ADC_REGISTER_CALLBACKS` and `USE_HAL_SPI_REGISTER_CALLBACKS` at 0. micras-lib + defines `HAL_ADC_ConvCpltCallback`, `HAL_ADC_ErrorCallback`, `HAL_SPI_TxRxCpltCallback` and + `HAL_SPI_ErrorCallback`, and the consumer must not. They win over the vendor's weak defaults because + each lives in the source of a constructor the program references (`AdcDma`, `Spi`). +4. Places every object that holds a DMA buffer in memory the DMA reaches, with the data cache off or + the buffers non-cacheable. +5. Configures the CRC unit for the AS5047U (polynomial 0x1D, 8 bits, initial value 0xC4) when it uses + `RotarySensor`. +6. Keeps code out of the flash `Storage` uses: the upper half of the sectors on the STM32H7. + +The code of the backend is in `micras_hal/stm32/src/`. Code that only one family runs goes to +`micras_hal/stm32//`, which CMake compiles for the detected family alone; the shared code +names no chip. + +### Reference project and bench + +`reference/h7/` is the smallest STM32CubeMX project that exercises everything the HAL and the proxies +use, on the STM32H725RGVx and nothing of Micras: an ADC scanning the internal reference and the +battery channel into a circular DMA buffer, the CRC unit set up for the AS5047U, the FMAC, a full +duplex SPI master with DMA and a GPIO chip select, a PWM channel, a DMA-fed PWM channel, an encoder +timer, a free-running timer, a UART with DMA both ways, the independent watchdog, one input and one +output pin, and the internal oscillator through the PLL at 400 MHz. CubeMX generates no `main()` and +no callback registration. Its `CMakeLists.txt` glues the generated tree in as a consumer does, adds +micras-lib with `MICRAS_LIB_WERROR=ON`, and builds and links `micras_lib_bench_h7`, which proves the +link-time contract above. + +The Cube files are generated, never committed: + +```bash +docker build --file .docker/Dockerfile --target generated-h7 --output type=local,dest=reference/h7 . +cmake --preset reference-h7 +cmake --build --preset reference-h7 +``` + +The bench drives no pin, so it can be flashed on any board with the part whose supply is the direct +SMPS one the project configures, as on Micras v1. It writes each result into a `volatile` global for +the debugger or STM32CubeMonitor: + +| Global | Check | +|---|---| +| `storage_passed` | a `Storage` saved before a watchdog reset is restored after it | +| `watchdog_passed` | a `Watchdog` left alone resets the chip, and the reset cause reads back | +| `timer_passed`, `timer_clock_error` | the free-running timer against the cycle counter, for the timer clock the RCC reports | +| `crc_passed`, `crc_value` | the CRC unit against the same CRC computed in software | +| `fmac_passed`, `fmac_error` | `FmacFilter` against the same filter computed in software | +| `adc_passed`, `vdda_mv` | the internal reference converted over DMA, against its factory calibration | +| `bench_done` | every check ran | + +The pin-driving classes (`Gpio`, `Pwm`, `PwmDma`, `Encoder`, `Spi`, `UartDma`) are compiled and linked, +but started only when the debugger sets `start_pin_drivers` before the checks end. + +## The host backend + +A consumer of the host backend: + +1. Defines its board target, a static library standing in for the Cube layer, before adding + micras-lib, and sets `MICRAS_LIB_CUBE_TARGET` to it. It provides `main.h` (which includes + `stm32_host.h`), the per-peripheral headers its configuration includes, `SystemCoreClock`, the + flash geometry macros (`FLASH_NB_32BITWORD_IN_FLASHWORD`, `FLASH_SECTOR_SIZE`, + `FLASH_SECTOR_TOTAL`), and the handles and `MX_*_Init` functions that fill the registers the host + `Pwm`, `Timer` and `Spi` read. It links `micras::hal`, and `micras_hal` links it back. +2. Keeps exceptions on if a thread of its own runs the program the HAL serves. +3. Resets `micras::hal::host::Board` and `micras::hal::host::Clock` between independent runs in one + process. + +`tests/support/host_board/` is the board of micras-lib's own tests: the peripherals of the reference +project under the same handle and pin names, with the values its CubeMX project generates. + +### Design + +The host backend implements every `micras_hal` class on a PC, private and static data members +included, and includes nothing but the HAL's headers and its own. A change to a shared header lands +with both implementations in one commit. + +- **Ports.** `Board` is a registry of ports (`GpioPort`, `PwmPort`, `PwmDmaPort`, `AdcPort`, + `UartPort`, `EncoderPort`, `SpiPort`, `FmacPort`, `FlashPort`, `McuPort`) **keyed by the address of + the Cube handle or GPIO port** that names them, so the consumer's own configuration is the key: + `.handle = &htim15` there and `Board::pwm(&htim15, TIM_CHANNEL_1)` in a binding reach the same port. + Nothing in the backend knows a physics engine; a consumer's bindings connect ports to whatever + simulates the board. A port the program touched that nothing is bound to is listed by + `Board::unbound()`, and `McuPort` counts watchdog expiries and emergency stops instead of resetting a + process. +- **Time.** `Clock` is the only place time exists. **Every read of the timer costs one microsecond + of host time**, since a read on the robot takes time too and a loop that polls the timer must see it + advance. When a read crosses the end of a step, the clock calls its handover: whatever drives the + world advances it by one step and the read returns after it. A busy wait therefore runs the world + exactly as long as it waits, and the time inside one iteration of a loop is the number of timer reads + times the quantum: deterministic, not a measurement of anything. +- **Registers.** `stm32_host.h` holds the handle types and HAL constants, with only the fields the + backend reads or writes. The host `Pwm` and `Timer` compute frequencies and duty cycles from the + prescaler, autoreload and counter mode the board's init functions write, with the timer's kernel + clock in its `Instance`. +- **FMAC.** `Fmac` keeps the configured IIR filter in the accelerator's port and computes it in q1.15, + with exact products and the output clipped to the q1.15 range. +- **Flash.** `FlashPort` holds the reserved region as bytes, erased to 0xFF and written once per + erase, and its `write_budget` cuts writes off after a number of flash words, as a power loss would. + +### The SPI device slot + +A chip on an SPI bus is a `SpiDevice` (`micras/hal/host/spi_device.hpp`): `select()`, +`exchange(tx, rx)`, `deselect()` and the SPI mode it answers in. A binding attaches it with +`Board::spi_device(handle, cs_port, cs_pin, device)`, **keyed by the bus and the chip select**, since +one bus carries several chips. The host `Spi` selects the device in `select_device`, routes every +`transmit`, `receive` and `transmit_receive` to it, and deselects it in `unselect_device`, so a register +read the driver makes of two HAL calls is one transaction for the chip, as on the bus. + +- **The mode is checked on every transfer.** A device whose mode differs from the one `select_device` + wrote into the handle is not reached and the program reads all ones, as from a chip clocked on the + wrong edge. So is a chip select with no device, which is also an unbound port. +- **`start_transfer` completes on the host clock.** The bytes are exchanged at the start, and the + transfer ends when the host clock passes the time its bytes take at the bus's bit rate: the kernel + clock the board writes into the handle's `Instance`, divided by the baud rate prescaler. + `get_transfer`, and a `select_device` that finds the bus busy, end it through `on_transfer_end`, + which raises the chip select and sets `COMPLETE`, as the DMA interrupt does. +- **Blocking transfers take no time.** Only timer reads cost time on the host. +- **Devices outlive the drivers that talk to them.** A driver ends its last transfer when it is + destroyed; forgetting the ports with `Board::reset()` detaches the devices first. + +### The chip models + +`micras_proxy/models/` builds `micras_proxy_models` (namespace `micras::models`): SPI devices that +know the slot and nothing else, so the real `Imu` and `RotarySensor` proxies run on the host. + +- **`Lsm6dsvModel`**: the 128 registers of the main page with the datasheet defaults (WHO_AM_I 0x70), + `SW_POR` and `SW_RESET`, auto-increment under `IF_INC`, SPI mode 3. `push_sample(angular_velocity, + linear_acceleration)`, in rad/s and m/s^2 at the configured rate, encodes a sample with the full + scale in CTRL6/CTRL8, the one the driver wrote, and sets `GDA`/`XLDA`; reading a sensor's output + high byte clears its bit. A sensor whose output data rate is off ignores samples. +- **`As5047uModel`**: 24-bit frames with the CRC-8 (polynomial 0x1D, initial value 0xC4, final XOR + 0xFF) checked on every frame, answers pipelined by one frame, the volatile registers (DISABLE, + ZPOSM, ZPOSL, SETTINGS1 to 3, ECC), ERRFL with the CRC and framing error bits, SPI mode 1. The + position is not modeled: it reaches the program through the timer encoder, as on the robot. + +## Developing + +| Preset | What | +|---|---| +| `host` | Debug, the host backend and the tests under the address and undefined behavior sanitizers | +| `host-release` | Release, the host backend and the tests | +| `reference-h7` | The reference STM32H725 project and its bench, with the ARM toolchain | + +```bash +cmake --preset host +cmake --build --preset host +ctest --preset host +cmake --build --preset host --target format_check lint docs +``` + +The host presets use GCC 15 (`gcc-15`, `g++-15`) and Ninja. The tests are doctest suites, one +executable per folder of `tests/` (`core`, `comm`, `hal`, `proxy`, `models`, `nav`), each case +registered in CTest. The nav suite plans on the ten contest mazes of `tests/data/mazes/` with golden +route times and step counts, which an intended change of the planner regenerates. + +`format`, `format_check`, `lint` and `lint_fix` use clang 22, found as `clang-format-22`, +`clang-tidy-22`, `run-clang-tidy-22` and `clang-apply-replacements-22`; configuring the top level +fails without them. `lint` covers the host build; `cmake --build --preset reference-h7 --target lint` +covers the stm32 backend and the bench against the ARM headers. `docs` builds the Doxygen pages into +`build//docs/`. + +`.docker/Dockerfile` has the image every CI job runs in (the `host` stage, shared byte for byte with +micras-simulation and MicrasFirmware), the CubeMX generation of the reference project, and the ARM +toolchain on top of the host stage (`stm32`). `.github/workflows/ci.yaml` builds, tests, checks the +format, lints and builds the docs on the host, and builds and lints the H7 reference and its bench. + +The code style is the one of every Micras repository: `.clang-format`, `.clang-tidy` (with +`tests/.clang-tidy`), Doxygen on every declaration of a header, no comments in sources, American +English, and gitmoji commit messages with one imperative sentence. diff --git a/cmake/micras_docs.cmake b/cmake/micras_docs.cmake index bc2616d..1ee8c7d 100644 --- a/cmake/micras_docs.cmake +++ b/cmake/micras_docs.cmake @@ -19,13 +19,13 @@ FetchContent_Declare( FetchContent_MakeAvailable(doxygen-awesome-css) -# MICRAS_LIB_VERSION is resolved by Doxygen through $(MICRAS_LIB_VERSION) in the Doxyfile. -# Backticks are used instead of $() so that the make generator does not expand it first. +# The Doxyfile reads the version, the theme and the output directory from the environment add_custom_target(docs - COMMAND MICRAS_LIB_VERSION=`git describe --always --dirty --tags 2>/dev/null || echo unknown` + COMMAND ${CMAKE_COMMAND} -E env DOXYGEN_AWESOME_DIR=${doxygen-awesome-css_SOURCE_DIR} MICRAS_LIB_DOCS_DIR=${CMAKE_BINARY_DIR}/docs - ${MICRAS_DOXYGEN} Doxyfile + sh -c "MICRAS_LIB_VERSION=\"$(git describe --always --dirty --tags 2>/dev/null || echo unknown)\" exec ${MICRAS_DOXYGEN} Doxyfile" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} COMMENT "Generating the HTML documentation in ${CMAKE_BINARY_DIR}/docs" + VERBATIM ) From a2a520360dae7c76e61d7466ab0261fc25afa173 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 06:23:50 -0300 Subject: [PATCH 540/561] =?UTF-8?q?=F0=9F=91=B7=20Check=20every=20push=20i?= =?UTF-8?q?n=20the=20host=20and=20STM32H7=20jobs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The host job builds the host preset with warnings as errors in the shared host image, runs the tests under the sanitizers, checks the formatting, lints and builds the docs, with the fetched dependencies cached. The stm32-h7 job generates the reference project's Cube files once per .ioc and Dockerfile, builds the reference and links its bench, and lints the stm32 backend. Both run on a push to any branch and on pull requests, so the restructure branch is checked before anything is merged. --- .github/workflows/ci.yaml | 109 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 .github/workflows/ci.yaml diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 0000000..cbf5c98 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,109 @@ +name: Build test lint and check formatting + +on: + push: + pull_request: + workflow_dispatch: + +permissions: + contents: read + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +jobs: + host: + name: ๐Ÿงช Host build, tests, format, lint and docs + runs-on: ubuntu-latest + timeout-minutes: 45 + steps: + - name: ๐Ÿ”€ Checkout + uses: actions/checkout@v7 + + - name: ๐Ÿ‹ Build the host image + run: docker build --file .docker/Dockerfile --target host --tag micras-lib:host . + + - name: ๐Ÿ—ƒ๏ธ Restore the fetched dependencies + uses: actions/cache@v6 + with: + path: build/host/_deps + key: deps-host-${{ hashFiles('micras_proxy/CMakeLists.txt', 'tests/CMakeLists.txt', 'cmake/micras_docs.cmake') }} + + # One long lived container runs every step below, as the user that owns the checkout + - name: ๐Ÿ“ฆ Start the host container + run: > + docker run --detach --name host --user "$(id -u):$(id -g)" --env HOME=/tmp + --volume "$PWD:/project" --workdir /project micras-lib:host sleep infinity + + - name: โš™๏ธ Configure + run: docker exec host cmake --preset host -DMICRAS_LIB_WERROR=ON + + - name: ๐Ÿ”จ Build + run: docker exec host cmake --build --preset host + + - name: ๐Ÿงช Run the tests + run: docker exec host ctest --preset host + + - name: ๐ŸŽจ Check the formatting + run: docker exec host cmake --build --preset host --target format_check + + - name: ๐Ÿšจ Lint + run: docker exec host cmake --build --preset host --target lint + + - name: ๐Ÿ“ Build the documentation + run: docker exec host cmake --build --preset host --target docs + + - name: ๐Ÿงน Stop the host container + if: always() + run: docker rm --force host + + stm32-h7: + name: ๐Ÿ”ฉ STM32H7 reference and bench + runs-on: ubuntu-latest + timeout-minutes: 45 + steps: + - name: ๐Ÿ”€ Checkout + uses: actions/checkout@v7 + + - name: ๐Ÿ—ƒ๏ธ Restore the generated Cube files + id: cube + uses: actions/cache/restore@v6 + with: + path: build/generated-h7 + key: cube-h7-${{ hashFiles('reference/h7/reference.ioc', '.docker/Dockerfile') }} + + - name: โš™๏ธ Generate the Cube files + if: steps.cube.outputs.cache-hit != 'true' + run: docker build --file .docker/Dockerfile --target generated-h7 --output type=local,dest=build/generated-h7 . + + - name: ๐Ÿ—ƒ๏ธ Save the generated Cube files + if: steps.cube.outputs.cache-hit != 'true' + uses: actions/cache/save@v6 + with: + path: build/generated-h7 + key: ${{ steps.cube.outputs.cache-primary-key }} + + - name: ๐Ÿ“‚ Place the Cube files in the reference project + run: cp -r build/generated-h7/. reference/h7/ + + - name: ๐Ÿ‹ Build the ARM image + run: docker build --file .docker/Dockerfile --target stm32 --tag micras-lib:stm32 . + + - name: ๐Ÿ“ฆ Start the ARM container + run: > + docker run --detach --name stm32 --user "$(id -u):$(id -g)" --env HOME=/tmp + --volume "$PWD:/project" --workdir /project micras-lib:stm32 sleep infinity + + - name: โš™๏ธ Configure the reference project + run: docker exec stm32 cmake --preset reference-h7 + + - name: ๐Ÿ”จ Build the reference and its bench + run: docker exec stm32 cmake --build --preset reference-h7 --parallel + + - name: ๐Ÿšจ Lint the stm32 backend and the bench + run: docker exec stm32 cmake --build --preset reference-h7 --target lint + + - name: ๐Ÿงน Stop the ARM container + if: always() + run: docker rm --force stm32 From b75c6966c60bd2074b4628791b8e01c51b0744c6 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 06:34:17 -0300 Subject: [PATCH 541/561] =?UTF-8?q?=F0=9F=94=A7=20Store=20every=20text=20f?= =?UTF-8?q?ile=20with=20LF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit STM32CubeMX writes the reference projects' .ioc files with the line endings of the platform it runs on, so without normalization a regeneration on Windows would show as a rewrite of the whole file. --- .gitattributes | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 .gitattributes diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..ce5f8a7 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,17 @@ +# Every text file is stored with LF in the repository and checked out with LF, whatever platform +# wrote it. +# +# The file this exists for is reference/*/reference.ioc. STM32CubeMX writes its project file with the line endings +# of the platform it runs on, CRLF from a Windows install and LF from the Linux container the CI +# uses, so without normalisation the same regeneration shows up as a whole file rewrite depending on +# who ran it, burying the handful of settings that actually changed. +* text=auto eol=lf + +# Binary types, so that git never rewrites them +*.png binary +*.jpg binary +*.pdf binary +*.ico binary +*.bin binary +*.elf binary +*.hex binary From 7d3d42bae3de781eee9b1b85f1e96bcde54f4636 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 06:34:38 -0300 Subject: [PATCH 542/561] =?UTF-8?q?=E2=9C=8F=EF=B8=8F=20Spell=20normalizat?= =?UTF-8?q?ion=20the=20American=20way=20in=20the=20attributes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitattributes | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.gitattributes b/.gitattributes index ce5f8a7..19901b0 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,10 +1,10 @@ # Every text file is stored with LF in the repository and checked out with LF, whatever platform # wrote it. # -# The file this exists for is reference/*/reference.ioc. STM32CubeMX writes its project file with the line endings -# of the platform it runs on, CRLF from a Windows install and LF from the Linux container the CI -# uses, so without normalisation the same regeneration shows up as a whole file rewrite depending on -# who ran it, burying the handful of settings that actually changed. +# The files this exists for are reference/*/reference.ioc. STM32CubeMX writes its project file with +# the line endings of the platform it runs on, CRLF from a Windows install and LF from the Linux +# container the CI uses, so without normalization the same regeneration shows up as a whole file +# rewrite depending on who ran it, burying the handful of settings that actually changed. * text=auto eol=lf # Binary types, so that git never rewrites them From 2ca7a5f6b6870106a2852f37295726df830c7356 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 06:42:30 -0300 Subject: [PATCH 543/561] =?UTF-8?q?=F0=9F=91=B7=20Cache=20the=20fetch=20st?= =?UTF-8?q?amps=20with=20the=20fetched=20sources?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit FetchContent keeps the record of what it fetched under CMakeFiles/fc-stamp, and without it a restored _deps is cloned again. --- .github/workflows/ci.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index cbf5c98..c1bfcb2 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -27,7 +27,9 @@ jobs: - name: ๐Ÿ—ƒ๏ธ Restore the fetched dependencies uses: actions/cache@v6 with: - path: build/host/_deps + path: | + build/host/_deps + build/host/CMakeFiles/fc-stamp key: deps-host-${{ hashFiles('micras_proxy/CMakeLists.txt', 'tests/CMakeLists.txt', 'cmake/micras_docs.cmake') }} # One long lived container runs every step below, as the user that owns the checkout From af794c0359d7eedfd68b6f0711aa5a5d74e473e8 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 07:26:17 -0300 Subject: [PATCH 544/561] =?UTF-8?q?=F0=9F=93=9D=20Name=20the=20HAL=20callb?= =?UTF-8?q?acks=20AdcDma=20and=20Spi=20define=20for=20their=20consumers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/include/micras/hal/adc_dma.hpp | 5 +++++ micras_hal/include/micras/hal/spi.hpp | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 1428455..22b1a30 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -13,6 +13,11 @@ namespace micras::hal { /** * @brief Class to handle ADC peripheral on STM32 microcontrollers using DMA. + * + * @note The stm32 backend defines HAL_ADC_ConvCpltCallback and HAL_ADC_ErrorCallback, so a consumer + * must not define them, and must keep USE_HAL_ADC_REGISTER_CALLBACKS at 0. They win over the vendor's + * weak defaults because they sit in the source of this class's constructor, which every program that + * uses the class references. */ class AdcDma { public: diff --git a/micras_hal/include/micras/hal/spi.hpp b/micras_hal/include/micras/hal/spi.hpp index 93941d0..ce357ad 100644 --- a/micras_hal/include/micras/hal/spi.hpp +++ b/micras_hal/include/micras/hal/spi.hpp @@ -15,6 +15,11 @@ namespace micras::hal { /** * @brief Class to handle SPI peripheral on STM32 microcontrollers. + * + * @note The stm32 backend defines HAL_SPI_TxRxCpltCallback and HAL_SPI_ErrorCallback, so a consumer + * must not define them, and must keep USE_HAL_SPI_REGISTER_CALLBACKS at 0. They win over the vendor's + * weak defaults because they sit in the source of this class's constructor, which every program that + * uses the class references. */ class Spi { public: From 1224489fcfbcd9bb371514b5918d65c220f0813d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 07:26:17 -0300 Subject: [PATCH 545/561] =?UTF-8?q?=F0=9F=94=A7=20Store=20the=20reference'?= =?UTF-8?q?s=20derived=20clock=20frequencies=20for=20its=20400=20MHz=20PLL?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The frequency fields of reference.ioc are the values STM32CubeMX 6.18.1 computes from the clock tree the file configures (HSI / 4 x 50 / 2), as it saves them. They are cached values only: the generated project is the same. --- reference/h7/reference.ioc | 86 +++++++++++++++++++------------------- 1 file changed, 43 insertions(+), 43 deletions(-) diff --git a/reference/h7/reference.ioc b/reference/h7/reference.ioc index b55858d..4e6f42f 100644 --- a/reference/h7/reference.ioc +++ b/reference/h7/reference.ioc @@ -358,87 +358,87 @@ ProjectManager.UAScriptBeforePath= ProjectManager.UnderRoot=false ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_SPI3_Init-SPI3-false-HAL-true,5-MX_TIM2_Init-TIM2-false-HAL-true,6-MX_TIM5_Init-TIM5-false-HAL-true,7-MX_UART4_Init-UART4-false-HAL-true,8-MX_TIM8_Init-TIM8-false-HAL-true,9-MX_TIM15_Init-TIM15-false-HAL-true,10-MX_ADC3_Init-ADC3-false-HAL-true,11-MX_CRC_Init-CRC-false-HAL-true,12-MX_FMAC_Init-FMAC-false-HAL-true,13-MX_IWDG1_Init-IWDG1-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true RCC.ADCFreq_Value=160000000 -RCC.AHB12Freq_Value=275000000 -RCC.AHB4Freq_Value=275000000 -RCC.APB1Freq_Value=137500000 -RCC.APB2Freq_Value=137500000 -RCC.APB3Freq_Value=137500000 -RCC.APB4Freq_Value=137500000 -RCC.AXIClockFreq_Value=275000000 +RCC.AHB12Freq_Value=200000000 +RCC.AHB4Freq_Value=200000000 +RCC.APB1Freq_Value=100000000 +RCC.APB2Freq_Value=100000000 +RCC.APB3Freq_Value=100000000 +RCC.APB4Freq_Value=100000000 +RCC.AXIClockFreq_Value=200000000 RCC.CECFreq_Value=32000 RCC.CKPERFreq_Value=64000000 -RCC.CortexFreq_Value=550000000 -RCC.CpuClockFreq_Value=550000000 -RCC.D1CPREFreq_Value=550000000 +RCC.CortexFreq_Value=400000000 +RCC.CpuClockFreq_Value=400000000 +RCC.D1CPREFreq_Value=400000000 RCC.D1PPRE=RCC_APB3_DIV2 RCC.D2PPRE1=RCC_APB1_DIV2 RCC.D2PPRE2=RCC_APB2_DIV2 RCC.D3PPRE=RCC_APB4_DIV2 -RCC.DFSDMACLkFreq_Value=275000000 -RCC.DFSDMFreq_Value=137500000 +RCC.DFSDMACLkFreq_Value=400000000 +RCC.DFSDMFreq_Value=100000000 RCC.DIVM1=4 RCC.DIVM2=63 RCC.DIVN1=50 RCC.DIVN2=315 RCC.DIVN3=125 RCC.DIVP1=2 -RCC.DIVP1Freq_Value=550000000 +RCC.DIVP1Freq_Value=400000000 RCC.DIVP2Freq_Value=160000000 RCC.DIVP3=2 RCC.DIVP3Freq_Value=125000000 -RCC.DIVQ1Freq_Value=275000000 +RCC.DIVQ1Freq_Value=400000000 RCC.DIVQ2=1 RCC.DIVQ2Freq_Value=320000000 RCC.DIVQ3=1 RCC.DIVQ3Freq_Value=250000000 -RCC.DIVR1Freq_Value=275000000 +RCC.DIVR1Freq_Value=400000000 RCC.DIVR2Freq_Value=160000000 RCC.DIVR3=1 RCC.DIVR3Freq_Value=250000000 -RCC.FDCANFreq_Value=275000000 -RCC.FMCFreq_Value=275000000 +RCC.FDCANFreq_Value=400000000 +RCC.FMCFreq_Value=200000000 RCC.FamilyName=M -RCC.HCLK3ClockFreq_Value=275000000 -RCC.HCLKFreq_Value=275000000 +RCC.HCLK3ClockFreq_Value=200000000 +RCC.HCLKFreq_Value=200000000 RCC.HPRE=RCC_HCLK_DIV2 -RCC.I2C123Freq_Value=137500000 -RCC.I2C4Freq_Value=137500000 +RCC.I2C123Freq_Value=100000000 +RCC.I2C4Freq_Value=100000000 RCC.IPParameters=ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVN1,DIVN2,DIVN3,DIVP1,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,I2C123Freq_Value,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL1_VCI_Range-AdvancedSettings,PLL2FRACN,PLL2_VCO_SEL-AdvancedSettings,PLL3FRACN,PLLFRACN,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123CLockSelection,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,SupplySource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value -RCC.LPTIM1Freq_Value=137500000 -RCC.LPTIM2Freq_Value=137500000 -RCC.LPTIM345Freq_Value=137500000 -RCC.LPUART1Freq_Value=137500000 +RCC.LPTIM1Freq_Value=100000000 +RCC.LPTIM2Freq_Value=100000000 +RCC.LPTIM345Freq_Value=100000000 +RCC.LPUART1Freq_Value=100000000 RCC.LTDCFreq_Value=250000000 RCC.MCO1PinFreq_Value=64000000 -RCC.MCO2PinFreq_Value=550000000 +RCC.MCO2PinFreq_Value=400000000 RCC.PLL1_VCI_Range-AdvancedSettings=RCC_PLL1VCIRANGE_1 RCC.PLL2FRACN=0 RCC.PLL2_VCO_SEL-AdvancedSettings=RCC_PLL2VCOWIDE RCC.PLL3FRACN=0 RCC.PLLFRACN=0 -RCC.QSPIFreq_Value=275000000 +RCC.QSPIFreq_Value=200000000 RCC.RNGFreq_Value=48000000 RCC.RTCFreq_Value=32000 -RCC.SAI1Freq_Value=275000000 -RCC.SAI4AFreq_Value=275000000 -RCC.SAI4BFreq_Value=275000000 -RCC.SDMMCFreq_Value=275000000 -RCC.SPDIFRXFreq_Value=275000000 +RCC.SAI1Freq_Value=400000000 +RCC.SAI4AFreq_Value=400000000 +RCC.SAI4BFreq_Value=400000000 +RCC.SDMMCFreq_Value=400000000 +RCC.SPDIFRXFreq_Value=400000000 RCC.SPI123CLockSelection=RCC_SPI123CLKSOURCE_PLL3 RCC.SPI123Freq_Value=125000000 -RCC.SPI45Freq_Value=137500000 -RCC.SPI6Freq_Value=137500000 -RCC.SWPMI1Freq_Value=137500000 -RCC.SYSCLKFreq_VALUE=550000000 +RCC.SPI45Freq_Value=100000000 +RCC.SPI6Freq_Value=100000000 +RCC.SWPMI1Freq_Value=100000000 +RCC.SYSCLKFreq_VALUE=400000000 RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK RCC.SupplySource=PWR_DIRECT_SMPS_SUPPLY -RCC.Tim1OutputFreq_Value=275000000 -RCC.Tim2OutputFreq_Value=275000000 -RCC.TraceFreq_Value=275000000 -RCC.USART16Freq_Value=137500000 -RCC.USART234578Freq_Value=137500000 -RCC.USBFreq_Value=275000000 -RCC.VCO1OutputFreq_Value=550000000 +RCC.Tim1OutputFreq_Value=200000000 +RCC.Tim2OutputFreq_Value=200000000 +RCC.TraceFreq_Value=400000000 +RCC.USART16Freq_Value=100000000 +RCC.USART234578Freq_Value=100000000 +RCC.USBFreq_Value=400000000 +RCC.VCO1OutputFreq_Value=800000000 RCC.VCO2OutputFreq_Value=320000000 RCC.VCO3OutputFreq_Value=250000000 RCC.VCOInput1Freq_Value=16000000 From 978476bcf5024ac4bd4da86655efebce0b1e0bb2 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 13:24:25 -0300 Subject: [PATCH 546/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Move=20the=20STM32?= =?UTF-8?q?H7=20steps=20of=20the=20stm32=20backend=20behind=20family=20fun?= =?UTF-8?q?ctions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The shared stm32 sources name no chip any more. The ADC calibration, the cache, the watchdog reset flag, debug freeze and registers, the core frequency boost, the timer clock and the cycle counter are the functions of micras/hal/family.hpp, and the flash driver and its geometry are in stm32/h7, which CMake compiles for an STM32H7 board target, with the H7 behavior unchanged. The host backend implements the same functions and reads its flash geometry from the board's macros, as before. --- .../host/include/micras/hal/family/flash.hpp | 39 ++++++++++ micras_hal/host/include/stm32_host.h | 18 +++++ micras_hal/host/src/family.cpp | 37 ++++++++++ micras_hal/include/micras/hal/family.hpp | 74 +++++++++++++++++++ micras_hal/include/micras/hal/flash.hpp | 27 ++++--- micras_hal/stm32/h7/README.md | 15 +++- .../h7/include/micras/hal/family/flash.hpp | 38 ++++++++++ micras_hal/stm32/h7/src/family.cpp | 66 +++++++++++++++++ micras_hal/stm32/{ => h7}/src/flash.cpp | 5 +- micras_hal/stm32/src/adc_dma.cpp | 11 +-- micras_hal/stm32/src/mcu.cpp | 56 ++++---------- micras_hal/stm32/src/pwm.cpp | 28 +------ micras_hal/stm32/src/timer.cpp | 15 +--- tests/hal/test_family.cpp | 48 ++++++++++++ 14 files changed, 370 insertions(+), 107 deletions(-) create mode 100644 micras_hal/host/include/micras/hal/family/flash.hpp create mode 100644 micras_hal/host/src/family.cpp create mode 100644 micras_hal/include/micras/hal/family.hpp create mode 100644 micras_hal/stm32/h7/include/micras/hal/family/flash.hpp create mode 100644 micras_hal/stm32/h7/src/family.cpp rename micras_hal/stm32/{ => h7}/src/flash.cpp (96%) create mode 100644 tests/hal/test_family.cpp diff --git a/micras_hal/host/include/micras/hal/family/flash.hpp b/micras_hal/host/include/micras/hal/family/flash.hpp new file mode 100644 index 0000000..26d0068 --- /dev/null +++ b/micras_hal/host/include/micras/hal/family/flash.hpp @@ -0,0 +1,39 @@ +/** + * @file + * + * @brief Flash memory geometry of the host backend, read from the board's macros. + * + * @note The board's main.h defines FLASH_NB_32BITWORD_IN_FLASHWORD, FLASH_SECTOR_SIZE and + * FLASH_SECTOR_TOTAL with the values of the chip it stands for, and the storage region is the upper + * half of those sectors, as on the stm32 backend. + */ + +#ifndef MICRAS_HAL_FAMILY_FLASH_HPP +#define MICRAS_HAL_FAMILY_FLASH_HPP + +#include +#include + +namespace micras::hal::family { +/** + * @brief Number of bits of the smallest unit the flash memory programs. + */ +inline constexpr uint32_t flash_word_bits{FLASH_NB_32BITWORD_IN_FLASHWORD * 32U}; + +/** + * @brief Number of bytes of an erasable sector. + */ +inline constexpr uint32_t sector_size{FLASH_SECTOR_SIZE}; + +/** + * @brief Index of the first sector reserved for data storage. + */ +inline constexpr uint16_t storage_first_sector{FLASH_SECTOR_TOTAL / 2}; + +/** + * @brief Number of sectors reserved for data storage. + */ +inline constexpr uint16_t storage_sectors{FLASH_SECTOR_TOTAL / 2}; +} // namespace micras::hal::family + +#endif // MICRAS_HAL_FAMILY_FLASH_HPP diff --git a/micras_hal/host/include/stm32_host.h b/micras_hal/host/include/stm32_host.h index d944070..1069b45 100644 --- a/micras_hal/host/include/stm32_host.h +++ b/micras_hal/host/include/stm32_host.h @@ -280,6 +280,24 @@ struct CRC_HandleTypeDef { uint32_t InputDataFormat; }; +/***************************************** + * Independent watchdog + *****************************************/ + +/** + * @brief The registers of the independent watchdog. + * + * @note The host backend times the watchdog in the MCU port and never reads + * these registers; they exist so that the family functions have the + * same signatures on every backend. + */ +struct IWDG_TypeDef { + uint32_t KR; + uint32_t PR; + uint32_t RLR; + uint32_t SR; +}; + /***************************************** * FMAC *****************************************/ diff --git a/micras_hal/host/src/family.cpp b/micras_hal/host/src/family.cpp new file mode 100644 index 0000000..579644c --- /dev/null +++ b/micras_hal/host/src/family.cpp @@ -0,0 +1,37 @@ +/** + * @file + */ + +#include +#include + +#include "micras/hal/family.hpp" + +namespace micras::hal::family { +bool calibrate_adc(ADC_HandleTypeDef* /*handle*/) { + return true; +} + +void enable_caches() { } + +bool was_reset_by_watchdog() { + return false; +} + +void freeze_watchdog_in_debug() { } + +bool is_cpu_frequency_supported(bool /*boost*/) { + return true; +} + +IWDG_TypeDef* watchdog() { + static IWDG_TypeDef registers{}; + return ®isters; +} + +uint32_t timer_clock(const TIM_TypeDef* instance) { + return instance->kernel_clock; +} + +void enable_cycle_counter() { } +} // namespace micras::hal::family diff --git a/micras_hal/include/micras/hal/family.hpp b/micras_hal/include/micras/hal/family.hpp new file mode 100644 index 0000000..9a494b1 --- /dev/null +++ b/micras_hal/include/micras/hal/family.hpp @@ -0,0 +1,74 @@ +/** + * @file + * + * @brief The steps of the shared HAL classes that differ between chip families. + * + * @note Internal to micras_hal: each backend implements these functions once, the stm32 backend in the + * sources of the family CMake selects and the host backend in its own. Nothing here names a chip, so + * the shared classes call them without any conditional compilation. + */ + +#ifndef MICRAS_HAL_FAMILY_HPP +#define MICRAS_HAL_FAMILY_HPP + +#include +#include + +namespace micras::hal::family { +/** + * @brief Calibrate a converter for single-ended conversions. + * + * @param handle Handle of the converter, initialized and not enabled. + * @return True if the calibration succeeded, false otherwise. + */ +bool calibrate_adc(ADC_HandleTypeDef* handle); + +/** + * @brief Enable the caches the family runs its code from, before the vendor HAL is initialized. + */ +void enable_caches(); + +/** + * @brief Read from the reset flags whether the last reset came from the independent watchdog. + * + * @note The flags are read before they are cleared, so this is called once at startup. + * + * @return True if the independent watchdog reset the microcontroller, false otherwise. + */ +bool was_reset_by_watchdog(); + +/** + * @brief Stop the independent watchdog while the core is halted by a debugger. + */ +void freeze_watchdog_in_debug(); + +/** + * @brief Check whether the part can run at the core frequency the board asks for. + * + * @param boost Whether the board runs the core above the frequency the part allows by default. + * @return True if the part supports the requested frequency, false otherwise. + */ +bool is_cpu_frequency_supported(bool boost); + +/** + * @brief Get the registers of the independent watchdog. + * + * @return Registers of the independent watchdog. + */ +IWDG_TypeDef* watchdog(); + +/** + * @brief Get the frequency of the clock that feeds a timer. + * + * @param instance Timer peripheral. + * @return Timer clock frequency in Hz. + */ +uint32_t timer_clock(const TIM_TypeDef* instance); + +/** + * @brief Start the cycle counter of the core from zero. + */ +void enable_cycle_counter(); +} // namespace micras::hal::family + +#endif // MICRAS_HAL_FAMILY_HPP diff --git a/micras_hal/include/micras/hal/flash.hpp b/micras_hal/include/micras/hal/flash.hpp index 701bb60..736ac4c 100644 --- a/micras_hal/include/micras/hal/flash.hpp +++ b/micras_hal/include/micras/hal/flash.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include "micras/hal/family/flash.hpp" namespace micras::hal { /** @@ -20,14 +20,10 @@ namespace micras::hal { */ class FlashWord { public: -/** - * @brief Number of 32 bit words inside a flash word. - */ -#ifdef FLASH_NB_32BITWORD_IN_FLASHWORD - static constexpr uint32_t words{FLASH_NB_32BITWORD_IN_FLASHWORD}; -#else - static constexpr uint32_t words{sizeof(uint64_t) / sizeof(uint32_t)}; -#endif + /** + * @brief Number of 32 bit words inside a flash word. + */ + static constexpr uint32_t words{family::flash_word_bits / 32U}; /** * @brief Number of bytes inside a flash word. @@ -76,7 +72,9 @@ class FlashWord { * @brief Class to handle flash memory on STM32 microcontrollers. * * @note Only the region of the flash memory reserved for data storage is accessible, and every address is - * relative to the beginning of that region. + * relative to the beginning of that region. The family header micras/hal/family/flash.hpp gives the + * geometry: a sector is the unit the family erases, a 128 KB sector on the STM32H7 and a 2 KB page on + * the STM32G4. */ class Flash { public: @@ -98,12 +96,12 @@ class Flash { /** * @brief Number of bytes of an erasable sector. */ - static constexpr uint32_t sector_size{FLASH_SECTOR_SIZE}; + static constexpr uint32_t sector_size{family::sector_size}; /** * @brief Number of sectors reserved for data storage. */ - static constexpr uint16_t total_sectors{FLASH_SECTOR_TOTAL / 2}; + static constexpr uint16_t total_sectors{family::storage_sectors}; /** * @brief Number of bytes reserved for data storage. @@ -158,8 +156,9 @@ class Flash { /** * @brief Erase sectors of the flash memory. * - * @note This operation blocks the processor for around 2 s per sector, up to 4 s in the worst case, since - * the flash memory cannot be read while it is being erased. + * @note This operation blocks the processor, since the flash memory cannot be read while it is being + * erased: around 2 s per sector and up to 4 s in the worst case on the STM32H7, around 20 ms per page + * on the STM32G4. * * @param start_sector First sector to erase, counting from the first reserved sector. * @param number_of_sectors Number of sectors to erase. diff --git a/micras_hal/stm32/h7/README.md b/micras_hal/stm32/h7/README.md index 37cb2d8..cc809d4 100644 --- a/micras_hal/stm32/h7/README.md +++ b/micras_hal/stm32/h7/README.md @@ -1,6 +1,15 @@ # STM32H7 family Code of the stm32 backend that only the STM32H7 family runs, which CMake compiles when the board -target defines an STM32H7 device (or `MICRAS_LIB_STM32_FAMILY` is `h7`): `src/` holds the family's -sources and `include/micras/hal/family/` the facts its shared headers read. The code in `../src` is -the same on every family and names none. +target defines an STM32H7 device (or `MICRAS_LIB_STM32_FAMILY` is `h7`): + +- `src/family.cpp`: the functions of `micras/hal/family.hpp`. The ADC calibrates its offset for + single-ended conversions, `Mcu::init` enables the instruction cache, the watchdog is `IWDG1`, the + `CPUFREQ_BOOST` option byte of the STM32H72x/73x tells whether the core may run above its default + maximum, a timer runs at twice its APB clock when the APB prescaler (`RCC_APBx_DIV1`) is not 1, and + the cycle counter is unlocked through the lock access register of the Cortex-M7 before it starts. +- `src/flash.cpp`: the flash driver, sectors of 128 KB programmed in 256-bit flash words. +- `include/micras/hal/family/flash.hpp`: the flash geometry the shared `flash.hpp` reads. The storage + region is the upper half of bank 1: sectors 4 to 7 of an STM32H725xG. + +The code in `../src` is the same on every family and names no chip. diff --git a/micras_hal/stm32/h7/include/micras/hal/family/flash.hpp b/micras_hal/stm32/h7/include/micras/hal/family/flash.hpp new file mode 100644 index 0000000..ff95c66 --- /dev/null +++ b/micras_hal/stm32/h7/include/micras/hal/family/flash.hpp @@ -0,0 +1,38 @@ +/** + * @file + * + * @brief Flash memory geometry of the STM32H7 family. + * + * @note The flash memory is erased in sectors and programmed in flash words of 256 bits. The storage + * region is the upper half of bank 1: on the STM32H725xG, sectors 4 to 7 of 128 KB. + */ + +#ifndef MICRAS_HAL_FAMILY_FLASH_HPP +#define MICRAS_HAL_FAMILY_FLASH_HPP + +#include +#include + +namespace micras::hal::family { +/** + * @brief Number of bits of the smallest unit the flash memory programs. + */ +inline constexpr uint32_t flash_word_bits{FLASH_NB_32BITWORD_IN_FLASHWORD * 32U}; + +/** + * @brief Number of bytes of an erasable sector. + */ +inline constexpr uint32_t sector_size{FLASH_SECTOR_SIZE}; + +/** + * @brief Index of the first sector reserved for data storage. + */ +inline constexpr uint16_t storage_first_sector{FLASH_SECTOR_TOTAL / 2}; + +/** + * @brief Number of sectors reserved for data storage. + */ +inline constexpr uint16_t storage_sectors{FLASH_SECTOR_TOTAL / 2}; +} // namespace micras::hal::family + +#endif // MICRAS_HAL_FAMILY_FLASH_HPP diff --git a/micras_hal/stm32/h7/src/family.cpp b/micras_hal/stm32/h7/src/family.cpp new file mode 100644 index 0000000..8f2efd0 --- /dev/null +++ b/micras_hal/stm32/h7/src/family.cpp @@ -0,0 +1,66 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/hal/family.hpp" + +namespace micras::hal::family { +static constexpr uint32_t software_lock_key{0xC5ACCE55}; + +bool calibrate_adc(ADC_HandleTypeDef* handle) { + return HAL_ADCEx_Calibration_Start(handle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) == HAL_OK; +} + +void enable_caches() { + SCB_EnableICache(); +} + +bool was_reset_by_watchdog() { + return __HAL_RCC_GET_FLAG(RCC_FLAG_IWDG1RST) != 0; +} + +void freeze_watchdog_in_debug() { + __HAL_DBGMCU_FREEZE_IWDG1(); +} + +bool is_cpu_frequency_supported(bool boost) { +#ifdef FLASH_OPTSR2_CPUFREQ_BOOST + return not boost or (FLASH->OPTSR2_CUR & FLASH_OPTSR2_CPUFREQ_BOOST) != 0; +#else + return not boost; +#endif +} + +IWDG_TypeDef* watchdog() { + return IWDG1; +} + +uint32_t timer_clock(const TIM_TypeDef* instance) { + RCC_ClkInitTypeDef clock_config{}; + uint32_t flash_latency{}; + HAL_RCC_GetClockConfig(&clock_config, &flash_latency); + + if (std::bit_cast(instance) >= APB2PERIPH_BASE) { + const uint32_t pclk2 = HAL_RCC_GetPCLK2Freq(); + return clock_config.APB2CLKDivider == RCC_APB2_DIV1 ? pclk2 : 2 * pclk2; + } + + const uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); + return clock_config.APB1CLKDivider == RCC_APB1_DIV1 ? pclk1 : 2 * pclk1; +} + +void enable_cycle_counter() { + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + if ((DWT->LSR & ITM_LSR_Present_Msk) != 0 and (DWT->LSR & ITM_LSR_Access_Msk) != 0) { + DWT->LAR = software_lock_key; + } + + DWT->CYCCNT = 0; + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; +} +} // namespace micras::hal::family diff --git a/micras_hal/stm32/src/flash.cpp b/micras_hal/stm32/h7/src/flash.cpp similarity index 96% rename from micras_hal/stm32/src/flash.cpp rename to micras_hal/stm32/h7/src/flash.cpp index c5924f7..1be2c47 100644 --- a/micras_hal/stm32/src/flash.cpp +++ b/micras_hal/stm32/h7/src/flash.cpp @@ -8,6 +8,9 @@ #include #include +#include + +#include "micras/hal/family/flash.hpp" #include "micras/hal/flash.hpp" namespace micras::hal { @@ -21,7 +24,7 @@ static const uint32_t base_address = FLASH_BASE + FLASH_SIZE / 2; /** * @brief Index of the first sector of the flash memory region reserved for data storage. */ -static constexpr uint16_t base_sector{FLASH_SECTOR_TOTAL / 2}; +static constexpr uint16_t base_sector{family::storage_first_sector}; /** * @brief Round a number of bytes up to a whole number of flash words. diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index e29b1ee..3683a41 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -10,6 +10,7 @@ #include #include "micras/hal/adc_dma.hpp" +#include "micras/hal/family.hpp" extern "C" { /** @@ -43,13 +44,7 @@ AdcDma::AdcDma(const Config& config) : config.init_function(); } -#ifdef STM32H7 - const HAL_StatusTypeDef status = HAL_ADCEx_Calibration_Start(this->handle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED); -#elifdef STM32G4 - const HAL_StatusTypeDef status = HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); -#else - #error "ADC calibration is only supported for STM32H7 and STM32G4 platforms." -#endif + const bool calibrated = family::calibrate_adc(this->handle); auto* const slot = std::ranges::find(instances, nullptr); @@ -58,7 +53,7 @@ AdcDma::AdcDma(const Config& config) : } *slot = this; - this->initialized = status == HAL_OK; + this->initialized = calibrated; } AdcDma::~AdcDma() { diff --git a/micras_hal/stm32/src/mcu.cpp b/micras_hal/stm32/src/mcu.cpp index d9887b1..663b035 100644 --- a/micras_hal/stm32/src/mcu.cpp +++ b/micras_hal/stm32/src/mcu.cpp @@ -7,23 +7,13 @@ #include #include +#include "micras/hal/family.hpp" #include "micras/hal/gpio.hpp" #include "micras/hal/mcu.hpp" #include "micras/hal/pwm.hpp" #include "micras/hal/timer.hpp" namespace micras::hal { -/** - * @brief Independent watchdog instance, spelled IWDG1 on the parts that have more than one. - */ -static IWDG_TypeDef* watchdog_instance() { -#ifdef IWDG1 - return IWDG1; -#else - return IWDG; -#endif -} - /** * @brief Key values of the independent watchdog key register. */ @@ -63,39 +53,17 @@ static constexpr uint32_t inactive_compare{0xFFFFFFFF}; bool Mcu::watchdog_reset{}; bool Mcu::cpu_frequency_supported{}; -/** - * @brief Check whether the option byte that lets the core run above its default maximum frequency is - * set. - * - * @return True if the option byte is set, false otherwise or on a part without it. - */ -static bool cpu_frequency_boosted() { -#ifdef FLASH_OPTSR2_CPUFREQ_BOOST - return (FLASH->OPTSR2_CUR & FLASH_OPTSR2_CPUFREQ_BOOST) != 0; -#else - return false; -#endif -} - void Mcu::init(const Config& config) { -#ifdef IWDG1 - watchdog_reset = __HAL_RCC_GET_FLAG(RCC_FLAG_IWDG1RST) != 0; -#else - watchdog_reset = __HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) != 0; -#endif + watchdog_reset = family::was_reset_by_watchdog(); __HAL_RCC_CLEAR_RESET_FLAGS(); - SCB_EnableICache(); + family::enable_caches(); HAL_Init(); -#ifdef IWDG1 - __HAL_DBGMCU_FREEZE_IWDG1(); -#else - __HAL_DBGMCU_FREEZE_IWDG(); -#endif + family::freeze_watchdog_in_debug(); - cpu_frequency_supported = not config.cpu_frequency_boost or cpu_frequency_boosted(); + cpu_frequency_supported = family::is_cpu_frequency_supported(config.cpu_frequency_boost); config.clock_init(); @@ -132,15 +100,17 @@ void Mcu::set_watchdog_timeout(uint32_t timeout_ms) { const uint32_t reload = std::clamp(ticks, 1, watchdog_max_reload + 1) - 1; - watchdog_instance()->KR = watchdog_key_start; - watchdog_instance()->KR = watchdog_key_write; - watchdog_instance()->PR = prescaler; - watchdog_instance()->RLR = reload; + IWDG_TypeDef* const watchdog = family::watchdog(); + + watchdog->KR = watchdog_key_start; + watchdog->KR = watchdog_key_write; + watchdog->PR = prescaler; + watchdog->RLR = reload; const uint32_t start = Timer::get_counter(); const uint32_t limit = Timer::to_cycles(watchdog_timeout_us); - while ((watchdog_instance()->SR & (IWDG_SR_PVU | IWDG_SR_RVU)) != 0) { + while ((watchdog->SR & (IWDG_SR_PVU | IWDG_SR_RVU)) != 0) { if (Timer::get_counter() - start > limit) { break; } @@ -150,7 +120,7 @@ void Mcu::set_watchdog_timeout(uint32_t timeout_ms) { } void Mcu::refresh_watchdog() { - watchdog_instance()->KR = watchdog_key_reload; + family::watchdog()->KR = watchdog_key_reload; } bool Mcu::was_reset_by_watchdog() { diff --git a/micras_hal/stm32/src/pwm.cpp b/micras_hal/stm32/src/pwm.cpp index c4b7869..d06a66e 100644 --- a/micras_hal/stm32/src/pwm.cpp +++ b/micras_hal/stm32/src/pwm.cpp @@ -3,34 +3,12 @@ */ #include -#include #include +#include "micras/hal/family.hpp" #include "micras/hal/pwm.hpp" namespace micras::hal { -/** - * @brief Get the frequency of the clock that feeds a timer. - * - * @note Timers run at twice the frequency of their APB bus when the APB prescaler is not 1. - * - * @param instance Timer peripheral. - * @return Timer clock frequency in Hz. - */ -static uint32_t get_timer_clock_frequency(const TIM_TypeDef* instance) { - RCC_ClkInitTypeDef clock_config{}; - uint32_t flash_latency{}; - HAL_RCC_GetClockConfig(&clock_config, &flash_latency); - - if (std::bit_cast(instance) >= APB2PERIPH_BASE) { - const uint32_t pclk2 = HAL_RCC_GetPCLK2Freq(); - return clock_config.APB2CLKDivider == RCC_APB2_DIV1 ? pclk2 : 2 * pclk2; - } - - const uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); - return clock_config.APB1CLKDivider == RCC_APB1_DIV1 ? pclk1 : 2 * pclk1; -} - /** * @brief Mask that turns the identifier of one of the first four channels into the position of its * bits in the capture and compare enable register. @@ -89,7 +67,7 @@ void Pwm::set_duty_cycle(float duty_cycle) { } void Pwm::set_frequency(uint32_t frequency) { - const uint32_t base_freq = get_timer_clock_frequency(this->handle->Instance); + const uint32_t base_freq = family::timer_clock(this->handle->Instance); const uint32_t prescaler = this->handle->Instance->PSC; const uint32_t autoreload = base_freq / ((prescaler + 1) * frequency) - 1; @@ -103,7 +81,7 @@ float Pwm::get_frequency() const { const uint32_t period = center_aligned ? 2 * autoreload : autoreload + 1; const auto ticks = static_cast(this->handle->Instance->PSC + 1) * static_cast(period); - return static_cast(get_timer_clock_frequency(this->handle->Instance)) / ticks; + return static_cast(family::timer_clock(this->handle->Instance)) / ticks; } bool Pwm::was_initialized() const { diff --git a/micras_hal/stm32/src/timer.cpp b/micras_hal/stm32/src/timer.cpp index dfa2878..6291004 100644 --- a/micras_hal/stm32/src/timer.cpp +++ b/micras_hal/stm32/src/timer.cpp @@ -5,25 +5,14 @@ #include #include +#include "micras/hal/family.hpp" #include "micras/hal/timer.hpp" namespace micras::hal { -/** - * @brief Key that unlocks write access to the debug components on the Cortex-M7. - */ -static constexpr uint32_t software_lock_key{0xC5ACCE55}; - uint32_t Timer::cycles_per_microsecond{1}; void Timer::init() { - CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; - - if ((DWT->LSR & ITM_LSR_Present_Msk) != 0 and (DWT->LSR & ITM_LSR_Access_Msk) != 0) { - DWT->LAR = software_lock_key; - } - - DWT->CYCCNT = 0; - DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; + family::enable_cycle_counter(); cycles_per_microsecond = SystemCoreClock / 1000000; } diff --git a/tests/hal/test_family.cpp b/tests/hal/test_family.cpp new file mode 100644 index 0000000..bd9cfe5 --- /dev/null +++ b/tests/hal/test_family.cpp @@ -0,0 +1,48 @@ +/** + * @file + */ + +#include + +#include "host_fixture.hpp" +#include "micras/hal/family.hpp" +#include "micras/hal/family/flash.hpp" +#include "micras/hal/flash.hpp" +#include "tim.h" + +namespace micras::test { +TEST_SUITE("family") { + TEST_CASE("reads the flash geometry from the board's macros") { + CHECK(hal::family::flash_word_bits == 256); + CHECK(hal::family::sector_size == 128 * 1024); + CHECK(hal::family::storage_first_sector == 4); + CHECK(hal::family::storage_sectors == 4); + + CHECK(hal::FlashWord::words == 8); + CHECK(hal::FlashWord::size == 32); + CHECK(hal::Flash::total_size == hal::family::storage_sectors * hal::family::sector_size); + } + + TEST_CASE_FIXTURE(HostBoard, "takes a timer's clock from its kernel clock") { + MX_TIM15_Init(); + + CHECK(hal::family::timer_clock(htim15.Instance) == htim15.Instance->kernel_clock); + CHECK(hal::family::timer_clock(htim15.Instance) == 200000000); + } + + TEST_CASE("has nothing to calibrate, cache or unlock on a host") { + ADC_HandleTypeDef adc{}; + + CHECK(hal::family::calibrate_adc(&adc)); + CHECK_FALSE(hal::family::was_reset_by_watchdog()); + CHECK(hal::family::is_cpu_frequency_supported(true)); + CHECK(hal::family::is_cpu_frequency_supported(false)); + CHECK(hal::family::watchdog() != nullptr); + CHECK(hal::family::watchdog() == hal::family::watchdog()); + + hal::family::enable_caches(); + hal::family::freeze_watchdog_in_debug(); + hal::family::enable_cycle_counter(); + } +} +} // namespace micras::test From 0478b4a5bd3c3b0c6ec99d741bdbd7ab9530d2f3 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 13:24:25 -0300 Subject: [PATCH 547/561] =?UTF-8?q?=E2=9C=A8=20Add=20the=20STM32G4=20famil?= =?UTF-8?q?y=20to=20the=20stm32=20backend?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit An STM32G4 board target, such as the STM32G474 of the old Micras board, selects stm32/g4: ADC calibration with the single-ended argument only, no cache to enable, IWDG, no boost option byte, RCC_HCLK_DIV1 as the undivided APB prescaler, a cycle counter without a lock, and a page-based flash driver that programs double words into bank 2 of a 512 KB part in dual bank mode and refuses any other layout rather than erase what may be code. --- CMakeLists.txt | 2 +- micras_hal/stm32/g4/README.md | 19 +++ .../g4/include/micras/hal/family/flash.hpp | 46 ++++++ micras_hal/stm32/g4/src/family.cpp | 54 ++++++ micras_hal/stm32/g4/src/flash.cpp | 155 ++++++++++++++++++ 5 files changed, 275 insertions(+), 1 deletion(-) create mode 100644 micras_hal/stm32/g4/README.md create mode 100644 micras_hal/stm32/g4/include/micras/hal/family/flash.hpp create mode 100644 micras_hal/stm32/g4/src/family.cpp create mode 100644 micras_hal/stm32/g4/src/flash.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 30da05f..3d313be 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,7 +64,7 @@ set(MICRAS_LIB_HAL_BACKEND "stm32" CACHE STRING "Backend of micras_hal: stm32 or set_property(CACHE MICRAS_LIB_HAL_BACKEND PROPERTY STRINGS stm32 host) set(MICRAS_LIB_STM32_FAMILY "" CACHE STRING "STM32 family of the stm32 backend, detected from the board target when empty") -set_property(CACHE MICRAS_LIB_STM32_FAMILY PROPERTY STRINGS "" h7) +set_property(CACHE MICRAS_LIB_STM32_FAMILY PROPERTY STRINGS "" h7 g4) set(MICRAS_LIB_CUBE_TARGET "stm32cubemx" CACHE STRING "Board target micras_hal links, on every backend") diff --git a/micras_hal/stm32/g4/README.md b/micras_hal/stm32/g4/README.md new file mode 100644 index 0000000..2015179 --- /dev/null +++ b/micras_hal/stm32/g4/README.md @@ -0,0 +1,19 @@ +# STM32G4 family + +Code of the stm32 backend that only the STM32G4 family runs, which CMake compiles when the board +target defines an STM32G4 device (or `MICRAS_LIB_STM32_FAMILY` is `g4`): + +- `src/family.cpp`: the functions of `micras/hal/family.hpp`. The ADC calibrates for single-ended + conversions, there is no cache to enable (`HAL_Init` sets up the ART accelerator), the watchdog is + `IWDG`, every core frequency the clock tree reaches is supported (the family has no boost option + byte), a timer runs at twice its APB clock when the APB prescaler (`RCC_HCLK_DIV1`) is not 1, and + the cycle counter starts without an unlock, since the Cortex-M4 has no lock access register. +- `src/flash.cpp`: the flash driver, pages of 2 KB programmed in 64-bit double words. A sector of the + shared `Flash` class is a page. +- `include/micras/hal/family/flash.hpp`: the flash geometry the shared `flash.hpp` reads. The storage + region is bank 2 of a 512 KB part, such as the STM32G474RET6 of the old Micras board, in the dual + bank mode it leaves the factory in: 128 pages from 0x08040000. Every flash operation checks the + `DBANK` option bit and the size of the part, and fails on any other layout rather than erase what + may be code. + +The code in `../src` is the same on every family and names no chip. diff --git a/micras_hal/stm32/g4/include/micras/hal/family/flash.hpp b/micras_hal/stm32/g4/include/micras/hal/family/flash.hpp new file mode 100644 index 0000000..eef578e --- /dev/null +++ b/micras_hal/stm32/g4/include/micras/hal/family/flash.hpp @@ -0,0 +1,46 @@ +/** + * @file + * + * @brief Flash memory geometry of the STM32G4 family. + * + * @note The flash memory is erased in pages and programmed in double words of 64 bits; a sector of the + * shared Flash class is a page. The geometry is the one the 512 KB parts, such as the STM32G474xE, have + * in the dual bank mode they leave the factory in: two banks of 128 pages of 2 KB. The storage region + * is bank 2, the upper half of the flash memory. Every operation checks the part and the mode at run + * time and fails on any other layout. + */ + +#ifndef MICRAS_HAL_FAMILY_FLASH_HPP +#define MICRAS_HAL_FAMILY_FLASH_HPP + +#include +#include + +namespace micras::hal::family { +/** + * @brief Number of pages of a bank of the 512 KB parts in dual bank mode. + */ +inline constexpr uint16_t pages_per_bank{128}; + +/** + * @brief Number of bits of the smallest unit the flash memory programs. + */ +inline constexpr uint32_t flash_word_bits{64}; + +/** + * @brief Number of bytes of an erasable page. + */ +inline constexpr uint32_t sector_size{FLASH_PAGE_SIZE}; + +/** + * @brief Index of the first page reserved for data storage, counting from the start of bank 1. + */ +inline constexpr uint16_t storage_first_sector{pages_per_bank}; + +/** + * @brief Number of pages reserved for data storage, the whole of bank 2. + */ +inline constexpr uint16_t storage_sectors{pages_per_bank}; +} // namespace micras::hal::family + +#endif // MICRAS_HAL_FAMILY_FLASH_HPP diff --git a/micras_hal/stm32/g4/src/family.cpp b/micras_hal/stm32/g4/src/family.cpp new file mode 100644 index 0000000..92cd80c --- /dev/null +++ b/micras_hal/stm32/g4/src/family.cpp @@ -0,0 +1,54 @@ +/** + * @file + */ + +#include +#include +#include + +#include "micras/hal/family.hpp" + +namespace micras::hal::family { +bool calibrate_adc(ADC_HandleTypeDef* handle) { + return HAL_ADCEx_Calibration_Start(handle, ADC_SINGLE_ENDED) == HAL_OK; +} + +void enable_caches() { } + +bool was_reset_by_watchdog() { + return __HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) != 0; +} + +void freeze_watchdog_in_debug() { + __HAL_DBGMCU_FREEZE_IWDG(); +} + +bool is_cpu_frequency_supported(bool /*boost*/) { + return true; +} + +IWDG_TypeDef* watchdog() { + return IWDG; +} + +uint32_t timer_clock(const TIM_TypeDef* instance) { + RCC_ClkInitTypeDef clock_config{}; + uint32_t flash_latency{}; + HAL_RCC_GetClockConfig(&clock_config, &flash_latency); + + if (std::bit_cast(instance) >= APB2PERIPH_BASE) { + const uint32_t pclk2 = HAL_RCC_GetPCLK2Freq(); + return clock_config.APB2CLKDivider == RCC_HCLK_DIV1 ? pclk2 : 2 * pclk2; + } + + const uint32_t pclk1 = HAL_RCC_GetPCLK1Freq(); + return clock_config.APB1CLKDivider == RCC_HCLK_DIV1 ? pclk1 : 2 * pclk1; +} + +void enable_cycle_counter() { + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + DWT->CYCCNT = 0; + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; +} +} // namespace micras::hal::family diff --git a/micras_hal/stm32/g4/src/flash.cpp b/micras_hal/stm32/g4/src/flash.cpp new file mode 100644 index 0000000..0bcaf25 --- /dev/null +++ b/micras_hal/stm32/g4/src/flash.cpp @@ -0,0 +1,155 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/hal/family/flash.hpp" +#include "micras/hal/flash.hpp" + +namespace micras::hal { +static constexpr uint32_t base_address{FLASH_BASE + (family::storage_first_sector * family::sector_size)}; + +static constexpr uint32_t erase_complete{0xFFFFFFFFU}; + +static_assert(FlashWord::size == sizeof(uint64_t)); + +static constexpr uint32_t align_size(uint32_t size) { + return (size + FlashWord::size - 1) / FlashWord::size * FlashWord::size; +} + +static bool is_storage_available() { + return (FLASH->OPTR & FLASH_OPTR_DBANK) != 0 and FLASH_PAGE_NB == family::pages_per_bank; +} + +static void flush_data_cache() { + if ((FLASH->ACR & FLASH_ACR_DCEN) == 0) { + return; + } + + __HAL_FLASH_DATA_CACHE_DISABLE(); + __HAL_FLASH_DATA_CACHE_RESET(); + __HAL_FLASH_DATA_CACHE_ENABLE(); +} + +FlashWord::FlashWord(std::span data) { + const auto data_address = std::bit_cast(data.data()); + + if (data.size() >= size and data_address % alignof(uint32_t) == 0) { + this->source = std::bit_cast(data.data()); + return; + } + + const std::span bytes{std::bit_cast(this->buffer.data()), size}; + + std::ranges::fill(bytes, erased_value); + std::ranges::copy(data.first(std::min(data.size(), size)), bytes.begin()); + + this->source = this->buffer.data(); +} + +const uint32_t* FlashWord::data() const { + return this->source; +} + +bool FlashWord::is_padded() const { + return this->source == this->buffer.data(); +} + +std::span Flash::read(uint32_t address, uint32_t size) { + if (address > total_size or size > total_size - address or not is_storage_available()) { + return {}; + } + + return {std::bit_cast(base_address + address), size}; +} + +std::span Flash::read(uint16_t sector, uint32_t sector_address, uint32_t size) { + if (sector >= total_sectors or sector_address > sector_size) { + return {}; + } + + return read(sector * sector_size + sector_address, size); +} + +Flash::Status Flash::write(uint32_t address, std::span data) { + if (address % FlashWord::size != 0) { + return Status::MISALIGNED; + } + + if (address > total_size or align_size(data.size()) > total_size - address) { + return Status::OUT_OF_BOUNDS; + } + + if (not is_storage_available() or HAL_FLASH_Unlock() != HAL_OK) { + return Status::ERROR; + } + + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS); + + Status status = Status::OK; + + for (uint32_t offset = 0; offset < data.size(); offset += FlashWord::size) { + const FlashWord word{data.subspan(offset)}; + uint64_t double_word{}; + std::memcpy(&double_word, word.data(), sizeof(double_word)); + + if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, base_address + address + offset, double_word) != HAL_OK) { + status = Status::ERROR; + break; + } + } + + HAL_FLASH_Lock(); + flush_data_cache(); + + return status; +} + +Flash::Status Flash::write(uint16_t sector, uint32_t sector_address, std::span data) { + if (sector >= total_sectors or sector_address > sector_size) { + return Status::OUT_OF_BOUNDS; + } + + return write(sector * sector_size + sector_address, data); +} + +Flash::Status Flash::erase_sectors(uint16_t start_sector, uint16_t number_of_sectors) { + if (start_sector >= total_sectors or number_of_sectors > total_sectors - start_sector) { + return Status::OUT_OF_BOUNDS; + } + + FLASH_EraseInitTypeDef erase_struct = { + .TypeErase = FLASH_TYPEERASE_PAGES, + .Banks = FLASH_BANK_2, + .Page = start_sector, + .NbPages = number_of_sectors, + }; + + if (not is_storage_available() or HAL_FLASH_Unlock() != HAL_OK) { + return Status::ERROR; + } + + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_ALL_ERRORS); + pFlash.ErrorCode = HAL_FLASH_ERROR_NONE; + + uint32_t page_error{}; + + const HAL_StatusTypeDef hal_status = HAL_FLASHEx_Erase(&erase_struct, &page_error); + + HAL_FLASH_Lock(); + + if (hal_status != HAL_OK or page_error != erase_complete) { + return Status::ERROR; + } + + return Status::OK; +} +} // namespace micras::hal From 81e549663abbcd032f18dd371a751fab76e26686 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 13:24:33 -0300 Subject: [PATCH 548/561] =?UTF-8?q?=F0=9F=9A=A8=20Check=20the=20shared=20c?= =?UTF-8?q?ode=20for=20chip=20macros=20in=20the=20host=20job?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit scripts/check_no_chip_macros.sh fails when a conditional directive in the code every STM32 family shares (the HAL headers, stm32/src, the proxies, core, nav and comm) names a chip, a core or a peripheral only some parts have, so chip-specific code stays in the family folders CMake selects. --- .github/workflows/ci.yaml | 3 +++ scripts/check_no_chip_macros.sh | 46 +++++++++++++++++++++++++++++++++ 2 files changed, 49 insertions(+) create mode 100755 scripts/check_no_chip_macros.sh diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c1bfcb2..81f769d 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -56,6 +56,9 @@ jobs: - name: ๐Ÿ“ Build the documentation run: docker exec host cmake --build --preset host --target docs + - name: ๐Ÿ” Check the shared code for chip macros + run: docker exec host scripts/check_no_chip_macros.sh + - name: ๐Ÿงน Stop the host container if: always() run: docker rm --force host diff --git a/scripts/check_no_chip_macros.sh b/scripts/check_no_chip_macros.sh new file mode 100755 index 0000000..0a73bd9 --- /dev/null +++ b/scripts/check_no_chip_macros.sh @@ -0,0 +1,46 @@ +#!/usr/bin/env bash +# Fails when the code every STM32 family shares tests a chip in the preprocessor. +# +# Usage: scripts/check_no_chip_macros.sh [] +# +# The chip-specific code of the stm32 backend lives in micras_hal/stm32//, and CMake compiles +# exactly one family. Everything else is shared, so a conditional directive there (#if, #ifdef, +# #ifndef, #elif, #elifdef, #elifndef) that names a chip, a family, a core or a peripheral that only +# some parts have is an error: that code belongs in a family function or a family header. The include +# guards (#ifndef MICRAS_..._HPP) name none of them. + +set -euo pipefail + +root=${1:-$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)} + +shared_paths=( + micras_hal/include + micras_hal/stm32/src + micras_proxy + micras_core + micras_nav + micras_comm +) + +directive='^[[:space:]]*#[[:space:]]*(if|ifdef|ifndef|elif|elifdef|elifndef)\b' +chip_names='\b(STM32\w*|IWDG\w*|FLASH_\w+|RCC_\w+|DBGMCU\w*|DWT\w*|SCB_\w+|ITM_\w+|__CORTEX_M|__ARM_ARCH\w*|__FPU_\w+|__[ID]CACHE_PRESENT|__MPU_PRESENT|CORE_CM\w*|ADC_CALIB\w*|\w+_TypeDef)\b' + +cd "$root" + +for path in "${shared_paths[@]}"; do + if [ ! -d "$path" ]; then + echo "check_no_chip_macros: $path does not exist under $root" >&2 + exit 2 + fi +done + +matches=$(grep -rnE --include='*.hpp' --include='*.tpp' --include='*.cpp' --include='*.h' --include='*.c' \ + "$directive.*($chip_names)" "${shared_paths[@]}" || true) + +if [ -n "$matches" ]; then + echo "Chip macros in code every family shares; move them into micras_hal/stm32//:" >&2 + echo "$matches" >&2 + exit 1 +fi + +echo "No chip macro in ${shared_paths[*]}" From c97845da19973e943afeace6c80d820f5e0758ba Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 13:24:41 -0300 Subject: [PATCH 549/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Share=20the=20benc?= =?UTF-8?q?h=20program=20and=20the=20reference=20CMake=20between=20familie?= =?UTF-8?q?s?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit reference/common holds what every family's reference project runs and builds: the bench checks, the toolchain and flag setup read before project(), and the generated tree, micras-lib, bench and lint glue read after it. A family's bench.cpp only names its UART and converts the internal reference with its low-layer ADC header, and the timer check compares against the clock family::timer_clock reports, the one Pwm uses. --- cmake/micras_lint.cmake | 5 +- reference/common/bench.cpp | 301 +++++++++++++++++++++++++++++++ reference/common/bench.hpp | 54 ++++++ reference/common/reference.cmake | 105 +++++++++++ reference/common/toolchain.cmake | 55 ++++++ reference/h7/CMakeLists.txt | 150 +-------------- reference/h7/bench.cpp | 299 +----------------------------- 7 files changed, 531 insertions(+), 438 deletions(-) create mode 100644 reference/common/bench.cpp create mode 100644 reference/common/bench.hpp create mode 100644 reference/common/reference.cmake create mode 100644 reference/common/toolchain.cmake diff --git a/cmake/micras_lint.cmake b/cmake/micras_lint.cmake index 73bc67a..6d2323b 100644 --- a/cmake/micras_lint.cmake +++ b/cmake/micras_lint.cmake @@ -74,7 +74,10 @@ file(GLOB_RECURSE MICRAS_LIB_FORMAT_FILES CONFIGURE_DEPENDS ${MICRAS_LIB_ROOT}/tests/*.h ) -file(GLOB MICRAS_LIB_BENCH_FILES CONFIGURE_DEPENDS ${MICRAS_LIB_ROOT}/reference/*/bench.cpp) +file(GLOB MICRAS_LIB_BENCH_FILES CONFIGURE_DEPENDS + ${MICRAS_LIB_ROOT}/reference/*/bench.cpp + ${MICRAS_LIB_ROOT}/reference/common/*.hpp +) list(APPEND MICRAS_LIB_FORMAT_FILES ${MICRAS_LIB_BENCH_FILES}) if(micras_lib_IS_TOP_LEVEL) diff --git a/reference/common/bench.cpp b/reference/common/bench.cpp new file mode 100644 index 0000000..a8877e4 --- /dev/null +++ b/reference/common/bench.cpp @@ -0,0 +1,301 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "bench.hpp" +#include "micras/core/butterworth_filter.hpp" +#include "micras/core/variable_pool.hpp" +#include "micras/hal/adc_dma.hpp" +#include "micras/hal/crc.hpp" +#include "micras/hal/encoder.hpp" +#include "micras/hal/family.hpp" +#include "micras/hal/gpio.hpp" +#include "micras/hal/mcu.hpp" +#include "micras/hal/pwm.hpp" +#include "micras/hal/pwm_dma.hpp" +#include "micras/hal/spi.hpp" +#include "micras/hal/timer.hpp" +#include "micras/hal/uart_dma.hpp" +#include "micras/proxy/fmac_filter.hpp" +#include "micras/proxy/storage.hpp" +#include "micras/proxy/watchdog.hpp" + +extern "C" { +void SystemClock_Config(); +} + +namespace { +// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) +volatile bool storage_passed{}; +volatile bool watchdog_passed{}; +volatile float timer_clock_error{}; +volatile bool timer_passed{}; +volatile uint32_t crc_value{}; +volatile bool crc_passed{}; +volatile float fmac_error{}; +volatile bool fmac_passed{}; +volatile uint32_t vdda_mv{}; +volatile bool adc_passed{}; +volatile bool bench_done{}; +volatile bool start_pin_drivers{}; + +std::array adc_buffer{}; +std::array adc_snapshot{}; +std::array pwm_dma_compares{}; +std::array spi_transmitted{}; +std::array spi_received{}; +std::array uart_ring{}; + +// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) + +constexpr uint32_t storage_magic{0x5EED1E55}; + +constexpr micras::proxy::Storage::Config storage_config{.start_sector = 0, .number_of_sectors = 1}; + +constexpr uint32_t watchdog_timeout_ms{50}; + +constexpr uint32_t timer_window_us{10000}; + +constexpr float timer_tolerance{1e-3F}; + +constexpr uint8_t crc_polynomial{0x1D}; + +constexpr uint8_t crc_initial_value{0xC4}; + +constexpr micras::core::ButterworthFilter::Config filter_config{ + .cutoff_frequency = 200.0F, + .sampling_frequency = 1000.0F, +}; + +constexpr float fmac_tolerance{2e-3F}; + +constexpr uint32_t adc_timeout_us{10000}; + +constexpr uint32_t min_vdda_mv{1620}; + +constexpr uint32_t max_vdda_mv{3600}; +} // namespace + +namespace micras::reference { +static void save_before_reset() { + core::TVariablePool<1> pool; + uint32_t magic = storage_magic; + pool.add("bench/", "magic", magic, {.persist = true}); + + proxy::Storage storage{storage_config}; + storage.save(pool); +} + +static bool restored_after_reset() { + core::TVariablePool<1> pool; + uint32_t magic{}; + pool.add("bench/", "magic", magic, {.persist = true}); + + proxy::Storage storage{storage_config}; + return storage.restore(pool) == 1 and magic == storage_magic; +} + +[[noreturn]] static void expire_watchdog() { + const proxy::Watchdog watchdog{{.timeout_ms = watchdog_timeout_ms}}; + + while (true) { + bench_done = false; + } +} + +static bool check_timer_clock() { + MX_TIM2_Init(); + HAL_TIM_Base_Start(&htim2); + + const uint32_t start_cycles = hal::Timer::get_counter(); + const uint32_t start_counts = __HAL_TIM_GET_COUNTER(&htim2); + const uint32_t window = hal::Timer::to_cycles(timer_window_us); + + while (hal::Timer::get_counter() - start_cycles < window) { } + + const uint32_t cycles = hal::Timer::get_counter() - start_cycles; + const uint32_t counts = __HAL_TIM_GET_COUNTER(&htim2) - start_counts; + + const uint32_t timer_clock = hal::family::timer_clock(htim2.Instance); + const float measured = + static_cast(counts) * static_cast(SystemCoreClock) / static_cast(cycles); + const float error = measured / static_cast(timer_clock) - 1.0F; + + timer_clock_error = error; + return std::abs(error) < timer_tolerance; +} + +static uint8_t software_crc(std::span data) { + uint8_t crc = crc_initial_value; + + for (const uint8_t byte : data) { + crc ^= byte; + + for (uint8_t bit = 0; bit < 8; bit++) { + crc = (crc & 0x80U) != 0 ? static_cast((crc << 1U) ^ crc_polynomial) : + static_cast(crc << 1U); + } + } + + return crc; +} + +static bool check_crc() { + constexpr std::string_view check{"123456789"}; + const std::vector bytes(check.begin(), check.end()); + + hal::Crc crc{{.handle = &hcrc}}; + const uint32_t value = crc.calculate(bytes); + + crc_value = value; + return value == software_crc(bytes); +} + +static bool check_fmac() { + proxy::FmacFilter filter{{ + .fmac = {.init_function = MX_FMAC_Init, .handle = &hfmac}, + .filter = filter_config, + }}; + + if (not filter.was_initialized()) { + return false; + } + + const core::ButterworthFilter::Coefficients coefficients = + core::ButterworthFilter::compute_coefficients(filter_config); + std::array inputs{}; + std::array outputs{}; + float largest_error{}; + + for (uint32_t sample = 0; sample < 300; sample++) { + const float input = (sample < 150 ? 0.5F : -0.25F) + 0.1F * std::sin(0.9F * static_cast(sample)); + + inputs = {input, inputs.at(0), inputs.at(1)}; + + const float expected = + coefficients.feed_forward.at(0) * inputs.at(0) + coefficients.feed_forward.at(1) * inputs.at(1) + + coefficients.feed_forward.at(2) * inputs.at(2) - coefficients.feedback.at(0) * outputs.at(0) - + coefficients.feedback.at(1) * outputs.at(1); + + outputs = {expected, outputs.at(0)}; + largest_error = std::max(largest_error, std::abs(filter.update(input) - expected)); + } + + fmac_error = largest_error; + return largest_error < fmac_tolerance; +} + +static bool check_adc(const Board& board) { + hal::AdcDma adc{{ + .init_function = MX_ADC3_Init, + .handle = &hadc3, + .max_reading = 4095, + .reference_voltage = 3.3F, + }}; + + if (not adc.start_dma(adc_buffer, adc_snapshot)) { + return false; + } + + std::array readings{}; + const uint32_t start = hal::Timer::get_counter(); + const uint32_t timeout = hal::Timer::to_cycles(adc_timeout_us); + + while (adc.read_snapshot(readings) == 0) { + if (hal::Timer::get_counter() - start > timeout) { + adc.stop_dma(); + return false; + } + } + + adc.stop_dma(); + + const uint32_t vdda = board.vdda_mv(readings.at(0)); + + vdda_mv = vdda; + return vdda >= min_vdda_mv and vdda <= max_vdda_mv; +} + +static void run_pin_drivers(const Board& board) { + MX_GPIO_Init(); + + hal::Gpio led{{.port = Status_LED_GPIO_Port, .pin = Status_LED_Pin}}; + const hal::Gpio button{{.port = Button_GPIO_Port, .pin = Button_Pin}}; + led.write(button.read()); + + hal::Pwm pwm{ + {.init_function = MX_TIM15_Init, .handle = &htim15, .timer_channel = TIM_CHANNEL_1, .inverted = false} + }; + pwm.set_duty_cycle(50.0F); + + hal::PwmDma pwm_dma{{.init_function = MX_TIM8_Init, .handle = &htim8, .timer_channel = TIM_CHANNEL_1}}; + pwm_dma_compares.fill(static_cast(pwm_dma.get_compare(50.0F))); + pwm_dma.start_dma(pwm_dma_compares); + + const hal::Encoder encoder{{.init_function = MX_TIM5_Init, .handle = &htim5, .timer_channel = TIM_CHANNEL_ALL}}; + led.write(encoder.get_counter() > 0); + + hal::Spi spi{{ + .init_function = MX_SPI3_Init, + .handle = &hspi3, + .cs_gpio = {.port = SPI_CSn_GPIO_Port, .pin = SPI_CSn_Pin}, + .timeout = 2, + .clock_polarity = SPI_POLARITY_HIGH, + .clock_phase = SPI_PHASE_2EDGE, + }}; + spi.start_transfer(spi_transmitted, spi_received); + + hal::UartDma uart{{.init_function = board.uart_init, .handle = board.uart}}; + uart.start_rx(uart_ring); + uart.start_tx(spi_received); +} + +void run_bench(const Board& board) { + constexpr std::array peripheral_inits{MX_DMA_Init, MX_CRC_Init}; + + hal::Mcu::init({ + .clock_init = SystemClock_Config, + .peripheral_clock_init = nullptr, + .peripheral_inits = peripheral_inits, + .cpu_frequency_boost = false, + }); + + if (not hal::Mcu::was_reset_by_watchdog()) { + save_before_reset(); + expire_watchdog(); + } + + watchdog_passed = true; + storage_passed = restored_after_reset(); + timer_passed = check_timer_clock(); + crc_passed = check_crc(); + fmac_passed = check_fmac(); + adc_passed = check_adc(board); + bench_done = true; + + if (start_pin_drivers) { + run_pin_drivers(board); + } + + while (true) { + bench_done = true; + } +} +} // namespace micras::reference diff --git a/reference/common/bench.hpp b/reference/common/bench.hpp new file mode 100644 index 0000000..860b35f --- /dev/null +++ b/reference/common/bench.hpp @@ -0,0 +1,54 @@ +/** + * @file + * + * @brief The bench program every reference project runs, over the peripherals its board names. + * + * @note The bench initializes only peripherals that drive no pin, so its image is safe to flash on any + * board with the part. It writes each result into a volatile global for the debugger or + * STM32CubeMonitor, and ends with bench_done set. + */ + +#ifndef MICRAS_LIB_REFERENCE_BENCH_HPP +#define MICRAS_LIB_REFERENCE_BENCH_HPP + +#include +#include + +#include "micras/hal/mcu.hpp" + +namespace micras::reference { +/** + * @brief What differs between the reference projects of the families. + */ +struct Board { + /** + * @brief Initialization function of the UART with DMA. + */ + hal::Mcu::InitFunction uart_init; + + /** + * @brief Handle of the UART with DMA. + */ + UART_HandleTypeDef* uart; + + /** + * @brief Convert a conversion of the internal reference into the analog supply voltage. + * + * @note Each family reads its factory calibration of the internal reference through its own + * low-layer ADC header. + */ + uint32_t (*vdda_mv)(uint16_t vrefint_reading); +}; + +/** + * @brief Initialize the microcontroller and run every check of the bench. + * + * @note The first run saves to the storage and lets the watchdog expire, the run after the reset + * checks the reset cause and restores the storage before the other checks. + * + * @param board Peripherals of the reference project that differ between families. + */ +[[noreturn]] void run_bench(const Board& board); +} // namespace micras::reference + +#endif // MICRAS_LIB_REFERENCE_BENCH_HPP diff --git a/reference/common/reference.cmake b/reference/common/reference.cmake new file mode 100644 index 0000000..83e1562 --- /dev/null +++ b/reference/common/reference.cmake @@ -0,0 +1,105 @@ +############################################################################### +## Reference project: generated tree, micras-lib, bench program and lint +############################################################################### + +# Included by reference//CMakeLists.txt after its project(), with common/toolchain.cmake read +# before it. + +# The STM32CubeMX CMake generator writes its sources into ${CMAKE_PROJECT_NAME} instead of a target of +# its own, so the variable is pointed at a target created to hold them +add_library(cube_app OBJECT) +set(MICRAS_REFERENCE_PROJECT_NAME ${CMAKE_PROJECT_NAME}) +set(CMAKE_PROJECT_NAME cube_app) +add_subdirectory(${MICRAS_REFERENCE_DIR}/cmake/stm32cubemx stm32cubemx) +set(CMAKE_PROJECT_NAME ${MICRAS_REFERENCE_PROJECT_NAME}) + +# The generated headers are not the project's to warn about +set_target_properties(stm32cubemx PROPERTIES SYSTEM TRUE) + +foreach(CUBE_OBJECT_LIBRARY cube_app STM32_Drivers) + target_compile_options(${CUBE_OBJECT_LIBRARY} PRIVATE -w) +endforeach() + +############################################################################### +## micras-lib +############################################################################### + +set(MICRAS_LIB_WERROR ON) +add_subdirectory(${MICRAS_REFERENCE_COMMON_DIR}/../.. micras-lib EXCLUDE_FROM_ALL) + +############################################################################### +## Bench program +############################################################################### + +set(MICRAS_BENCH_TARGET micras_lib_bench_${MICRAS_REFERENCE_FAMILY}) + +# The vector table, the interrupt handlers and the MX_*_Init functions are linked into the executable, +# which is where micras-lib's callbacks have to win over the vendor's weak ones +add_executable(${MICRAS_BENCH_TARGET} + ${MICRAS_REFERENCE_DIR}/bench.cpp + ${MICRAS_REFERENCE_COMMON_DIR}/bench.cpp +) + +target_include_directories(${MICRAS_BENCH_TARGET} PRIVATE + ${MICRAS_REFERENCE_COMMON_DIR} +) + +target_link_libraries(${MICRAS_BENCH_TARGET} PRIVATE + micras::proxy + micras::nav + micras::comm + cube_app + STM32_Drivers +) + +micras_apply_warnings(${MICRAS_BENCH_TARGET} WERROR ON) + +target_link_options(${MICRAS_BENCH_TARGET} PRIVATE -Wl,-Map=$.map) + +add_custom_command(TARGET ${MICRAS_BENCH_TARGET} POST_BUILD + COMMAND ${CMAKE_OBJCOPY} -O ihex $ $.hex + COMMAND ${CMAKE_SIZE} $ +) + +############################################################################### +## Lint +############################################################################### + +# The stm32 backend of this family and the bench are linted here, against the family's ARM headers, +# since no host build compiles them +include(${MICRAS_REFERENCE_COMMON_DIR}/../../cmake/micras_lint.cmake) + +# clang finds the ARM target from the compiler's name, but not the headers of its toolchain +execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -print-search-dirs + OUTPUT_VARIABLE MICRAS_ARM_SEARCH_DIRS + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine + OUTPUT_VARIABLE MICRAS_ARM_TRIPLE + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +string(REGEX MATCH "install: ([^\n]+)/" MICRAS_ARM_INSTALL_MATCH ${MICRAS_ARM_SEARCH_DIRS}) +set(MICRAS_ARM_INSTALL_DIR ${CMAKE_MATCH_1}) +get_filename_component(MICRAS_ARM_GCC_VERSION ${MICRAS_ARM_INSTALL_DIR} NAME) +get_filename_component(MICRAS_ARM_PREFIX "${MICRAS_ARM_INSTALL_DIR}/../../../" REALPATH) +set(MICRAS_ARM_SYSROOT ${MICRAS_ARM_PREFIX}/${MICRAS_ARM_TRIPLE}) +set(MICRAS_ARM_CXX_INCLUDE_DIR ${MICRAS_ARM_SYSROOT}/include/c++/${MICRAS_ARM_GCC_VERSION}) + +file(GLOB MICRAS_REFERENCE_LINT_SOURCES CONFIGURE_DEPENDS + ${MICRAS_LIB_ROOT}/micras_hal/stm32/src/*.cpp + ${MICRAS_LIB_ROOT}/micras_hal/stm32/${MICRAS_REFERENCE_FAMILY}/src/*.cpp + ${MICRAS_LIB_ROOT}/reference/common/*.cpp + ${MICRAS_LIB_ROOT}/reference/${MICRAS_REFERENCE_FAMILY}/bench.cpp +) + +micras_add_lint_targets( + SOURCES ${MICRAS_REFERENCE_LINT_SOURCES} + EXTRA_ARGS + --sysroot=${MICRAS_ARM_SYSROOT}/ + -I${MICRAS_ARM_CXX_INCLUDE_DIR}/ + -I${MICRAS_ARM_CXX_INCLUDE_DIR}/${MICRAS_ARM_TRIPLE}/ +) diff --git a/reference/common/toolchain.cmake b/reference/common/toolchain.cmake new file mode 100644 index 0000000..7339142 --- /dev/null +++ b/reference/common/toolchain.cmake @@ -0,0 +1,55 @@ +############################################################################### +## Reference project: language, flags and the generated toolchain file +############################################################################### + +# Included by reference//CMakeLists.txt before its project(), with MICRAS_REFERENCE_FAMILY set. +# The Cube files are generated from reference//reference.ioc (the CI generates them with +# .docker/Dockerfile) and never committed. + +set(CMAKE_C_STANDARD 17) +set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_C_EXTENSIONS ON) + +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_CXX_SCAN_FOR_MODULES OFF) + +get_filename_component(MICRAS_REFERENCE_DIR ${CMAKE_CURRENT_LIST_DIR}/../${MICRAS_REFERENCE_FAMILY} ABSOLUTE) +set(MICRAS_REFERENCE_COMMON_DIR ${CMAKE_CURRENT_LIST_DIR}) + +if(NOT EXISTS ${MICRAS_REFERENCE_DIR}/cmake/stm32cubemx/CMakeLists.txt) + message(FATAL_ERROR + "micras-lib reference/${MICRAS_REFERENCE_FAMILY}: the Cube files are not generated. Generate them into " + "reference/${MICRAS_REFERENCE_FAMILY} with 'docker build --file .docker/Dockerfile --target " + "generated-${MICRAS_REFERENCE_FAMILY} --output type=local,dest=reference/${MICRAS_REFERENCE_FAMILY} .' " + "from the root of micras-lib, or with STM32CubeMX from reference.ioc.") +endif() + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# The generated toolchain file names the linker script relative to CMAKE_SOURCE_DIR, so it is pointed +# at the generated tree while the file is read +set(MICRAS_REFERENCE_SOURCE_DIR ${CMAKE_SOURCE_DIR}) +set(CMAKE_SOURCE_DIR ${MICRAS_REFERENCE_DIR}) +include(${MICRAS_REFERENCE_DIR}/cmake/gcc-arm-none-eabi.cmake) +set(CMAKE_SOURCE_DIR ${MICRAS_REFERENCE_SOURCE_DIR}) + +# One map file per target instead of the one name the generated file gives every target +string(REGEX REPLACE " *-Wl,-Map=[^ ]*" "" CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS}") + +# The flags a consumer builds its firmware with: the same as the Micras firmware's +set(MICRAS_MATH_FLAGS "-fno-math-errno -ffp-contract=fast") + +set(CMAKE_C_FLAGS_DEBUG "-Og -g3") +set(CMAKE_CXX_FLAGS_DEBUG "-Og -g3") +set(CMAKE_C_FLAGS_RELEASE "-O2 -g3 ${MICRAS_MATH_FLAGS}") +set(CMAKE_CXX_FLAGS_RELEASE "-O2 -g3 ${MICRAS_MATH_FLAGS}") + +# Link time optimization through the property, so that CMake archives with gcc-ar +set(CMAKE_INTERPROCEDURAL_OPTIMIZATION_RELEASE TRUE) +set(CMAKE_EXE_LINKER_FLAGS_RELEASE "-O2") diff --git a/reference/h7/CMakeLists.txt b/reference/h7/CMakeLists.txt index 00302c3..77f6bb8 100644 --- a/reference/h7/CMakeLists.txt +++ b/reference/h7/CMakeLists.txt @@ -5,152 +5,12 @@ cmake_minimum_required(VERSION 3.28...4.4) ############################################################################### # The smallest CubeMX project that exercises every peripheral micras-lib uses, built and linked with -# its bench program the way a consumer of the stm32 backend builds its firmware. The Cube files are -# generated from reference.ioc (the CI generates them with .docker/Dockerfile) and never committed. +# its bench program the way a consumer of the stm32 backend builds its firmware. What every family's +# reference shares is in ../common. -set(CMAKE_C_STANDARD 17) -set(CMAKE_C_STANDARD_REQUIRED ON) -set(CMAKE_C_EXTENSIONS ON) - -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CMAKE_CXX_SCAN_FOR_MODULES OFF) - -set(MICRAS_REFERENCE_DIR ${CMAKE_CURRENT_LIST_DIR}) - -if(NOT EXISTS ${MICRAS_REFERENCE_DIR}/cmake/stm32cubemx/CMakeLists.txt) - message(FATAL_ERROR - "micras-lib reference/h7: the Cube files are not generated. Generate them into reference/h7 with " - "'docker build --file .docker/Dockerfile --target generated-h7 --output type=local,dest=reference/h7 .' " - "from the root of micras-lib, or with STM32CubeMX from reference.ioc.") -endif() - -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - -############################################################################### -## Toolchain -############################################################################### - -# The generated toolchain file names the linker script relative to CMAKE_SOURCE_DIR, so it is pointed -# at the generated tree while the file is read -set(MICRAS_REFERENCE_SOURCE_DIR ${CMAKE_SOURCE_DIR}) -set(CMAKE_SOURCE_DIR ${MICRAS_REFERENCE_DIR}) -include(${MICRAS_REFERENCE_DIR}/cmake/gcc-arm-none-eabi.cmake) -set(CMAKE_SOURCE_DIR ${MICRAS_REFERENCE_SOURCE_DIR}) - -# One map file per target instead of the one name the generated file gives every target -string(REGEX REPLACE " *-Wl,-Map=[^ ]*" "" CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS}") - -# The flags a consumer builds its firmware with: the same as the Micras firmware's -set(MICRAS_MATH_FLAGS "-fno-math-errno -ffp-contract=fast") - -set(CMAKE_C_FLAGS_DEBUG "-Og -g3") -set(CMAKE_CXX_FLAGS_DEBUG "-Og -g3") -set(CMAKE_C_FLAGS_RELEASE "-O2 -g3 ${MICRAS_MATH_FLAGS}") -set(CMAKE_CXX_FLAGS_RELEASE "-O2 -g3 ${MICRAS_MATH_FLAGS}") - -# Link time optimization through the property, so that CMake archives with gcc-ar -set(CMAKE_INTERPROCEDURAL_OPTIMIZATION_RELEASE TRUE) -set(CMAKE_EXE_LINKER_FLAGS_RELEASE "-O2") +set(MICRAS_REFERENCE_FAMILY h7) +include(${CMAKE_CURRENT_LIST_DIR}/../common/toolchain.cmake) project(micras_lib_reference_h7 C CXX ASM) -############################################################################### -## Generated tree -############################################################################### - -# The STM32CubeMX CMake generator writes its sources into ${CMAKE_PROJECT_NAME} instead of a target of -# its own, so the variable is pointed at a target created to hold them -add_library(cube_app OBJECT) -set(MICRAS_REFERENCE_PROJECT_NAME ${CMAKE_PROJECT_NAME}) -set(CMAKE_PROJECT_NAME cube_app) -add_subdirectory(${MICRAS_REFERENCE_DIR}/cmake/stm32cubemx stm32cubemx) -set(CMAKE_PROJECT_NAME ${MICRAS_REFERENCE_PROJECT_NAME}) - -# The generated headers are not the project's to warn about -set_target_properties(stm32cubemx PROPERTIES SYSTEM TRUE) - -foreach(CUBE_OBJECT_LIBRARY cube_app STM32_Drivers) - target_compile_options(${CUBE_OBJECT_LIBRARY} PRIVATE -w) -endforeach() - -############################################################################### -## micras-lib -############################################################################### - -set(MICRAS_LIB_WERROR ON) -add_subdirectory(${MICRAS_REFERENCE_DIR}/../.. micras-lib EXCLUDE_FROM_ALL) - -############################################################################### -## Bench program -############################################################################### - -# The vector table, the interrupt handlers and the MX_*_Init functions are linked into the executable, -# which is where micras-lib's callbacks have to win over the vendor's weak ones -add_executable(micras_lib_bench_h7 - ${MICRAS_REFERENCE_DIR}/bench.cpp -) - -target_link_libraries(micras_lib_bench_h7 PRIVATE - micras::proxy - micras::nav - micras::comm - cube_app - STM32_Drivers -) - -micras_apply_warnings(micras_lib_bench_h7 WERROR ON) - -target_link_options(micras_lib_bench_h7 PRIVATE -Wl,-Map=$.map) - -add_custom_command(TARGET micras_lib_bench_h7 POST_BUILD - COMMAND ${CMAKE_OBJCOPY} -O ihex $ $.hex - COMMAND ${CMAKE_SIZE} $ -) - -############################################################################### -## Lint -############################################################################### - -# The stm32 backend and the bench are linted here, against the ARM headers, since no host build -# compiles them -include(${MICRAS_REFERENCE_DIR}/../../cmake/micras_lint.cmake) - -# clang finds the ARM target from the compiler's name, but not the headers of its toolchain -execute_process( - COMMAND ${CMAKE_CXX_COMPILER} -print-search-dirs - OUTPUT_VARIABLE MICRAS_ARM_SEARCH_DIRS - OUTPUT_STRIP_TRAILING_WHITESPACE -) - -execute_process( - COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine - OUTPUT_VARIABLE MICRAS_ARM_TRIPLE - OUTPUT_STRIP_TRAILING_WHITESPACE -) - -string(REGEX MATCH "install: ([^\n]+)/" MICRAS_ARM_INSTALL_MATCH ${MICRAS_ARM_SEARCH_DIRS}) -set(MICRAS_ARM_INSTALL_DIR ${CMAKE_MATCH_1}) -get_filename_component(MICRAS_ARM_GCC_VERSION ${MICRAS_ARM_INSTALL_DIR} NAME) -get_filename_component(MICRAS_ARM_PREFIX "${MICRAS_ARM_INSTALL_DIR}/../../../" REALPATH) -set(MICRAS_ARM_SYSROOT ${MICRAS_ARM_PREFIX}/${MICRAS_ARM_TRIPLE}) -set(MICRAS_ARM_CXX_INCLUDE_DIR ${MICRAS_ARM_SYSROOT}/include/c++/${MICRAS_ARM_GCC_VERSION}) - -file(GLOB MICRAS_REFERENCE_LINT_SOURCES CONFIGURE_DEPENDS - ${MICRAS_LIB_ROOT}/micras_hal/stm32/src/*.cpp - ${MICRAS_LIB_ROOT}/micras_hal/stm32/h7/src/*.cpp - ${MICRAS_LIB_ROOT}/reference/h7/bench.cpp -) - -micras_add_lint_targets( - SOURCES ${MICRAS_REFERENCE_LINT_SOURCES} - EXTRA_ARGS - --sysroot=${MICRAS_ARM_SYSROOT}/ - -I${MICRAS_ARM_CXX_INCLUDE_DIR}/ - -I${MICRAS_ARM_CXX_INCLUDE_DIR}/${MICRAS_ARM_TRIPLE}/ -) +include(${CMAKE_CURRENT_LIST_DIR}/../common/reference.cmake) diff --git a/reference/h7/bench.cpp b/reference/h7/bench.cpp index b9dd51d..4297f4a 100644 --- a/reference/h7/bench.cpp +++ b/reference/h7/bench.cpp @@ -2,306 +2,21 @@ * @file */ -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include #include -#include "micras/core/butterworth_filter.hpp" -#include "micras/core/variable_pool.hpp" -#include "micras/hal/adc_dma.hpp" -#include "micras/hal/crc.hpp" -#include "micras/hal/encoder.hpp" -#include "micras/hal/gpio.hpp" -#include "micras/hal/mcu.hpp" -#include "micras/hal/pwm.hpp" -#include "micras/hal/pwm_dma.hpp" -#include "micras/hal/spi.hpp" -#include "micras/hal/timer.hpp" -#include "micras/hal/uart_dma.hpp" -#include "micras/proxy/fmac_filter.hpp" -#include "micras/proxy/storage.hpp" -#include "micras/proxy/watchdog.hpp" +#include "bench.hpp" -extern "C" { -void SystemClock_Config(); -} - -namespace { -// NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) -volatile bool storage_passed{}; -volatile bool watchdog_passed{}; -volatile float timer_clock_error{}; -volatile bool timer_passed{}; -volatile uint32_t crc_value{}; -volatile bool crc_passed{}; -volatile float fmac_error{}; -volatile bool fmac_passed{}; -volatile uint32_t vdda_mv{}; -volatile bool adc_passed{}; -volatile bool bench_done{}; -volatile bool start_pin_drivers{}; - -std::array adc_buffer{}; -std::array adc_snapshot{}; -std::array pwm_dma_compares{}; -std::array spi_transmitted{}; -std::array spi_received{}; -std::array uart_ring{}; - -// NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) - -constexpr uint32_t storage_magic{0x5EED1E55}; - -constexpr micras::proxy::Storage::Config storage_config{.start_sector = 0, .number_of_sectors = 1}; - -constexpr uint32_t watchdog_timeout_ms{50}; - -constexpr uint32_t timer_window_us{10000}; - -constexpr float timer_tolerance{1e-3F}; - -constexpr uint8_t crc_polynomial{0x1D}; - -constexpr uint8_t crc_initial_value{0xC4}; - -constexpr micras::core::ButterworthFilter::Config filter_config{ - .cutoff_frequency = 200.0F, - .sampling_frequency = 1000.0F, -}; - -constexpr float fmac_tolerance{2e-3F}; - -constexpr uint32_t adc_timeout_us{10000}; - -constexpr uint32_t min_vdda_mv{1620}; - -constexpr uint32_t max_vdda_mv{3600}; -} // namespace - -static void save_before_reset() { - micras::core::TVariablePool<1> pool; - uint32_t magic = storage_magic; - pool.add("bench/", "magic", magic, {.persist = true}); - - micras::proxy::Storage storage{storage_config}; - storage.save(pool); -} - -static bool restored_after_reset() { - micras::core::TVariablePool<1> pool; - uint32_t magic{}; - pool.add("bench/", "magic", magic, {.persist = true}); - - micras::proxy::Storage storage{storage_config}; - return storage.restore(pool) == 1 and magic == storage_magic; -} - -[[noreturn]] static void expire_watchdog() { - const micras::proxy::Watchdog watchdog{{.timeout_ms = watchdog_timeout_ms}}; - - while (true) { - bench_done = false; - } -} - -static bool check_timer_clock() { - MX_TIM2_Init(); - HAL_TIM_Base_Start(&htim2); - - const uint32_t start_cycles = micras::hal::Timer::get_counter(); - const uint32_t start_counts = __HAL_TIM_GET_COUNTER(&htim2); - const uint32_t window = micras::hal::Timer::to_cycles(timer_window_us); - - while (micras::hal::Timer::get_counter() - start_cycles < window) { } - - const uint32_t cycles = micras::hal::Timer::get_counter() - start_cycles; - const uint32_t counts = __HAL_TIM_GET_COUNTER(&htim2) - start_counts; - - RCC_ClkInitTypeDef clocks{}; - uint32_t latency{}; - HAL_RCC_GetClockConfig(&clocks, &latency); - - const uint32_t bus_clock = HAL_RCC_GetPCLK1Freq(); - const uint32_t timer_clock = clocks.APB1CLKDivider == RCC_APB1_DIV1 ? bus_clock : 2 * bus_clock; - const float measured = - static_cast(counts) * static_cast(SystemCoreClock) / static_cast(cycles); - const float error = measured / static_cast(timer_clock) - 1.0F; - - timer_clock_error = error; - return std::abs(error) < timer_tolerance; -} - -static uint8_t software_crc(std::span data) { - uint8_t crc = crc_initial_value; - - for (const uint8_t byte : data) { - crc ^= byte; - - for (uint8_t bit = 0; bit < 8; bit++) { - crc = (crc & 0x80U) != 0 ? static_cast((crc << 1U) ^ crc_polynomial) : - static_cast(crc << 1U); - } - } - - return crc; -} - -static bool check_crc() { - constexpr std::string_view check{"123456789"}; - const std::vector bytes(check.begin(), check.end()); - - micras::hal::Crc crc{{.handle = &hcrc}}; - const uint32_t value = crc.calculate(bytes); - - crc_value = value; - return value == software_crc(bytes); -} - -static bool check_fmac() { - micras::proxy::FmacFilter filter{{ - .fmac = {.init_function = MX_FMAC_Init, .handle = &hfmac}, - .filter = filter_config, - }}; - - if (not filter.was_initialized()) { - return false; - } - - const micras::core::ButterworthFilter::Coefficients coefficients = - micras::core::ButterworthFilter::compute_coefficients(filter_config); - std::array inputs{}; - std::array outputs{}; - float largest_error{}; - - for (uint32_t sample = 0; sample < 300; sample++) { - const float input = (sample < 150 ? 0.5F : -0.25F) + 0.1F * std::sin(0.9F * static_cast(sample)); - - inputs = {input, inputs.at(0), inputs.at(1)}; - - const float expected = - coefficients.feed_forward.at(0) * inputs.at(0) + coefficients.feed_forward.at(1) * inputs.at(1) + - coefficients.feed_forward.at(2) * inputs.at(2) - coefficients.feedback.at(0) * outputs.at(0) - - coefficients.feedback.at(1) * outputs.at(1); - - outputs = {expected, outputs.at(0)}; - largest_error = std::max(largest_error, std::abs(filter.update(input) - expected)); - } - - fmac_error = largest_error; - return largest_error < fmac_tolerance; -} - -static bool check_adc() { - micras::hal::AdcDma adc{{ - .init_function = MX_ADC3_Init, - .handle = &hadc3, - .max_reading = 4095, - .reference_voltage = 3.3F, - }}; - - if (not adc.start_dma(adc_buffer, adc_snapshot)) { - return false; - } - - std::array readings{}; - const uint32_t start = micras::hal::Timer::get_counter(); - const uint32_t timeout = micras::hal::Timer::to_cycles(adc_timeout_us); - - while (adc.read_snapshot(readings) == 0) { - if (micras::hal::Timer::get_counter() - start > timeout) { - adc.stop_dma(); - return false; - } - } - - adc.stop_dma(); - - const uint32_t vdda = - __LL_ADC_CALC_VREFANALOG_VOLTAGE(static_cast(readings.at(0)), LL_ADC_RESOLUTION_12B); - - vdda_mv = vdda; - return vdda >= min_vdda_mv and vdda <= max_vdda_mv; -} - -static void run_pin_drivers() { - MX_GPIO_Init(); - - micras::hal::Gpio led{{.port = Status_LED_GPIO_Port, .pin = Status_LED_Pin}}; - const micras::hal::Gpio button{{.port = Button_GPIO_Port, .pin = Button_Pin}}; - led.write(button.read()); - - micras::hal::Pwm pwm{ - {.init_function = MX_TIM15_Init, .handle = &htim15, .timer_channel = TIM_CHANNEL_1, .inverted = false} - }; - pwm.set_duty_cycle(50.0F); - - micras::hal::PwmDma pwm_dma{{.init_function = MX_TIM8_Init, .handle = &htim8, .timer_channel = TIM_CHANNEL_1}}; - pwm_dma_compares.fill(static_cast(pwm_dma.get_compare(50.0F))); - pwm_dma.start_dma(pwm_dma_compares); - - const micras::hal::Encoder encoder{ - {.init_function = MX_TIM5_Init, .handle = &htim5, .timer_channel = TIM_CHANNEL_ALL} - }; - led.write(encoder.get_counter() > 0); - - micras::hal::Spi spi{{ - .init_function = MX_SPI3_Init, - .handle = &hspi3, - .cs_gpio = {.port = SPI_CSn_GPIO_Port, .pin = SPI_CSn_Pin}, - .timeout = 2, - .clock_polarity = SPI_POLARITY_HIGH, - .clock_phase = SPI_PHASE_2EDGE, - }}; - spi.start_transfer(spi_transmitted, spi_received); - - micras::hal::UartDma uart{{.init_function = MX_UART4_Init, .handle = &huart4}}; - uart.start_rx(uart_ring); - uart.start_tx(spi_received); +static uint32_t vdda_mv(uint16_t vrefint_reading) { + return __LL_ADC_CALC_VREFANALOG_VOLTAGE(static_cast(vrefint_reading), LL_ADC_RESOLUTION_12B); } int main() { - constexpr std::array peripheral_inits{MX_DMA_Init, MX_CRC_Init}; - - micras::hal::Mcu::init({ - .clock_init = SystemClock_Config, - .peripheral_clock_init = nullptr, - .peripheral_inits = peripheral_inits, - .cpu_frequency_boost = false, + micras::reference::run_bench({ + .uart_init = MX_UART4_Init, + .uart = &huart4, + .vdda_mv = vdda_mv, }); - - if (not micras::hal::Mcu::was_reset_by_watchdog()) { - save_before_reset(); - expire_watchdog(); - } - - watchdog_passed = true; - storage_passed = restored_after_reset(); - timer_passed = check_timer_clock(); - crc_passed = check_crc(); - fmac_passed = check_fmac(); - adc_passed = check_adc(); - bench_done = true; - - if (start_pin_drivers) { - run_pin_drivers(); - } - - while (true) { - bench_done = true; - } } From 8bbf81a5c8d02d722847d8c7f3448f31d32028f0 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 13:24:49 -0300 Subject: [PATCH 550/561] =?UTF-8?q?=E2=9C=A8=20Build=20and=20link=20a=20be?= =?UTF-8?q?nch=20on=20a=20reference=20STM32G474=20project?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit reference/g4 is the G4 counterpart of reference/h7: the same peripherals on the STM32G474RETx, the part of the old Micras board, at 170 MHz from the internal oscillator through the PLL, with USART3 as its UART. The Dockerfile generates its Cube files with the G4 image of STM32CubeMX, the reference-g4 preset builds it, and the CI builds, links and lints both references in one job per family, each with its generated tree cached on its .ioc and the Dockerfile. --- .docker/Dockerfile | 20 ++ .github/workflows/ci.yaml | 30 ++- CMakePresets.json | 14 ++ reference/g4/.gitignore | 6 + reference/g4/CMakeLists.txt | 16 ++ reference/g4/bench.cpp | 22 ++ reference/g4/reference.ioc | 391 ++++++++++++++++++++++++++++++++++++ 7 files changed, 489 insertions(+), 10 deletions(-) create mode 100644 reference/g4/.gitignore create mode 100644 reference/g4/CMakeLists.txt create mode 100644 reference/g4/bench.cpp create mode 100644 reference/g4/reference.ioc diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 4f43eb5..8726d41 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -63,6 +63,26 @@ FROM scratch AS generated-h7 COPY --from=generator-h7 /project/reference/h7 / +FROM thunderatz/stm32cubemx:6.18.1-g4 AS generator-g4 + +WORKDIR /project +COPY reference/g4/reference.ioc reference/g4/ + +# The same generation as the H7 reference's, with the STM32CubeMX image that carries the G4 package +RUN --network=none < /tmp/cube_script + xvfb-run --auto-servernum "${CUBE_PATH}/STM32CubeMX" -q /tmp/cube_script + rm /tmp/cube_script + test -f reference/g4/cmake/stm32cubemx/CMakeLists.txt || { echo "STM32CubeMX failed to generate the project" >&2; exit 1; } + rm -f reference/g4/CMakeLists.txt reference/g4/CMakePresets.json reference/g4/reference.ioc +EOT + +# Only the generated files, can be replaced by already generated ones with --build-context generated-g4= +FROM scratch AS generated-g4 + +COPY --from=generator-g4 /project/reference/g4 / + ########################################################## # ARM toolchain for the reference projects and benches # ########################################################## diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 81f769d..823f714 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -63,10 +63,18 @@ jobs: if: always() run: docker rm --force host - stm32-h7: - name: ๐Ÿ”ฉ STM32H7 reference and bench + stm32: + name: ๐Ÿ”ฉ ${{ matrix.name }} reference and bench runs-on: ubuntu-latest timeout-minutes: 45 + strategy: + fail-fast: false + matrix: + include: + - family: h7 + name: STM32H7 + - family: g4 + name: STM32G4 steps: - name: ๐Ÿ”€ Checkout uses: actions/checkout@v7 @@ -75,22 +83,24 @@ jobs: id: cube uses: actions/cache/restore@v6 with: - path: build/generated-h7 - key: cube-h7-${{ hashFiles('reference/h7/reference.ioc', '.docker/Dockerfile') }} + path: build/generated-${{ matrix.family }} + key: cube-${{ matrix.family }}-${{ hashFiles(format('reference/{0}/reference.ioc', matrix.family), '.docker/Dockerfile') }} - name: โš™๏ธ Generate the Cube files if: steps.cube.outputs.cache-hit != 'true' - run: docker build --file .docker/Dockerfile --target generated-h7 --output type=local,dest=build/generated-h7 . + run: > + docker build --file .docker/Dockerfile --target generated-${{ matrix.family }} + --output type=local,dest=build/generated-${{ matrix.family }} . - name: ๐Ÿ—ƒ๏ธ Save the generated Cube files if: steps.cube.outputs.cache-hit != 'true' uses: actions/cache/save@v6 with: - path: build/generated-h7 + path: build/generated-${{ matrix.family }} key: ${{ steps.cube.outputs.cache-primary-key }} - name: ๐Ÿ“‚ Place the Cube files in the reference project - run: cp -r build/generated-h7/. reference/h7/ + run: cp -r build/generated-${{ matrix.family }}/. reference/${{ matrix.family }}/ - name: ๐Ÿ‹ Build the ARM image run: docker build --file .docker/Dockerfile --target stm32 --tag micras-lib:stm32 . @@ -101,13 +111,13 @@ jobs: --volume "$PWD:/project" --workdir /project micras-lib:stm32 sleep infinity - name: โš™๏ธ Configure the reference project - run: docker exec stm32 cmake --preset reference-h7 + run: docker exec stm32 cmake --preset reference-${{ matrix.family }} - name: ๐Ÿ”จ Build the reference and its bench - run: docker exec stm32 cmake --build --preset reference-h7 --parallel + run: docker exec stm32 cmake --build --preset reference-${{ matrix.family }} --parallel - name: ๐Ÿšจ Lint the stm32 backend and the bench - run: docker exec stm32 cmake --build --preset reference-h7 --target lint + run: docker exec stm32 cmake --build --preset reference-${{ matrix.family }} --target lint - name: ๐Ÿงน Stop the ARM container if: always() diff --git a/CMakePresets.json b/CMakePresets.json index afc1bad..a3722d3 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -46,6 +46,16 @@ "MICRAS_LIB_REFERENCE": "h7", "CMAKE_BUILD_TYPE": "Release" } + }, + { + "name": "reference-g4", + "displayName": "Reference STM32G474 project and its bench program", + "generator": "Unix Makefiles", + "binaryDir": "${sourceDir}/build/${presetName}", + "cacheVariables": { + "MICRAS_LIB_REFERENCE": "g4", + "CMAKE_BUILD_TYPE": "Release" + } } ], "buildPresets": [ @@ -60,6 +70,10 @@ { "name": "reference-h7", "configurePreset": "reference-h7" + }, + { + "name": "reference-g4", + "configurePreset": "reference-g4" } ], "testPresets": [ diff --git a/reference/g4/.gitignore b/reference/g4/.gitignore new file mode 100644 index 0000000..765b11c --- /dev/null +++ b/reference/g4/.gitignore @@ -0,0 +1,6 @@ +# The Cube files STM32CubeMX generates from reference.ioc +* +!.gitignore +!CMakeLists.txt +!bench.cpp +!reference.ioc diff --git a/reference/g4/CMakeLists.txt b/reference/g4/CMakeLists.txt new file mode 100644 index 0000000..2688a2a --- /dev/null +++ b/reference/g4/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.28...4.4) + +############################################################################### +## Reference STM32G474 project +############################################################################### + +# The smallest CubeMX project that exercises every peripheral micras-lib uses, built and linked with +# its bench program the way a consumer of the stm32 backend builds its firmware. What every family's +# reference shares is in ../common. + +set(MICRAS_REFERENCE_FAMILY g4) +include(${CMAKE_CURRENT_LIST_DIR}/../common/toolchain.cmake) + +project(micras_lib_reference_g4 C CXX ASM) + +include(${CMAKE_CURRENT_LIST_DIR}/../common/reference.cmake) diff --git a/reference/g4/bench.cpp b/reference/g4/bench.cpp new file mode 100644 index 0000000..e628aa8 --- /dev/null +++ b/reference/g4/bench.cpp @@ -0,0 +1,22 @@ +/** + * @file + */ + +#include + +#include +#include + +#include "bench.hpp" + +static uint32_t vdda_mv(uint16_t vrefint_reading) { + return __LL_ADC_CALC_VREFANALOG_VOLTAGE(static_cast(vrefint_reading), LL_ADC_RESOLUTION_12B); +} + +int main() { + micras::reference::run_bench({ + .uart_init = MX_USART3_UART_Init, + .uart = &huart3, + .vdda_mv = vdda_mv, + }); +} diff --git a/reference/g4/reference.ioc b/reference/g4/reference.ioc new file mode 100644 index 0000000..79c21eb --- /dev/null +++ b/reference/g4/reference.ioc @@ -0,0 +1,391 @@ +#MicroXplorer Configuration settings - do not modify +ADC3.Channel-0\#ChannelRegularConversion=ADC_CHANNEL_VREFINT +ADC3.Channel-1\#ChannelRegularConversion=ADC_CHANNEL_VBAT +ADC3.ClockPrescaler=ADC_CLOCK_ASYNC_DIV4 +ADC3.CommonPathInternal=ADC_CHANNEL_VREFINT|null|ADC_CHANNEL_VBAT|null +ADC3.ContinuousConvMode=ENABLE +ADC3.DMAContinuousRequests=ENABLE +ADC3.IPParameters=Channel-0\#ChannelRegularConversion,Rank-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,Rank-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,OffsetNumber-1\#ChannelRegularConversion,NbrOfConversion,NbrOfConversionFlag,ScanConvMode,ClockPrescaler,ContinuousConvMode,DMAContinuousRequests,Overrun,master,CommonPathInternal +ADC3.NbrOfConversion=2 +ADC3.NbrOfConversionFlag=1 +ADC3.OffsetNumber-0\#ChannelRegularConversion=ADC_OFFSET_NONE +ADC3.OffsetNumber-1\#ChannelRegularConversion=ADC_OFFSET_NONE +ADC3.Overrun=ADC_OVR_DATA_OVERWRITTEN +ADC3.Rank-0\#ChannelRegularConversion=1 +ADC3.Rank-1\#ChannelRegularConversion=2 +ADC3.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_640CYCLES_5 +ADC3.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_640CYCLES_5 +ADC3.ScanConvMode=ADC_SCAN_ENABLE +ADC3.master=1 +CAD.formats= +CAD.pinconfig= +CAD.provider= +CRC.CRCLength=CRC_POLYLENGTH_8B +CRC.DefaultInitValueUse=DEFAULT_INIT_VALUE_DISABLE +CRC.DefaultPolynomialUse=DEFAULT_POLYNOMIAL_DISABLE +CRC.GeneratingPolynomial=X4+X3+X2+X0 +CRC.IPParameters=DefaultPolynomialUse,DefaultInitValueUse,CRCLength,GeneratingPolynomial,InitValue +CRC.InitValue=0xC4 +Dma.ADC3.3.Direction=DMA_PERIPH_TO_MEMORY +Dma.ADC3.3.EventEnable=DISABLE +Dma.ADC3.3.Instance=DMA1_Channel4 +Dma.ADC3.3.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.ADC3.3.MemInc=DMA_MINC_ENABLE +Dma.ADC3.3.Mode=DMA_CIRCULAR +Dma.ADC3.3.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.ADC3.3.PeriphInc=DMA_PINC_DISABLE +Dma.ADC3.3.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.ADC3.3.Priority=DMA_PRIORITY_LOW +Dma.ADC3.3.RequestNumber=1 +Dma.ADC3.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.ADC3.3.SignalID=NONE +Dma.ADC3.3.SyncEnable=DISABLE +Dma.ADC3.3.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.ADC3.3.SyncRequestNumber=1 +Dma.ADC3.3.SyncSignalID=NONE +Dma.Request0=TIM8_CH1 +Dma.Request1=USART3_RX +Dma.Request2=USART3_TX +Dma.Request3=ADC3 +Dma.Request4=SPI3_RX +Dma.Request5=SPI3_TX +Dma.RequestsNb=6 +Dma.SPI3_RX.4.Direction=DMA_PERIPH_TO_MEMORY +Dma.SPI3_RX.4.EventEnable=DISABLE +Dma.SPI3_RX.4.Instance=DMA1_Channel5 +Dma.SPI3_RX.4.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.SPI3_RX.4.MemInc=DMA_MINC_ENABLE +Dma.SPI3_RX.4.Mode=DMA_NORMAL +Dma.SPI3_RX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.SPI3_RX.4.PeriphInc=DMA_PINC_DISABLE +Dma.SPI3_RX.4.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.SPI3_RX.4.Priority=DMA_PRIORITY_LOW +Dma.SPI3_RX.4.RequestNumber=1 +Dma.SPI3_RX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.SPI3_RX.4.SignalID=NONE +Dma.SPI3_RX.4.SyncEnable=DISABLE +Dma.SPI3_RX.4.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.SPI3_RX.4.SyncRequestNumber=1 +Dma.SPI3_RX.4.SyncSignalID=NONE +Dma.SPI3_TX.5.Direction=DMA_MEMORY_TO_PERIPH +Dma.SPI3_TX.5.EventEnable=DISABLE +Dma.SPI3_TX.5.Instance=DMA1_Channel6 +Dma.SPI3_TX.5.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.SPI3_TX.5.MemInc=DMA_MINC_ENABLE +Dma.SPI3_TX.5.Mode=DMA_NORMAL +Dma.SPI3_TX.5.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.SPI3_TX.5.PeriphInc=DMA_PINC_DISABLE +Dma.SPI3_TX.5.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.SPI3_TX.5.Priority=DMA_PRIORITY_LOW +Dma.SPI3_TX.5.RequestNumber=1 +Dma.SPI3_TX.5.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.SPI3_TX.5.SignalID=NONE +Dma.SPI3_TX.5.SyncEnable=DISABLE +Dma.SPI3_TX.5.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.SPI3_TX.5.SyncRequestNumber=1 +Dma.SPI3_TX.5.SyncSignalID=NONE +Dma.TIM8_CH1.0.Direction=DMA_MEMORY_TO_PERIPH +Dma.TIM8_CH1.0.EventEnable=DISABLE +Dma.TIM8_CH1.0.Instance=DMA1_Channel1 +Dma.TIM8_CH1.0.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.TIM8_CH1.0.MemInc=DMA_MINC_ENABLE +Dma.TIM8_CH1.0.Mode=DMA_NORMAL +Dma.TIM8_CH1.0.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.TIM8_CH1.0.PeriphInc=DMA_PINC_DISABLE +Dma.TIM8_CH1.0.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.TIM8_CH1.0.Priority=DMA_PRIORITY_LOW +Dma.TIM8_CH1.0.RequestNumber=1 +Dma.TIM8_CH1.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.TIM8_CH1.0.SignalID=NONE +Dma.TIM8_CH1.0.SyncEnable=DISABLE +Dma.TIM8_CH1.0.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.TIM8_CH1.0.SyncRequestNumber=1 +Dma.TIM8_CH1.0.SyncSignalID=NONE +Dma.USART3_RX.1.Direction=DMA_PERIPH_TO_MEMORY +Dma.USART3_RX.1.EventEnable=DISABLE +Dma.USART3_RX.1.Instance=DMA1_Channel2 +Dma.USART3_RX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART3_RX.1.MemInc=DMA_MINC_ENABLE +Dma.USART3_RX.1.Mode=DMA_CIRCULAR +Dma.USART3_RX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART3_RX.1.PeriphInc=DMA_PINC_DISABLE +Dma.USART3_RX.1.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.USART3_RX.1.Priority=DMA_PRIORITY_LOW +Dma.USART3_RX.1.RequestNumber=1 +Dma.USART3_RX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.USART3_RX.1.SignalID=NONE +Dma.USART3_RX.1.SyncEnable=DISABLE +Dma.USART3_RX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.USART3_RX.1.SyncRequestNumber=1 +Dma.USART3_RX.1.SyncSignalID=NONE +Dma.USART3_TX.2.Direction=DMA_MEMORY_TO_PERIPH +Dma.USART3_TX.2.EventEnable=DISABLE +Dma.USART3_TX.2.Instance=DMA1_Channel3 +Dma.USART3_TX.2.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART3_TX.2.MemInc=DMA_MINC_ENABLE +Dma.USART3_TX.2.Mode=DMA_NORMAL +Dma.USART3_TX.2.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART3_TX.2.PeriphInc=DMA_PINC_DISABLE +Dma.USART3_TX.2.Polarity=HAL_DMAMUX_REQ_GEN_RISING +Dma.USART3_TX.2.Priority=DMA_PRIORITY_LOW +Dma.USART3_TX.2.RequestNumber=1 +Dma.USART3_TX.2.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber +Dma.USART3_TX.2.SignalID=NONE +Dma.USART3_TX.2.SyncEnable=DISABLE +Dma.USART3_TX.2.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT +Dma.USART3_TX.2.SyncRequestNumber=1 +Dma.USART3_TX.2.SyncSignalID=NONE +File.Version=6 +GPIO.groupedBy=Group By Peripherals +KeepUserPlacement=false +Mcu.CPN=STM32G474RET6 +Mcu.Family=STM32G4 +Mcu.IP0=ADC3 +Mcu.IP1=CRC +Mcu.IP10=TIM5 +Mcu.IP11=TIM8 +Mcu.IP12=TIM15 +Mcu.IP13=USART3 +Mcu.IP2=DMA +Mcu.IP3=FMAC +Mcu.IP4=IWDG +Mcu.IP5=NVIC +Mcu.IP6=RCC +Mcu.IP7=SPI3 +Mcu.IP8=SYS +Mcu.IP9=TIM2 +Mcu.IPNb=14 +Mcu.Name=STM32G474R(B-C-E)Tx +Mcu.Package=LQFP64 +Mcu.Pin0=PA0 +Mcu.Pin1=PA1 +Mcu.Pin10=PC10 +Mcu.Pin11=PC11 +Mcu.Pin12=PC12 +Mcu.Pin13=PD2 +Mcu.Pin14=VP_ADC3_Vref_Input +Mcu.Pin15=VP_ADC3_Vbat_Input +Mcu.Pin16=VP_CRC_VS_CRC +Mcu.Pin17=VP_FMAC_VS_FMAC +Mcu.Pin18=VP_IWDG_VS_IWDG +Mcu.Pin19=VP_SYS_VS_Systick +Mcu.Pin2=PA2 +Mcu.Pin20=VP_SYS_VS_DBSignals +Mcu.Pin21=VP_TIM2_VS_ClockSourceINT +Mcu.Pin3=PA7 +Mcu.Pin4=PB10 +Mcu.Pin5=PB11 +Mcu.Pin6=PB12 +Mcu.Pin7=PC6 +Mcu.Pin8=PA13 +Mcu.Pin9=PA14 +Mcu.PinsNb=22 +Mcu.ThirdPartyNb=0 +Mcu.UserConstants= +Mcu.UserName=STM32G474RETx +MxCube.Version=6.18.1 +MxDb.Version=DB.6.0.181 +NVIC.ADC3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DMA1_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Channel6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.ForceEnableDMAVector=true +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 +NVIC.SPI3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +PA0.GPIOParameters=GPIO_PuPd,GPIO_Label +PA0.GPIO_Label=Encoder_A +PA0.GPIO_PuPd=GPIO_PULLUP +PA0.Locked=true +PA0.Signal=S_TIM5_CH1 +PA1.GPIOParameters=GPIO_PuPd,GPIO_Label +PA1.GPIO_Label=Encoder_B +PA1.GPIO_PuPd=GPIO_PULLUP +PA1.Locked=true +PA1.Signal=S_TIM5_CH2 +PA13.Locked=true +PA13.Mode=Serial_Wire +PA13.Signal=SYS_JTMS-SWDIO +PA14.Locked=true +PA14.Mode=Serial_Wire +PA14.Signal=SYS_JTCK-SWCLK +PA2.GPIOParameters=GPIO_Label +PA2.GPIO_Label=PWM +PA2.Locked=true +PA2.Signal=S_TIM15_CH1 +PA7.GPIOParameters=GPIO_PuPd,GPIO_Label +PA7.GPIO_Label=Button +PA7.GPIO_PuPd=GPIO_PULLUP +PA7.Locked=true +PA7.Signal=GPIO_Input +PB10.Locked=true +PB10.Mode=Asynchronous +PB10.Signal=USART3_TX +PB11.Locked=true +PB11.Mode=Asynchronous +PB11.Signal=USART3_RX +PB12.GPIOParameters=GPIO_Label +PB12.GPIO_Label=Status_LED +PB12.Locked=true +PB12.Signal=GPIO_Output +PC10.Locked=true +PC10.Mode=Full_Duplex_Master +PC10.Signal=SPI3_SCK +PC11.Locked=true +PC11.Mode=Full_Duplex_Master +PC11.Signal=SPI3_MISO +PC12.Locked=true +PC12.Mode=Full_Duplex_Master +PC12.Signal=SPI3_MOSI +PC6.GPIOParameters=GPIO_Label +PC6.GPIO_Label=PWM_DMA +PC6.Locked=true +PC6.Signal=S_TIM8_CH1 +PD2.GPIOParameters=PinState,GPIO_Label +PD2.GPIO_Label=SPI_CSn +PD2.Locked=true +PD2.PinState=GPIO_PIN_SET +PD2.Signal=GPIO_Output +PinOutPanel.RotationAngle=0 +ProjectManager.AskForMigrate=true +ProjectManager.BackupPrevious=false +ProjectManager.CompilerLinker=GCC +ProjectManager.CompilerOptimize=6 +ProjectManager.ComputerToolchain=false +ProjectManager.CoupleFile=true +ProjectManager.CustomerFirmwarePackage= +ProjectManager.DefaultFWLocation=true +ProjectManager.DeletePrevious=true +ProjectManager.DeviceId=STM32G474RETx +ProjectManager.FirmwarePackage=STM32Cube FW_G4 V1.6.3 +ProjectManager.FreePins=false +ProjectManager.FreePinsContext= +ProjectManager.HalAssertFull=false +ProjectManager.HeapSize=0x1000 +ProjectManager.KeepUserCode=false +ProjectManager.LastFirmware=true +ProjectManager.LibraryCopy=1 +ProjectManager.MainLocation=Src +ProjectManager.NoMain=true +ProjectManager.PreviousToolchain= +ProjectManager.ProjectBuild=false +ProjectManager.ProjectFileName=reference.ioc +ProjectManager.ProjectName=reference +ProjectManager.ProjectStructure= +ProjectManager.RegisterCallBack= +ProjectManager.StackSize=0x4000 +ProjectManager.TargetToolchain=CMake +ProjectManager.ToolChainLocation= +ProjectManager.UAScriptAfterPath= +ProjectManager.UAScriptBeforePath= +ProjectManager.UnderRoot=false +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_SPI3_Init-SPI3-false-HAL-true,5-MX_TIM2_Init-TIM2-false-HAL-true,6-MX_TIM5_Init-TIM5-false-HAL-true,7-MX_USART3_UART_Init-USART3-false-HAL-true,8-MX_TIM8_Init-TIM8-false-HAL-true,9-MX_TIM15_Init-TIM15-false-HAL-true,10-MX_ADC3_Init-ADC3-false-HAL-true,11-MX_CRC_Init-CRC-false-HAL-true,12-MX_FMAC_Init-FMAC-false-HAL-true,13-MX_IWDG_Init-IWDG-false-HAL-true +RCC.ADC12Freq_Value=170000000 +RCC.ADC345Freq_Value=170000000 +RCC.AHBFreq_Value=170000000 +RCC.APB1Freq_Value=170000000 +RCC.APB1TimFreq_Value=170000000 +RCC.APB2Freq_Value=170000000 +RCC.APB2TimFreq_Value=170000000 +RCC.CRSFreq_Value=48000000 +RCC.CortexFreq_Value=170000000 +RCC.EXTERNAL_CLOCK_VALUE=12288000 +RCC.FCLKCortexFreq_Value=170000000 +RCC.FDCANFreq_Value=170000000 +RCC.FamilyName=M +RCC.HCLKFreq_Value=170000000 +RCC.HRTIM1Freq_Value=170000000 +RCC.HSE_VALUE=8000000 +RCC.HSI48_VALUE=48000000 +RCC.HSI_VALUE=16000000 +RCC.I2C1Freq_Value=170000000 +RCC.I2C2Freq_Value=170000000 +RCC.I2C3Freq_Value=170000000 +RCC.I2C4Freq_Value=170000000 +RCC.I2SFreq_Value=170000000 +RCC.IPParameters=ADC12Freq_Value,ADC345Freq_Value,AHBFreq_Value,APB1Freq_Value,APB1TimFreq_Value,APB2Freq_Value,APB2TimFreq_Value,CRSFreq_Value,CortexFreq_Value,EXTERNAL_CLOCK_VALUE,FCLKCortexFreq_Value,FDCANFreq_Value,FamilyName,HCLKFreq_Value,HRTIM1Freq_Value,HSE_VALUE,HSI48_VALUE,HSI_VALUE,I2C1Freq_Value,I2C2Freq_Value,I2C3Freq_Value,I2C4Freq_Value,I2SFreq_Value,LPTIM1Freq_Value,LPUART1Freq_Value,LSCOPinFreq_Value,LSE_VALUE,LSI_VALUE,MCO1PinFreq_Value,PLLM,PLLN,PLLPoutputFreq_Value,PLLQoutputFreq_Value,PLLRCLKFreq_Value,PWRFreq_Value,QSPIFreq_Value,RNGFreq_Value,SAI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,UART4Freq_Value,UART5Freq_Value,USART1Freq_Value,USART2Freq_Value,USART3Freq_Value,USBFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value +RCC.LPTIM1Freq_Value=170000000 +RCC.LPUART1Freq_Value=170000000 +RCC.LSCOPinFreq_Value=32000 +RCC.LSE_VALUE=32768 +RCC.LSI_VALUE=32000 +RCC.MCO1PinFreq_Value=16000000 +RCC.PLLM=RCC_PLLM_DIV4 +RCC.PLLN=85 +RCC.PLLPoutputFreq_Value=170000000 +RCC.PLLQoutputFreq_Value=170000000 +RCC.PLLRCLKFreq_Value=170000000 +RCC.PWRFreq_Value=170000000 +RCC.QSPIFreq_Value=170000000 +RCC.RNGFreq_Value=170000000 +RCC.SAI1Freq_Value=170000000 +RCC.SYSCLKFreq_VALUE=170000000 +RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.UART4Freq_Value=170000000 +RCC.UART5Freq_Value=170000000 +RCC.USART1Freq_Value=170000000 +RCC.USART2Freq_Value=170000000 +RCC.USART3Freq_Value=170000000 +RCC.USBFreq_Value=170000000 +RCC.VCOInputFreq_Value=4000000 +RCC.VCOOutputFreq_Value=340000000 +SH.S_TIM15_CH1.0=TIM15_CH1,PWM Generation1 CH1 +SH.S_TIM15_CH1.ConfNb=1 +SH.S_TIM5_CH1.0=TIM5_CH1,Encoder_Interface +SH.S_TIM5_CH1.ConfNb=1 +SH.S_TIM5_CH2.0=TIM5_CH2,Encoder_Interface +SH.S_TIM5_CH2.ConfNb=1 +SH.S_TIM8_CH1.0=TIM8_CH1,PWM Generation1 CH1 +SH.S_TIM8_CH1.ConfNb=1 +SPI3.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_64 +SPI3.CLKPhase=SPI_PHASE_2EDGE +SPI3.CLKPolarity=SPI_POLARITY_HIGH +SPI3.CalculateBaudRate=2.65625 MBits/s +SPI3.DataSize=SPI_DATASIZE_8BIT +SPI3.Direction=SPI_DIRECTION_2LINES +SPI3.IPParameters=VirtualType,Mode,Direction,BaudRatePrescaler,DataSize,CLKPhase,CLKPolarity,CalculateBaudRate +SPI3.Mode=SPI_MODE_MASTER +SPI3.VirtualType=VM_MASTER +TIM15.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM15.IPParameters=Channel-PWM Generation1 CH1,Prescaler,PeriodNoDither +TIM15.PeriodNoDither=999 +TIM15.Prescaler=169 +TIM2.IPParameters=Prescaler,Period +TIM2.Period=4294967295 +TIM2.Prescaler=0 +TIM5.EncoderMode=TIM_ENCODERMODE_TI12 +TIM5.IPParameters=EncoderMode +TIM8.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM8.IPParameters=Channel-PWM Generation1 CH1,Prescaler,PeriodNoDither +TIM8.PeriodNoDither=212 +TIM8.Prescaler=0 +USART3.IPParameters=VirtualMode-Asynchronous +USART3.VirtualMode-Asynchronous=VM_ASYNC +VP_ADC3_Vbat_Input.Mode=IN-Vbat +VP_ADC3_Vbat_Input.Signal=ADC3_Vbat_Input +VP_ADC3_Vref_Input.Mode=IN-Vrefint +VP_ADC3_Vref_Input.Signal=ADC3_Vref_Input +VP_CRC_VS_CRC.Mode=CRC_Activate +VP_CRC_VS_CRC.Signal=CRC_VS_CRC +VP_FMAC_VS_FMAC.Mode=FMAC_Activate +VP_FMAC_VS_FMAC.Signal=FMAC_VS_FMAC +VP_IWDG_VS_IWDG.Mode=IWDG_Activate +VP_IWDG_VS_IWDG.Signal=IWDG_VS_IWDG +VP_SYS_VS_DBSignals.Mode=DisableDeadBatterySignals +VP_SYS_VS_DBSignals.Signal=SYS_VS_DBSignals +VP_SYS_VS_Systick.Mode=SysTick +VP_SYS_VS_Systick.Signal=SYS_VS_Systick +VP_TIM2_VS_ClockSourceINT.Mode=Internal +VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT +board=custom From 52209177e8da3c1fa25cb9a6367d85afbc60e2c6 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 13:24:49 -0300 Subject: [PATCH 551/561] =?UTF-8?q?=F0=9F=93=9D=20Document=20the=20STM32?= =?UTF-8?q?=20families=20for=20their=20consumers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The README names what each family needs from a consumer, above all the flash region Storage takes on each, where the family code lives and how the chip macro check keeps it there, and both reference projects with their bench. --- README.md | 111 +++++++++++++++++++++++++++++++++++++----------------- 1 file changed, 76 insertions(+), 35 deletions(-) diff --git a/README.md b/README.md index 40610b4..969b7a4 100644 --- a/README.md +++ b/README.md @@ -3,8 +3,8 @@ The libraries of the [Micras](https://github.com/Team-Micras/MicrasFirmware) micromouse, for any robot that wants them: the core utilities, the navigation, the communication protocol, the hardware abstraction layer and the proxies of the devices behind it. The HAL has one set of headers and two -backends, selected by CMake: **stm32**, for the robot, built against the files STM32CubeMX generates, -and **host**, which runs the same code on a PC for tests and simulations. +backends, selected by CMake: **stm32**, for the robot, built against the files STM32CubeMX generates +for an STM32H7 or an STM32G4, and **host**, which runs the same code on a PC for tests and simulations. | Library | Target | What it holds | Needs | |---|---|---|---| @@ -51,7 +51,7 @@ micras_apply_warnings(robot WERROR ${ROBOT_WERROR}) | `MICRAS_LIB_HAL` | `ON` | Add `micras_hal` | | `MICRAS_LIB_PROXY` | `ON` | Add `micras_proxy`, which fetches `lsm6dsv-pid`; forced off without the HAL | | `MICRAS_LIB_HAL_BACKEND` | `stm32` | `stm32` or `host` | -| `MICRAS_LIB_STM32_FAMILY` | empty | `h7`; read from the board target's device macro when empty | +| `MICRAS_LIB_STM32_FAMILY` | empty | `h7` or `g4`; read from the board target's device macro when empty | | `MICRAS_LIB_CUBE_TARGET` | `stm32cubemx` | The board target the HAL links, on every backend | | `MICRAS_LIB_WARNINGS` | `ON` | Apply the project warnings to every library target | | `MICRAS_LIB_WERROR` | `OFF` | Treat them as errors | @@ -66,7 +66,7 @@ A consumer of the stm32 backend: 1. Defines the board target (`stm32cubemx` by default) **before** adding micras-lib. It carries `main.h`, the HAL and CMSIS include directories and the device macro that selects the family - (`STM32H725xx`, for instance). The vendor driver and application objects (`cube_app` and + (`STM32H725xx` or `STM32G474xx`, for instance). The vendor driver and application objects (`cube_app` and `STM32_Drivers` in the reference project) are linked into every executable: they hold the vector table, the interrupt handlers and the `MX_*_Init` functions. 2. Enables in `stm32xx_hal_conf.h` every module the headers name: ADC, CRC, FMAC, SPI, TIM, UART, @@ -79,41 +79,76 @@ A consumer of the stm32 backend: the buffers non-cacheable. 5. Configures the CRC unit for the AS5047U (polynomial 0x1D, 8 bits, initial value 0xC4) when it uses `RotarySensor`. -6. Keeps code out of the flash `Storage` uses: the upper half of the sectors on the STM32H7. +6. Keeps code out of the flash `Storage` uses, which the family names (see the table below): the upper + half of bank 1 on the STM32H7, bank 2 on the STM32G4. A sector of `Storage::Config` is the unit + the family erases, so a G4 storage spans as many 2 KB pages as its pool needs. -The code of the backend is in `micras_hal/stm32/src/`. Code that only one family runs goes to -`micras_hal/stm32//`, which CMake compiles for the detected family alone; the shared code -names no chip. +### Families -### Reference project and bench - -`reference/h7/` is the smallest STM32CubeMX project that exercises everything the HAL and the proxies -use, on the STM32H725RGVx and nothing of Micras: an ADC scanning the internal reference and the -battery channel into a circular DMA buffer, the CRC unit set up for the AS5047U, the FMAC, a full -duplex SPI master with DMA and a GPIO chip select, a PWM channel, a DMA-fed PWM channel, an encoder -timer, a free-running timer, a UART with DMA both ways, the independent watchdog, one input and one -output pin, and the internal oscillator through the PLL at 400 MHz. CubeMX generates no `main()` and -no callback registration. Its `CMakeLists.txt` glues the generated tree in as a consumer does, adds -micras-lib with `MICRAS_LIB_WERROR=ON`, and builds and links `micras_lib_bench_h7`, which proves the -link-time contract above. +| | `h7` | `g4` | +|---|---|---| +| Parts | STM32H7, the STM32H725 of Micras v1 and the upcoming projects | STM32G4, the STM32G474RET6 of the old board, Micras v0 | +| Selected by | `STM32H7` among the board target's definitions | `STM32G4` among the board target's definitions | +| Flash | 128 KB sectors programmed in 256-bit flash words | 2 KB pages programmed in 64-bit double words | +| `Storage` region | the upper half of bank 1: sectors 4 to 7, from 0x08080000, on a 1 MB STM32H725xG | bank 2 of a 512 KB part in its factory dual bank mode: 128 pages from 0x08040000; on any other layout (single bank, a 128 or 256 KB part) every flash operation fails | +| Erasing | around 2 s per sector, up to 4 s | around 20 ms per page | +| Caches | `Mcu::init` enables the instruction cache | nothing to enable, `HAL_Init` sets up the ART accelerator | +| Watchdog | `IWDG1`, frozen under the debugger | `IWDG`, frozen under the debugger | +| `cpu_frequency_boost` | reads the `CPUFREQ_BOOST` option byte of the STM32H72x/73x | always supported, the family has no such option byte | +| ADC calibration | offset, single ended | single ended | +| Timer clock | twice the APB clock when its prescaler (`RCC_APBx_DIV1`) is not 1 | twice the APB clock when its prescaler (`RCC_HCLK_DIV1`) is not 1 | +| Cycle counter | `DWT` unlocked through its lock access register, then started | `DWT` started, the Cortex-M4 has no lock | + +The chip-independent code of the backend is in `micras_hal/stm32/src/`, and it names no chip. What +differs between families is in `micras_hal/stm32//`, which CMake compiles for the selected +family alone: the flash driver (`src/flash.cpp`), the flash geometry the shared `flash.hpp` reads +(`include/micras/hal/family/flash.hpp`: `flash_word_bits`, `sector_size`, `storage_first_sector`, +`storage_sectors`), and the steps of the shared classes that differ, the functions of the internal +header `micras/hal/family.hpp` (`calibrate_adc`, `enable_caches`, `was_reset_by_watchdog`, +`freeze_watchdog_in_debug`, `is_cpu_frequency_supported`, `watchdog`, `timer_clock`, +`enable_cycle_counter`) in `src/family.cpp`. `scripts/check_no_chip_macros.sh` fails when a +conditional directive in the shared code (`micras_hal/include`, `micras_hal/stm32/src`, the proxies, +core, nav and comm) names a chip, and the CI runs it. + +### Reference projects and benches + +`reference/h7/` and `reference/g4/` are the smallest STM32CubeMX projects that exercise everything the +HAL and the proxies use, and nothing of Micras: an ADC scanning the internal reference and the battery +channel into a circular DMA buffer, the CRC unit set up for the AS5047U, the FMAC, a full duplex SPI +master with DMA and a GPIO chip select, a PWM channel, a DMA-fed PWM channel, an encoder timer, a +free-running timer, a UART with DMA both ways, the independent watchdog, one input and one output pin, +and the internal oscillator through the PLL. CubeMX generates no `main()` and no callback +registration. Each `CMakeLists.txt` glues the generated tree in as a consumer does +(`reference/common/toolchain.cmake` and `reference/common/reference.cmake`), adds micras-lib with +`MICRAS_LIB_WERROR=ON`, and builds and links `micras_lib_bench_`, which proves the link-time +contract above. + +| | `reference/h7` | `reference/g4` | +|---|---|---| +| Part | STM32H725RGVx | STM32G474RETx | +| Core clock | 400 MHz: HSI 64 MHz / 4 x 50 / 2, VOS1 | 170 MHz: HSI 16 MHz / 4 x 85 / 2, range 1 boost | +| UART | UART4 | USART3 | +| Supply | the direct SMPS supply of Micras v1 | the internal regulator | The Cube files are generated, never committed: ```bash -docker build --file .docker/Dockerfile --target generated-h7 --output type=local,dest=reference/h7 . -cmake --preset reference-h7 -cmake --build --preset reference-h7 +docker build --file .docker/Dockerfile --target generated-g4 --output type=local,dest=reference/g4 . +cmake --preset reference-g4 +cmake --build --preset reference-g4 ``` -The bench drives no pin, so it can be flashed on any board with the part whose supply is the direct -SMPS one the project configures, as on Micras v1. It writes each result into a `volatile` global for -the debugger or STM32CubeMonitor: +and the same with `h7`. The bench program is `reference/common/bench.cpp`; each family's `bench.cpp` +names the UART and converts the internal reference with its family's low-layer ADC header. It drives +no pin, so it can be flashed on any board with the part (for the H7, one whose supply is the direct +SMPS one the project configures, as on Micras v1; for the G4, the old Micras board). It writes each +result into a `volatile` global for the debugger or STM32CubeMonitor: | Global | Check | |---|---| | `storage_passed` | a `Storage` saved before a watchdog reset is restored after it | | `watchdog_passed` | a `Watchdog` left alone resets the chip, and the reset cause reads back | -| `timer_passed`, `timer_clock_error` | the free-running timer against the cycle counter, for the timer clock the RCC reports | +| `timer_passed`, `timer_clock_error` | the free-running timer against the cycle counter, for the timer clock the family reports | | `crc_passed`, `crc_value` | the CRC unit against the same CRC computed in software | | `fmac_passed`, `fmac_error` | `FmacFilter` against the same filter computed in software | | `adc_passed`, `vdda_mv` | the internal reference converted over DMA, against its factory calibration | @@ -129,8 +164,9 @@ A consumer of the host backend: 1. Defines its board target, a static library standing in for the Cube layer, before adding micras-lib, and sets `MICRAS_LIB_CUBE_TARGET` to it. It provides `main.h` (which includes `stm32_host.h`), the per-peripheral headers its configuration includes, `SystemCoreClock`, the - flash geometry macros (`FLASH_NB_32BITWORD_IN_FLASHWORD`, `FLASH_SECTOR_SIZE`, - `FLASH_SECTOR_TOTAL`), and the handles and `MX_*_Init` functions that fill the registers the host + flash geometry macros the host's `micras/hal/family/flash.hpp` reads + (`FLASH_NB_32BITWORD_IN_FLASHWORD`, `FLASH_SECTOR_SIZE`, `FLASH_SECTOR_TOTAL`, with the values of + the chip it stands for), and the handles and `MX_*_Init` functions that fill the registers the host `Pwm`, `Timer` and `Spi` read. It links `micras::hal`, and `micras_hal` links it back. 2. Keeps exceptions on if a thread of its own runs the program the HAL serves. 3. Resets `micras::hal::host::Board` and `micras::hal::host::Clock` between independent runs in one @@ -142,8 +178,10 @@ project under the same handle and pin names, with the values its CubeMX project ### Design The host backend implements every `micras_hal` class on a PC, private and static data members -included, and includes nothing but the HAL's headers and its own. A change to a shared header lands -with both implementations in one commit. +included, and includes nothing but the HAL's headers and its own. It also implements the family +functions of `micras/hal/family.hpp` (`host/src/family.cpp`: nothing to calibrate, cache or unlock, a +timer's clock is its kernel clock). A change to a shared header lands with every implementation in one +commit. - **Ports.** `Board` is a registry of ports (`GpioPort`, `PwmPort`, `PwmDmaPort`, `AdcPort`, `UartPort`, `EncoderPort`, `SpiPort`, `FmacPort`, `FlashPort`, `McuPort`) **keyed by the address of @@ -211,6 +249,7 @@ know the slot and nothing else, so the real `Imu` and `RotarySensor` proxies run | `host` | Debug, the host backend and the tests under the address and undefined behavior sanitizers | | `host-release` | Release, the host backend and the tests | | `reference-h7` | The reference STM32H725 project and its bench, with the ARM toolchain | +| `reference-g4` | The reference STM32G474 project and its bench, with the ARM toolchain | ```bash cmake --preset host @@ -226,14 +265,16 @@ route times and step counts, which an intended change of the planner regenerates `format`, `format_check`, `lint` and `lint_fix` use clang 22, found as `clang-format-22`, `clang-tidy-22`, `run-clang-tidy-22` and `clang-apply-replacements-22`; configuring the top level -fails without them. `lint` covers the host build; `cmake --build --preset reference-h7 --target lint` -covers the stm32 backend and the bench against the ARM headers. `docs` builds the Doxygen pages into +fails without them. `lint` covers the host build; `cmake --build --preset reference- --target +lint` covers the stm32 backend of that family and the bench against its ARM headers. +`scripts/check_no_chip_macros.sh` checks the shared code for chip macros. `docs` builds the Doxygen pages into `build//docs/`. `.docker/Dockerfile` has the image every CI job runs in (the `host` stage, shared byte for byte with -micras-simulation and MicrasFirmware), the CubeMX generation of the reference project, and the ARM -toolchain on top of the host stage (`stm32`). `.github/workflows/ci.yaml` builds, tests, checks the -format, lints and builds the docs on the host, and builds and lints the H7 reference and its bench. +micras-simulation and MicrasFirmware), the CubeMX generation of the reference projects +(`generated-h7`, `generated-g4`), and the ARM toolchain on top of the host stage (`stm32`). +`.github/workflows/ci.yaml` builds, tests, checks the format, lints, builds the docs and checks the +shared code for chip macros on the host, and builds and lints each reference and its bench. The code style is the one of every Micras repository: `.clang-format`, `.clang-tidy` (with `tests/.clang-tidy`), Doxygen on every declaration of a header, no comments in sources, American From 3d1e0559eefa5e333fb1fb2cddbe67ab8ac7c43b Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 13:39:27 -0300 Subject: [PATCH 552/561] =?UTF-8?q?=F0=9F=93=9D=20Document=20the=20watchdo?= =?UTF-8?q?g=20registers=20of=20the=20host=20backend?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_hal/host/include/stm32_host.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/micras_hal/host/include/stm32_host.h b/micras_hal/host/include/stm32_host.h index 1069b45..08ca705 100644 --- a/micras_hal/host/include/stm32_host.h +++ b/micras_hal/host/include/stm32_host.h @@ -292,9 +292,24 @@ struct CRC_HandleTypeDef { * same signatures on every backend. */ struct IWDG_TypeDef { + /** + * @brief Key register: starts, reloads and unlocks the watchdog. + */ uint32_t KR; + + /** + * @brief Prescaler register. + */ uint32_t PR; + + /** + * @brief Reload register. + */ uint32_t RLR; + + /** + * @brief Status register, with the prescaler and reload update flags. + */ uint32_t SR; }; From 3f051122fc9cc6c0937853838680d850e6d4b7a4 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 14:13:01 -0300 Subject: [PATCH 553/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Derive=20the=20STM?= =?UTF-8?q?32H7=20storage=20address=20from=20the=20family=20flash=20header?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The storage region's address follows storage_first_sector and sector_size, as on the G4, so the family header is the single source of the region the driver reads, writes and erases. --- micras_hal/stm32/h7/README.md | 3 ++- micras_hal/stm32/h7/src/flash.cpp | 4 +--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/micras_hal/stm32/h7/README.md b/micras_hal/stm32/h7/README.md index cc809d4..575d71c 100644 --- a/micras_hal/stm32/h7/README.md +++ b/micras_hal/stm32/h7/README.md @@ -10,6 +10,7 @@ target defines an STM32H7 device (or `MICRAS_LIB_STM32_FAMILY` is `h7`): the cycle counter is unlocked through the lock access register of the Cortex-M7 before it starts. - `src/flash.cpp`: the flash driver, sectors of 128 KB programmed in 256-bit flash words. - `include/micras/hal/family/flash.hpp`: the flash geometry the shared `flash.hpp` reads. The storage - region is the upper half of bank 1: sectors 4 to 7 of an STM32H725xG. + region is the upper half of bank 1: sectors 4 to 7 of an STM32H725xG, from 0x08080000. The flash + driver derives the region's address from these constants, as every family does. The code in `../src` is the same on every family and names no chip. diff --git a/micras_hal/stm32/h7/src/flash.cpp b/micras_hal/stm32/h7/src/flash.cpp index 1be2c47..ca37d29 100644 --- a/micras_hal/stm32/h7/src/flash.cpp +++ b/micras_hal/stm32/h7/src/flash.cpp @@ -16,10 +16,8 @@ namespace micras::hal { /** * @brief First address of the flash memory region reserved for data storage. - * - * @note Needs to be here because it is not defined at compile time. */ -static const uint32_t base_address = FLASH_BASE + FLASH_SIZE / 2; +static constexpr uint32_t base_address{FLASH_BASE + (family::storage_first_sector * family::sector_size)}; /** * @brief Index of the first sector of the flash memory region reserved for data storage. From b7cdd333b8140309d011e2ee5eca3546642d832d Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 14:13:01 -0300 Subject: [PATCH 554/561] =?UTF-8?q?=F0=9F=94=A5=20Drop=20the=20G4=20data?= =?UTF-8?q?=20cache=20flush=20that=20HAL=5FFLASH=5FProgram=20already=20doe?= =?UTF-8?q?s?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The G4 HAL flushes the flash caches at the end of every programming operation. --- micras_hal/stm32/g4/src/flash.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/micras_hal/stm32/g4/src/flash.cpp b/micras_hal/stm32/g4/src/flash.cpp index 0bcaf25..27a61c7 100644 --- a/micras_hal/stm32/g4/src/flash.cpp +++ b/micras_hal/stm32/g4/src/flash.cpp @@ -29,16 +29,6 @@ static bool is_storage_available() { return (FLASH->OPTR & FLASH_OPTR_DBANK) != 0 and FLASH_PAGE_NB == family::pages_per_bank; } -static void flush_data_cache() { - if ((FLASH->ACR & FLASH_ACR_DCEN) == 0) { - return; - } - - __HAL_FLASH_DATA_CACHE_DISABLE(); - __HAL_FLASH_DATA_CACHE_RESET(); - __HAL_FLASH_DATA_CACHE_ENABLE(); -} - FlashWord::FlashWord(std::span data) { const auto data_address = std::bit_cast(data.data()); @@ -108,7 +98,6 @@ Flash::Status Flash::write(uint32_t address, std::span data) { } HAL_FLASH_Lock(); - flush_data_cache(); return status; } From dc2d3d8ed522ccefc7403e1f4814bdde7f7adebf Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 14:23:41 -0300 Subject: [PATCH 555/561] =?UTF-8?q?=E2=9C=8F=EF=B8=8F=20Spell=20traveled?= =?UTF-8?q?=20the=20American=20way=20in=20the=20mission?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- micras_nav/include/micras/nav/impl/mission.tpp | 14 +++++++------- micras_nav/include/micras/nav/mission.hpp | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/micras_nav/include/micras/nav/impl/mission.tpp b/micras_nav/include/micras/nav/impl/mission.tpp index 70aee4a..4d3e6dc 100644 --- a/micras_nav/include/micras/nav/impl/mission.tpp +++ b/micras_nav/include/micras/nav/impl/mission.tpp @@ -269,15 +269,15 @@ TMission::Status TMission::update( if (this->watching_front and watched != nullptr and watched->kind == SegmentKind::STRAIGHT and watched->start.position.distance(this->get_entry_pose(this->watched_cell).position) < std::max(commit_distance, 0.0F) + watch_tolerance) { - const float travelled = watched->start.position.distance(this->get_entry_pose(this->watched_cell).position) + - this->executor.get_reference().distance; + const float traveled = watched->start.position.distance(this->get_entry_pose(this->watched_cell).position) + + this->executor.get_reference().distance; const WallState wall = this->maze.get_wall(this->watched_cell); if (wall == WallState::NO_WALL) { this->watching_front = false; - } else if (wall == WallState::WALL or travelled >= commit_distance) { - this->divert_to_center(travelled); + } else if (wall == WallState::WALL or traveled >= commit_distance) { + this->divert_to_center(traveled); } } @@ -576,10 +576,10 @@ bool TMission::finish_at_entry() { } template -void TMission::divert_to_center(float travelled) { +void TMission::divert_to_center(float traveled) { const float cell_size = this->dynamics.get_model().maze.cell_size; const Reference& current = this->executor.get_reference(); - const float remaining = cell_size / 2.0F - travelled; + const float remaining = cell_size / 2.0F - traveled; this->watching_front = false; @@ -592,7 +592,7 @@ void TMission::divert_to_center(float travelled) { Move move{}; move.add(make_segment( SegmentKind::STRAIGHT, remaining, - this->get_entry_pose(this->watched_cell).compose({.position = {.x = travelled, .y = 0.0F}, .orientation = 0.0F}) + this->get_entry_pose(this->watched_cell).compose({.position = {.x = traveled, .y = 0.0F}, .orientation = 0.0F}) )); if (this->maze.get_wall(this->watched_cell) == WallState::WALL) { diff --git a/micras_nav/include/micras/nav/mission.hpp b/micras_nav/include/micras/nav/mission.hpp index ab0391c..e9465cf 100644 --- a/micras_nav/include/micras/nav/mission.hpp +++ b/micras_nav/include/micras/nav/mission.hpp @@ -323,9 +323,9 @@ class TMission { /** * @brief Stop at the center of the cell being crossed instead of going through it. * - * @param travelled The distance the reference covered since the robot crossed into the cell. + * @param traveled The distance the reference covered since the robot crossed into the cell. */ - void divert_to_center(float travelled); + void divert_to_center(float traveled); /** * @brief Get the distance into a cell of the last point from which the robot, crossing it at From 5782dabe1961f810e8a99fb5a623e1b944748ae0 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 15:28:16 -0300 Subject: [PATCH 556/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Declare=20the=20ST?= =?UTF-8?q?M32G4=20storage=20checks=20in=20the=20family=20flash=20header?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The rounding to double words and the check of the part's layout are facts of the family's flash, so the family header documents them; the driver's remaining file-local constants live in an anonymous namespace, which the documentation does not extract, as the host backend's do. --- .../g4/include/micras/hal/family/flash.hpp | 21 ++++++++++++++++++ micras_hal/stm32/g4/src/flash.cpp | 22 +++++++------------ 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/micras_hal/stm32/g4/include/micras/hal/family/flash.hpp b/micras_hal/stm32/g4/include/micras/hal/family/flash.hpp index eef578e..93d3a16 100644 --- a/micras_hal/stm32/g4/include/micras/hal/family/flash.hpp +++ b/micras_hal/stm32/g4/include/micras/hal/family/flash.hpp @@ -41,6 +41,27 @@ inline constexpr uint16_t storage_first_sector{pages_per_bank}; * @brief Number of pages reserved for data storage, the whole of bank 2. */ inline constexpr uint16_t storage_sectors{pages_per_bank}; + +/** + * @brief Round a number of bytes up to a whole number of the double words the flash memory programs. + * + * @param size Number of bytes to round up. + * @return Number of bytes the data occupies in the flash memory. + */ +constexpr uint32_t align_size(uint32_t size) { + constexpr uint32_t word_size{flash_word_bits / 8U}; + return (size + word_size - 1) / word_size * word_size; +} + +/** + * @brief Check that the part has the layout this header describes: dual bank mode, with banks of + * pages_per_bank pages. + * + * @return Whether the storage region exists on this part. + */ +inline bool is_storage_available() { + return (FLASH->OPTR & FLASH_OPTR_DBANK) != 0 and FLASH_PAGE_NB == pages_per_bank; +} } // namespace micras::hal::family #endif // MICRAS_HAL_FAMILY_FLASH_HPP diff --git a/micras_hal/stm32/g4/src/flash.cpp b/micras_hal/stm32/g4/src/flash.cpp index 27a61c7..741e748 100644 --- a/micras_hal/stm32/g4/src/flash.cpp +++ b/micras_hal/stm32/g4/src/flash.cpp @@ -15,19 +15,13 @@ #include "micras/hal/flash.hpp" namespace micras::hal { -static constexpr uint32_t base_address{FLASH_BASE + (family::storage_first_sector * family::sector_size)}; - -static constexpr uint32_t erase_complete{0xFFFFFFFFU}; - static_assert(FlashWord::size == sizeof(uint64_t)); -static constexpr uint32_t align_size(uint32_t size) { - return (size + FlashWord::size - 1) / FlashWord::size * FlashWord::size; -} +namespace { +constexpr uint32_t base_address{FLASH_BASE + (family::storage_first_sector * family::sector_size)}; -static bool is_storage_available() { - return (FLASH->OPTR & FLASH_OPTR_DBANK) != 0 and FLASH_PAGE_NB == family::pages_per_bank; -} +constexpr uint32_t erase_complete{0xFFFFFFFFU}; +} // namespace FlashWord::FlashWord(std::span data) { const auto data_address = std::bit_cast(data.data()); @@ -54,7 +48,7 @@ bool FlashWord::is_padded() const { } std::span Flash::read(uint32_t address, uint32_t size) { - if (address > total_size or size > total_size - address or not is_storage_available()) { + if (address > total_size or size > total_size - address or not family::is_storage_available()) { return {}; } @@ -74,11 +68,11 @@ Flash::Status Flash::write(uint32_t address, std::span data) { return Status::MISALIGNED; } - if (address > total_size or align_size(data.size()) > total_size - address) { + if (address > total_size or family::align_size(data.size()) > total_size - address) { return Status::OUT_OF_BOUNDS; } - if (not is_storage_available() or HAL_FLASH_Unlock() != HAL_OK) { + if (not family::is_storage_available() or HAL_FLASH_Unlock() != HAL_OK) { return Status::ERROR; } @@ -122,7 +116,7 @@ Flash::Status Flash::erase_sectors(uint16_t start_sector, uint16_t number_of_sec .NbPages = number_of_sectors, }; - if (not is_storage_available() or HAL_FLASH_Unlock() != HAL_OK) { + if (not family::is_storage_available() or HAL_FLASH_Unlock() != HAL_OK) { return Status::ERROR; } From 4b83fbdbec727d891e40cdc3e4f9338ba46eb53c Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 15:28:23 -0300 Subject: [PATCH 557/561] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Keep=20the=20STM32?= =?UTF-8?q?H7=20debug=20unlock=20key=20in=20an=20anonymous=20namespace?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The family source carries no comments, so its file-local constant lives in an anonymous namespace, which the documentation does not extract, as the host backend's do. --- micras_hal/stm32/h7/src/family.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/micras_hal/stm32/h7/src/family.cpp b/micras_hal/stm32/h7/src/family.cpp index 8f2efd0..7ed5e5a 100644 --- a/micras_hal/stm32/h7/src/family.cpp +++ b/micras_hal/stm32/h7/src/family.cpp @@ -9,7 +9,9 @@ #include "micras/hal/family.hpp" namespace micras::hal::family { -static constexpr uint32_t software_lock_key{0xC5ACCE55}; +namespace { +constexpr uint32_t software_lock_key{0xC5ACCE55}; +} // namespace bool calibrate_adc(ADC_HandleTypeDef* handle) { return HAL_ADCEx_Calibration_Start(handle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) == HAL_OK; From d826b41627a9657431cb73b7754102a0e31852b5 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Mon, 28 Sep 2026 15:28:23 -0300 Subject: [PATCH 558/561] =?UTF-8?q?=F0=9F=93=9D=20Document=20the=20SPI=20m?= =?UTF-8?q?odes,=20the=20host=20SPI=20and=20FMAC=20types=20and=20the=20tes?= =?UTF-8?q?t=20board's=20pins?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Every declaration carries Doxygen: the SPI device's modes, the fields of the host SPI registers, init structure and handle, the FMAC handle states and the GPIO groups of the test board had none. --- .../include/micras/hal/host/spi_device.hpp | 15 +++++++++ micras_hal/host/include/stm32_host.h | 32 +++++++++++++++++-- tests/support/host_board/main.h | 9 ++++++ 3 files changed, 54 insertions(+), 2 deletions(-) diff --git a/micras_hal/host/include/micras/hal/host/spi_device.hpp b/micras_hal/host/include/micras/hal/host/spi_device.hpp index 20b86ea..cc52f2c 100644 --- a/micras_hal/host/include/micras/hal/host/spi_device.hpp +++ b/micras_hal/host/include/micras/hal/host/spi_device.hpp @@ -30,9 +30,24 @@ class SpiDevice { * @brief SPI mode: the clock polarity in the high bit, the clock phase in the low one. */ enum class Mode : uint8_t { + /** + * @brief The clock idles low and the data is sampled on its first edge, the rising one. + */ MODE_0 = 0, + + /** + * @brief The clock idles low and the data is sampled on its second edge, the falling one. + */ MODE_1 = 1, + + /** + * @brief The clock idles high and the data is sampled on its first edge, the falling one. + */ MODE_2 = 2, + + /** + * @brief The clock idles high and the data is sampled on its second edge, the rising one. + */ MODE_3 = 3, }; diff --git a/micras_hal/host/include/stm32_host.h b/micras_hal/host/include/stm32_host.h index 08ca705..b3554e9 100644 --- a/micras_hal/host/include/stm32_host.h +++ b/micras_hal/host/include/stm32_host.h @@ -174,6 +174,9 @@ inline constexpr uint32_t SPI_BAUDRATEPRESCALER_256{0x70000000U}; * rate is that divided by the prescaler. */ struct SPI_TypeDef { + /** + * @brief Frequency of the bus's kernel clock, in hertz. + */ uint32_t kernel_clock; }; @@ -181,8 +184,19 @@ struct SPI_TypeDef { * @brief Configuration of an SPI bus, as the init function fills it. */ struct SPI_InitTypeDef { + /** + * @brief Level the clock idles at, SPI_POLARITY_LOW or SPI_POLARITY_HIGH. + */ uint32_t CLKPolarity; + + /** + * @brief Edge the data is sampled on, SPI_PHASE_1EDGE or SPI_PHASE_2EDGE. + */ uint32_t CLKPhase; + + /** + * @brief Divider of the kernel clock that gives the bit rate, SPI_BAUDRATEPRESCALER_2 to SPI_BAUDRATEPRESCALER_256. + */ uint32_t BaudRatePrescaler; }; @@ -198,8 +212,19 @@ enum HAL_SPI_StateTypeDef : uint8_t { * @brief An SPI handle. */ struct SPI_HandleTypeDef { - SPI_TypeDef* Instance; - SPI_InitTypeDef Init; + /** + * @brief Registers of the bus. + */ + SPI_TypeDef* Instance; + + /** + * @brief Configuration of the bus. + */ + SPI_InitTypeDef Init; + + /** + * @brief State of the handle. + */ HAL_SPI_StateTypeDef State; }; @@ -317,6 +342,9 @@ struct IWDG_TypeDef { * FMAC *****************************************/ +/** + * @brief States of the filter accelerator's handle. + */ ///@{ inline constexpr uint32_t HAL_FMAC_STATE_RESET{0x00U}; inline constexpr uint32_t HAL_FMAC_STATE_READY{0x20U}; diff --git a/tests/support/host_board/main.h b/tests/support/host_board/main.h index e6336c8..4c35429 100644 --- a/tests/support/host_board/main.h +++ b/tests/support/host_board/main.h @@ -40,6 +40,9 @@ inline constexpr uint32_t FLASH_SECTOR_TOTAL{8U}; * GPIO ports and pins *****************************************/ +/** + * @brief The GPIO ports of the reference, one instance each. + */ ///@{ extern GPIO_TypeDef GPIOA_instance; extern GPIO_TypeDef GPIOB_instance; @@ -47,6 +50,9 @@ extern GPIO_TypeDef GPIOC_instance; extern GPIO_TypeDef GPIOD_instance; ///@} +/** + * @brief The CMSIS names of the GPIO ports, the addresses of their instances. + */ ///@{ #define GPIOA (&GPIOA_instance) #define GPIOB (&GPIOB_instance) @@ -54,6 +60,9 @@ extern GPIO_TypeDef GPIOD_instance; #define GPIOD (&GPIOD_instance) ///@} +/** + * @brief The pin labels of the reference's CubeMX project: each pin and its port. + */ ///@{ #define Encoder_A_Pin GPIO_PIN_0 #define Encoder_A_GPIO_Port GPIOA From e621cd62b040b14da44863b95fc4d9230587cbe9 Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sat, 3 Oct 2026 21:24:08 -0300 Subject: [PATCH 559/561] =?UTF-8?q?=F0=9F=90=9B=20Finish=20a=20UART=20tran?= =?UTF-8?q?smission=20without=20the=20UART=20interrupt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The HAL only returns the peripheral to ready from the transmission complete interrupt of the UART, which the board leaves disabled so that a reception error never aborts the circular transfer. A transfer is over once its DMA stream is, and the next one closes the state the HAL left open. --- micras_hal/include/micras/hal/uart_dma.hpp | 10 +++++++++- micras_hal/stm32/src/uart_dma.cpp | 6 +++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/micras_hal/include/micras/hal/uart_dma.hpp b/micras_hal/include/micras/hal/uart_dma.hpp index 1d73937..9e79d2f 100644 --- a/micras_hal/include/micras/hal/uart_dma.hpp +++ b/micras_hal/include/micras/hal/uart_dma.hpp @@ -18,6 +18,11 @@ namespace micras::hal { * counter against what was already taken. Nothing is done in an interrupt, there is no window * between stopping and restarting in which bytes are lost, and the latency is one control loop * iteration, which is far below anything the radio adds. + * + * Transmission is polled the same way. The HAL only returns the peripheral to ready from the + * transmission complete interrupt of the UART, which is left disabled so that a reception error + * never aborts the circular transfer. A transfer is over once its DMA stream is, and the next + * one closes the state the HAL left open. */ class UartDma { public: @@ -85,7 +90,10 @@ class UartDma { /** * @brief Check if a transfer is still running. * - * @return True if the peripheral is still sending, false otherwise. + * @note The last byte may still be leaving the shift register, which does not stop the next + * transfer from starting. + * + * @return True if the DMA is still feeding the peripheral, false otherwise. */ bool is_transmitting() const; diff --git a/micras_hal/stm32/src/uart_dma.cpp b/micras_hal/stm32/src/uart_dma.cpp index 853cf94..ff25a9d 100644 --- a/micras_hal/stm32/src/uart_dma.cpp +++ b/micras_hal/stm32/src/uart_dma.cpp @@ -63,11 +63,15 @@ bool UartDma::start_tx(std::span from) { return false; } + if (this->handle->gState == HAL_UART_STATE_BUSY_TX) { + HAL_UART_AbortTransmit(this->handle); + } + return HAL_UART_Transmit_DMA(this->handle, std::bit_cast(from.data()), from.size()) == HAL_OK; } bool UartDma::is_transmitting() const { - return this->handle->gState == HAL_UART_STATE_BUSY_TX; + return this->handle->gState == HAL_UART_STATE_BUSY_TX and this->handle->hdmatx->State == HAL_DMA_STATE_BUSY; } bool UartDma::was_initialized() const { From 901ebd91f47550b04c3bb97208154f05b2f813bf Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 4 Oct 2026 01:41:01 -0300 Subject: [PATCH 560/561] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Run=20the=20torque?= =?UTF-8?q?,=20battery=20and=20serial=20transfers=20without=20interrupts?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Their transfers are observed by polling, and the interrupt callbacks still work on a board that keeps them. The ADCs restart after an overrun or a stopped stream, a snapshot is never taken once an overrun misaligned the buffer, and the UART closes a finished DMA transfer before starting the next. --- README.md | 6 ++--- micras_hal/host/include/stm32_host.h | 9 ++++++++ micras_hal/host/src/family.cpp | 8 +++++++ micras_hal/include/micras/hal/adc_dma.hpp | 18 ++++++++------- micras_hal/include/micras/hal/family.hpp | 24 ++++++++++++++++++++ micras_hal/include/micras/hal/uart_dma.hpp | 26 +++++++++++++++------- micras_hal/stm32/g4/src/family.cpp | 8 +++++++ micras_hal/stm32/h7/src/family.cpp | 17 ++++++++++++++ micras_hal/stm32/src/adc_dma.cpp | 18 +++++++++++++-- micras_hal/stm32/src/uart_dma.cpp | 16 +++++++++++-- tests/hal/test_family.cpp | 7 ++++++ 11 files changed, 134 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index 969b7a4..c6927d2 100644 --- a/README.md +++ b/README.md @@ -104,9 +104,9 @@ differs between families is in `micras_hal/stm32//`, which CMake compile family alone: the flash driver (`src/flash.cpp`), the flash geometry the shared `flash.hpp` reads (`include/micras/hal/family/flash.hpp`: `flash_word_bits`, `sector_size`, `storage_first_sector`, `storage_sectors`), and the steps of the shared classes that differ, the functions of the internal -header `micras/hal/family.hpp` (`calibrate_adc`, `enable_caches`, `was_reset_by_watchdog`, -`freeze_watchdog_in_debug`, `is_cpu_frequency_supported`, `watchdog`, `timer_clock`, -`enable_cycle_counter`) in `src/family.cpp`. `scripts/check_no_chip_macros.sh` fails when a +header `micras/hal/family.hpp` (`calibrate_adc`, `is_dma_enabled`, `has_dma_finished`, +`enable_caches`, `was_reset_by_watchdog`, `freeze_watchdog_in_debug`, `is_cpu_frequency_supported`, +`watchdog`, `timer_clock`, `enable_cycle_counter`) in `src/family.cpp`. `scripts/check_no_chip_macros.sh` fails when a conditional directive in the shared code (`micras_hal/include`, `micras_hal/stm32/src`, the proxies, core, nav and comm) names a chip, and the CI runs it. diff --git a/micras_hal/host/include/stm32_host.h b/micras_hal/host/include/stm32_host.h index b3554e9..0559b5b 100644 --- a/micras_hal/host/include/stm32_host.h +++ b/micras_hal/host/include/stm32_host.h @@ -112,6 +112,15 @@ inline constexpr uint32_t TIM_COUNTERMODE_CENTERALIGNED1{0x1U << 5U}; ///@} +/***************************************** + * DMA + *****************************************/ + +/** + * @brief A DMA handle, whose transfers the host backend never lets stop. + */ +struct DMA_HandleTypeDef { }; + /***************************************** * ADC *****************************************/ diff --git a/micras_hal/host/src/family.cpp b/micras_hal/host/src/family.cpp index 579644c..a303573 100644 --- a/micras_hal/host/src/family.cpp +++ b/micras_hal/host/src/family.cpp @@ -12,6 +12,14 @@ bool calibrate_adc(ADC_HandleTypeDef* /*handle*/) { return true; } +bool is_dma_enabled(const DMA_HandleTypeDef* /*handle*/) { + return true; +} + +bool has_dma_finished(const DMA_HandleTypeDef* /*handle*/) { + return false; +} + void enable_caches() { } bool was_reset_by_watchdog() { diff --git a/micras_hal/include/micras/hal/adc_dma.hpp b/micras_hal/include/micras/hal/adc_dma.hpp index 22b1a30..c73b82a 100644 --- a/micras_hal/include/micras/hal/adc_dma.hpp +++ b/micras_hal/include/micras/hal/adc_dma.hpp @@ -114,11 +114,12 @@ class AdcDma { * @details After an overrun the converter stops requesting transfers until its overrun flag is * cleared, and clearing only the flag would leave one conversion missing and every following * value in the place of another channel. A restart begins the buffer again at its first - * channel. The error callback only marks the converter, since stopping and starting it waits on - * the hardware, and this restarts it outside the interrupt. A transfer error of the DMA calls - * the error callback too, and leaves the vendor HAL in an error state that only a new - * initialization clears and that turns every complete transfer into another error, so that - * state is cleared before the start. + * channel. A converter is restarted when the error callback or the snapshot marked it, and also + * when its overrun flag is set or its DMA stopped while the vendor HAL still holds the transfer + * as running, which is how an overrun or a transfer error shows when the interrupts of the + * converter and of its DMA are disabled. A transfer error leaves the vendor HAL in an error + * state that only a new initialization clears and that turns every complete transfer into + * another error, so that state is cleared before the start. * * @note To be called once per iteration by the owner of the converter. No sequence completes * until then, so read_snapshot reports none as new. @@ -161,9 +162,10 @@ class AdcDma { /** * @brief Count a complete sequence of a converter and copy its buffer to its snapshot. * - * @note To be called by the conversion complete callback only. Nothing is counted while the - * converter is stopped: after an overrun the vendor HAL clears the flag and the transfers go - * on out of line with the sequence until the converter is restarted. + * @note To be called by the conversion complete callback only. An overrun drops a conversion, + * and the transfers go on out of line with the sequence until the converter is restarted, so + * nothing is counted once one happened: the converter is marked stopped here when the overrun + * flag is set, without waiting for the overrun interrupt or for recover to notice it. * * @param handle Handle of the converter that completed a sequence. */ diff --git a/micras_hal/include/micras/hal/family.hpp b/micras_hal/include/micras/hal/family.hpp index 9a494b1..6563654 100644 --- a/micras_hal/include/micras/hal/family.hpp +++ b/micras_hal/include/micras/hal/family.hpp @@ -23,6 +23,30 @@ namespace micras::hal::family { */ bool calibrate_adc(ADC_HandleTypeDef* handle); +/** + * @brief Check whether the stream or channel of a DMA is enabled. + * + * @note A transfer error disables the stream or channel in hardware, so a transfer the vendor HAL + * still holds as busy and whose stream or channel is disabled has stopped, whether or not its + * interrupt is enabled to report it. + * + * @param handle Handle of the DMA, initialized. + * @return True if the stream or channel is enabled, false otherwise. + */ +bool is_dma_enabled(const DMA_HandleTypeDef* handle); + +/** + * @brief Check whether a transfer of a DMA in normal mode is over. + * + * @note A stream of the DMA controllers of the STM32H7 clears its enable bit once it moved its last + * item, while a channel of its BDMA or of the STM32G4 keeps it set and only its counter reaches zero. + * A transfer error disables either, which ends the transfer as well. + * + * @param handle Handle of the DMA, running a transfer started in normal mode. + * @return True if the DMA moved its last item or an error stopped it, false while items are left. + */ +bool has_dma_finished(const DMA_HandleTypeDef* handle); + /** * @brief Enable the caches the family runs its code from, before the vendor HAL is initialized. */ diff --git a/micras_hal/include/micras/hal/uart_dma.hpp b/micras_hal/include/micras/hal/uart_dma.hpp index 9e79d2f..1112368 100644 --- a/micras_hal/include/micras/hal/uart_dma.hpp +++ b/micras_hal/include/micras/hal/uart_dma.hpp @@ -14,15 +14,22 @@ namespace micras::hal { /** * @brief Class to handle a UART peripheral on STM32 microcontrollers using DMA. * - * @note Reception is a circular DMA that is never stopped, and is polled by comparing the transfer + * @note Reception is a circular DMA that only an error stops, and is polled by comparing the transfer * counter against what was already taken. Nothing is done in an interrupt, there is no window * between stopping and restarting in which bytes are lost, and the latency is one control loop - * iteration, which is far below anything the radio adds. + * iteration, which is far below anything the radio adds. A transfer error disables the receive + * DMA stream, which read finds whether or not the stream's interrupt is enabled, so that interrupt + * is not needed. * - * Transmission is polled the same way. The HAL only returns the peripheral to ready from the - * transmission complete interrupt of the UART, which is left disabled so that a reception error - * never aborts the circular transfer. A transfer is over once its DMA stream is, and the next - * one closes the state the HAL left open. + * Transmission is polled the same way, and needs no interrupt either. The HAL only returns the + * peripheral to ready from the transmission complete interrupt of the UART, which is left disabled + * so that a reception error never aborts the circular transfer, and its DMA to ready from the + * interrupt of the transmit stream, which may be disabled too. A transfer is over once its DMA + * moved the last byte, which the stream or channel tells, and the next one closes what the HAL left + * open: aborting the transmission returns the DMA to ready and unlocks it, which starting a + * transfer needs, and leaves the byte the peripheral is still sending alone. Aborting also empties + * the transmit FIFO of the peripheral when it is enabled, so a transfer is only over once that + * FIFO is empty. */ class UartDma { public: @@ -70,7 +77,9 @@ class UartDma { * @brief Take the bytes that arrived since the last call. * * @note Bytes are lost silently if this is called more rarely than the buffer takes to fill, - * which the framing recovers from by resynchronizing on the next delimiter. + * which the framing recovers from by resynchronizing on the next delimiter. A reception that the + * vendor HAL ended or whose DMA stream was disabled by a transfer error is aborted and started + * again from the beginning of the buffer, and the bytes it held are dropped. * * @param into Buffer to copy the received bytes into. * @return Number of bytes copied. @@ -93,7 +102,8 @@ class UartDma { * @note The last byte may still be leaving the shift register, which does not stop the next * transfer from starting. * - * @return True if the DMA is still feeding the peripheral, false otherwise. + * @return True if the DMA is still feeding the peripheral or its transmit FIFO holds bytes, + * false otherwise. */ bool is_transmitting() const; diff --git a/micras_hal/stm32/g4/src/family.cpp b/micras_hal/stm32/g4/src/family.cpp index 92cd80c..66f8df1 100644 --- a/micras_hal/stm32/g4/src/family.cpp +++ b/micras_hal/stm32/g4/src/family.cpp @@ -13,6 +13,14 @@ bool calibrate_adc(ADC_HandleTypeDef* handle) { return HAL_ADCEx_Calibration_Start(handle, ADC_SINGLE_ENDED) == HAL_OK; } +bool is_dma_enabled(const DMA_HandleTypeDef* handle) { + return (handle->Instance->CCR & DMA_CCR_EN) != 0; +} + +bool has_dma_finished(const DMA_HandleTypeDef* handle) { + return (handle->Instance->CCR & DMA_CCR_EN) == 0 or handle->Instance->CNDTR == 0; +} + void enable_caches() { } bool was_reset_by_watchdog() { diff --git a/micras_hal/stm32/h7/src/family.cpp b/micras_hal/stm32/h7/src/family.cpp index 7ed5e5a..0e11b0b 100644 --- a/micras_hal/stm32/h7/src/family.cpp +++ b/micras_hal/stm32/h7/src/family.cpp @@ -17,6 +17,23 @@ bool calibrate_adc(ADC_HandleTypeDef* handle) { return HAL_ADCEx_Calibration_Start(handle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) == HAL_OK; } +bool is_dma_enabled(const DMA_HandleTypeDef* handle) { + if (IS_DMA_STREAM_INSTANCE(handle->Instance)) { + return (static_cast(handle->Instance)->CR & DMA_SxCR_EN) != 0; + } + + return (static_cast(handle->Instance)->CCR & BDMA_CCR_EN) != 0; +} + +bool has_dma_finished(const DMA_HandleTypeDef* handle) { + if (IS_DMA_STREAM_INSTANCE(handle->Instance)) { + return (static_cast(handle->Instance)->CR & DMA_SxCR_EN) == 0; + } + + const auto* const channel = static_cast(handle->Instance); + return (channel->CCR & BDMA_CCR_EN) == 0 or channel->CNDTR == 0; +} + void enable_caches() { SCB_EnableICache(); } diff --git a/micras_hal/stm32/src/adc_dma.cpp b/micras_hal/stm32/src/adc_dma.cpp index 3683a41..b34e46d 100644 --- a/micras_hal/stm32/src/adc_dma.cpp +++ b/micras_hal/stm32/src/adc_dma.cpp @@ -113,6 +113,11 @@ void AdcDma::on_sequence_complete(const ADC_HandleTypeDef* handle) { return; } + if (__HAL_ADC_GET_FLAG(handle, ADC_FLAG_OVR)) { + instance->stopped = true; + return; + } + std::ranges::copy(instance->buffer, instance->snapshot.begin()); instance->sequence = instance->sequence + 1; } @@ -138,13 +143,22 @@ void AdcDma::stop_dma() { } void AdcDma::recover() { - if (not this->stopped) { + const DMA_HandleTypeDef* const dma = this->handle->DMA_Handle; + const bool frozen = dma->State == HAL_DMA_STATE_BUSY and + (__HAL_ADC_GET_FLAG(this->handle, ADC_FLAG_OVR) or not family::is_dma_enabled(dma)); + + if (not this->stopped and not frozen) { return; } HAL_ADC_Stop_DMA(this->handle); this->handle->State = this->handle->State & ~(HAL_ADC_STATE_ERROR_DMA | HAL_ADC_STATE_ERROR_INTERNAL); - this->stopped = HAL_ADC_Start_DMA(this->handle, this->transfer.data(), this->transfer.size()) != HAL_OK; + this->stopped = false; + + if (HAL_ADC_Start_DMA(this->handle, this->transfer.data(), this->transfer.size()) != HAL_OK) { + this->stopped = true; + } + restarts++; } diff --git a/micras_hal/stm32/src/uart_dma.cpp b/micras_hal/stm32/src/uart_dma.cpp index ff25a9d..941b6bb 100644 --- a/micras_hal/stm32/src/uart_dma.cpp +++ b/micras_hal/stm32/src/uart_dma.cpp @@ -9,6 +9,7 @@ #include #include +#include "micras/hal/family.hpp" #include "micras/hal/uart_dma.hpp" namespace micras::hal { @@ -35,7 +36,8 @@ std::size_t UartDma::read(std::span into) { return 0; } - if (this->handle->RxState != HAL_UART_STATE_BUSY_RX) { + if (this->handle->RxState != HAL_UART_STATE_BUSY_RX or not family::is_dma_enabled(this->handle->hdmarx)) { + HAL_UART_AbortReceive(this->handle); this->start_rx(this->rx_buffer); return 0; } @@ -71,7 +73,17 @@ bool UartDma::start_tx(std::span from) { } bool UartDma::is_transmitting() const { - return this->handle->gState == HAL_UART_STATE_BUSY_TX and this->handle->hdmatx->State == HAL_DMA_STATE_BUSY; + if (this->handle->gState != HAL_UART_STATE_BUSY_TX) { + return false; + } + + const DMA_HandleTypeDef* const dma = this->handle->hdmatx; + + if (dma->State == HAL_DMA_STATE_BUSY and not family::has_dma_finished(dma)) { + return true; + } + + return this->handle->FifoMode == UART_FIFOMODE_ENABLE and not __HAL_UART_GET_FLAG(this->handle, UART_FLAG_TXFE); } bool UartDma::was_initialized() const { diff --git a/tests/hal/test_family.cpp b/tests/hal/test_family.cpp index bd9cfe5..4b18b7b 100644 --- a/tests/hal/test_family.cpp +++ b/tests/hal/test_family.cpp @@ -44,5 +44,12 @@ TEST_SUITE("family") { hal::family::freeze_watchdog_in_debug(); hal::family::enable_cycle_counter(); } + + TEST_CASE("never stops or ends a transfer on a host") { + const DMA_HandleTypeDef dma{}; + + CHECK(hal::family::is_dma_enabled(&dma)); + CHECK_FALSE(hal::family::has_dma_finished(&dma)); + } } } // namespace micras::test From d9beafda65c52b3b76c19f24d379e968e1b2db3e Mon Sep 17 00:00:00 2001 From: GabrielCosme Date: Sun, 4 Oct 2026 05:14:39 -0300 Subject: [PATCH 561/561] =?UTF-8?q?=F0=9F=91=B7=20Check=20each=20commit=20?= =?UTF-8?q?once,=20by=20its=20push,=20and=20pull=20requests=20only=20from?= =?UTF-8?q?=20forks?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/ci.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 823f714..e9a4002 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -12,10 +12,13 @@ concurrency: group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} cancel-in-progress: true +# A push to a branch of this repository is checked once by its push run, so a pull request only runs +# here when it comes from a fork jobs: host: name: ๐Ÿงช Host build, tests, format, lint and docs runs-on: ubuntu-latest + if: github.event_name != 'pull_request' || github.event.pull_request.head.repo.full_name != github.repository timeout-minutes: 45 steps: - name: ๐Ÿ”€ Checkout @@ -66,6 +69,7 @@ jobs: stm32: name: ๐Ÿ”ฉ ${{ matrix.name }} reference and bench runs-on: ubuntu-latest + if: github.event_name != 'pull_request' || github.event.pull_request.head.repo.full_name != github.repository timeout-minutes: 45 strategy: fail-fast: false