From 69e44e97bae1471714604f16ffccc50b26b87fa5 Mon Sep 17 00:00:00 2001 From: Michael Yu Date: Tue, 28 Jul 2026 13:27:38 -0400 Subject: [PATCH 1/4] Added new driver files for supporting BNO085 IMu testing --- embedded/STM32/src/Drivers/BNO085.cpp | 106 ++++++++++++++++ embedded/STM32/src/Drivers/BNO085.h | 42 +++++++ embedded/STM32/src/Drivers/I2C_STM32.cpp | 63 ++++++++++ embedded/STM32/src/Drivers/I2C_STM32.h | 15 +++ embedded/STM32/src/Drivers/UART_STM32.cpp | 48 +++++++ embedded/STM32/src/Drivers/UART_STM32.h | 32 +++++ embedded/STM32/src/Drivers/sh2_hal_stm32.cpp | 124 +++++++++++++++++++ embedded/STM32/src/Drivers/sh2_hal_stm32.h | 19 +++ embedded/STM32/src/main.cpp | 67 +++++++++- 9 files changed, 513 insertions(+), 3 deletions(-) create mode 100644 embedded/STM32/src/Drivers/BNO085.cpp create mode 100644 embedded/STM32/src/Drivers/BNO085.h create mode 100644 embedded/STM32/src/Drivers/I2C_STM32.cpp create mode 100644 embedded/STM32/src/Drivers/I2C_STM32.h create mode 100644 embedded/STM32/src/Drivers/UART_STM32.cpp create mode 100644 embedded/STM32/src/Drivers/UART_STM32.h create mode 100644 embedded/STM32/src/Drivers/sh2_hal_stm32.cpp create mode 100644 embedded/STM32/src/Drivers/sh2_hal_stm32.h diff --git a/embedded/STM32/src/Drivers/BNO085.cpp b/embedded/STM32/src/Drivers/BNO085.cpp new file mode 100644 index 00000000..d7065afd --- /dev/null +++ b/embedded/STM32/src/Drivers/BNO085.cpp @@ -0,0 +1,106 @@ +#include "BNO085.h" + +#include "sh2_hal_stm32.h" + +#include + +BNO085::BNO085() { + memset(&rotationVector, 0, sizeof(rotationVector)); + memset(&accelerometer, 0, sizeof(accelerometer)); + memset(&gyroscope, 0, sizeof(gyroscope)); +} + +bool BNO085::begin() { + int status = sh2_open(SH2_HAL_GetInstance(), EventCallback, this); + + if (status != SH2_OK) + return false; + + sh2_setSensorCallback(SensorCallback, this); + + enableRotationVector(); + + return true; +} + +void BNO085::update() { + sh2_service(); +} + +bool BNO085::enableRotationVector(uint32_t interval_us) { + sh2_SensorConfig_t config; + + memset(&config, 0, sizeof(config)); + + config.reportInterval_us = interval_us; + + return sh2_setSensorConfig(SH2_ROTATION_VECTOR, &config) == SH2_OK; +} + +bool BNO085::enableAccelerometer(uint32_t interval_us) { + sh2_SensorConfig_t config; + + memset(&config, 0, sizeof(config)); + + config.reportInterval_us = interval_us; + + return sh2_setSensorConfig(SH2_ACCELEROMETER, &config) == SH2_OK; +} + +bool BNO085::enableGyroscope(uint32_t interval_us) { + sh2_SensorConfig_t config; + + memset(&config, 0, sizeof(config)); + + config.reportInterval_us = interval_us; + + return sh2_setSensorConfig(SH2_GYROSCOPE_CALIBRATED, &config) == SH2_OK; +} + +Quaternion BNO085::getQuaternion() const { + return rotationVector; +} + +sh2_SensorValue_t BNO085::getAccelerometer() const { + return accelerometer; +} + +sh2_SensorValue_t BNO085::getGyroscope() const { + return gyroscope; +} + +void BNO085::SensorCallback(void* cookie, sh2_SensorEvent_t* event) { + BNO085* imu = static_cast(cookie); + + imu->handleSensorEvent(event); +} + +void BNO085::EventCallback(void* cookie, sh2_AsyncEvent_t* event) { + (void)cookie; + (void)event; +} + +void BNO085::handleSensorEvent(sh2_SensorEvent_t* event) { + sh2_SensorValue_t value; + + if (sh2_decodeSensorEvent(&value, event) != SH2_OK) { + return; + } + + switch (value.sensorId) { + case SH2_ROTATION_VECTOR: + rotationVector = value; + break; + + case SH2_ACCELEROMETER: + accelerometer = value; + break; + + case SH2_GYROSCOPE_CALIBRATED: + gyroscope = value; + break; + + default: + break; + } +} \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/BNO085.h b/embedded/STM32/src/Drivers/BNO085.h new file mode 100644 index 00000000..f6a9fe68 --- /dev/null +++ b/embedded/STM32/src/Drivers/BNO085.h @@ -0,0 +1,42 @@ +#ifndef BNO085_H +#define BNO085_H + +#include "sh2.h" +#include "sh2_SensorValue.h" + +struct Quaternion { + float w; + float x; + float y; + float z; +}; + +class BNO085 { +public: + BNO085(); + + bool begin(); + + void update(); + + bool enableRotationVector(uint32_t interval_us = 10000); + bool enableAccelerometer(uint32_t interval_us = 10000); + bool enableGyroscope(uint32_t interval_us = 10000); + + Quaternion getQuaternion() const; + sh2_SensorValue_t getAccelerometer() const; + sh2_SensorValue_t getGyroscope() const; + +private: + static void SensorCallback(void* cookie, sh2_SensorEvent_t* event); + + static void EventCallback(void* cookie, sh2_AsyncEvent_t* event); + + void handleSensorEvent(sh2_SensorEvent_t* event); + + sh2_SensorValue_t rotationVector; + sh2_SensorValue_t accelerometer; + sh2_SensorValue_t gyroscope; +}; + +#endif \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/I2C_STM32.cpp b/embedded/STM32/src/Drivers/I2C_STM32.cpp new file mode 100644 index 00000000..a1c8d8cc --- /dev/null +++ b/embedded/STM32/src/Drivers/I2C_STM32.cpp @@ -0,0 +1,63 @@ +#include "I2C_STM32.h" + +I2C_HandleTypeDef hi2c1; + +void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c) { + GPIO_InitTypeDef GPIO_InitStruct = {0}; + RCC_PeriphCLKInitTypeDef PeriphClkInit = {0}; + + if (hi2c->Instance == I2C1) { + /* Select PCLK1 as I2C1 clock source */ + PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C1; + PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1; + + HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit); + + /* Enable GPIOB clock */ + __HAL_RCC_GPIOB_CLK_ENABLE(); + + /* Configure PB8 (SCL) and PB9 (SDA) */ + GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9; + GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF4_I2C1; + + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* Enable I2C peripheral clock */ + __HAL_RCC_I2C1_CLK_ENABLE(); + } +} + +void MX_I2C1_Init(void) { + hi2c1.Instance = I2C1; + + hi2c1.Init.Timing = 0x00300617; + + hi2c1.Init.OwnAddress1 = 0; + + hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + + hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + + hi2c1.Init.OwnAddress2 = 0; + + hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK; + + hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + + hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; + + if (HAL_I2C_Init(&hi2c1) != HAL_OK) { + Error_Handler(); + } + + if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK) { + Error_Handler(); + } + + if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK) { + Error_Handler(); + } +} \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/I2C_STM32.h b/embedded/STM32/src/Drivers/I2C_STM32.h new file mode 100644 index 00000000..510b1878 --- /dev/null +++ b/embedded/STM32/src/Drivers/I2C_STM32.h @@ -0,0 +1,15 @@ +#pragma once + +#include "stm32g4xx_hal.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern I2C_HandleTypeDef hi2c1; + +void MX_I2C1_Init(void); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/UART_STM32.cpp b/embedded/STM32/src/Drivers/UART_STM32.cpp new file mode 100644 index 00000000..97a6ecc8 --- /dev/null +++ b/embedded/STM32/src/Drivers/UART_STM32.cpp @@ -0,0 +1,48 @@ +#include "UART_STM32.h" +#include + +UART_HandleTypeDef hlpuart1; + +void MX_LPUART1_Init(void) { + hlpuart1.Instance = LPUART1; + + hlpuart1.Init.BaudRate = 115200; + hlpuart1.Init.WordLength = UART_WORDLENGTH_8B; + hlpuart1.Init.StopBits = UART_STOPBITS_1; + hlpuart1.Init.Parity = UART_PARITY_NONE; + hlpuart1.Init.Mode = UART_MODE_TX_RX; + hlpuart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; + hlpuart1.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; + hlpuart1.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; + + if (HAL_UART_Init(&hlpuart1) != HAL_OK) { + while (1) { + } + } +} + +void HAL_UART_MspInit(UART_HandleTypeDef* huart) { + GPIO_InitTypeDef GPIO_InitStruct = {}; + + if (huart->Instance == LPUART1) { + __HAL_RCC_LPUART1_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + + GPIO_InitStruct.Pin = GPIO_PIN_2 | GPIO_PIN_3; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF8_LPUART1; + + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + } +} + +void UART_Print(const char* message) { + if (message == nullptr) { + return; + } + + HAL_UART_Transmit(&hlpuart1, reinterpret_cast(const_cast(message)), + strlen(message), HAL_MAX_DELAY); +} \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/UART_STM32.h b/embedded/STM32/src/Drivers/UART_STM32.h new file mode 100644 index 00000000..7eb4210f --- /dev/null +++ b/embedded/STM32/src/Drivers/UART_STM32.h @@ -0,0 +1,32 @@ +#ifndef UART_STM32_H +#define UART_STM32_H + +#include "stm32g4xx_hal.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern UART_HandleTypeDef hlpuart1; + +/** + * @brief Initialize LPUART1 for communication through the Nucleo + * ST-LINK Virtual COM Port. + * + * Baud rate: 115200 + * Format: 8-N-1 + */ +void MX_LPUART1_Init(void); + +/** + * @brief Transmit a null-terminated string over LPUART1. + * + * @param message String to transmit. + */ +void UART_Print(const char* message); + +#ifdef __cplusplus +} +#endif + +#endif // UART_STM32_H \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/sh2_hal_stm32.cpp b/embedded/STM32/src/Drivers/sh2_hal_stm32.cpp new file mode 100644 index 00000000..c1244408 --- /dev/null +++ b/embedded/STM32/src/Drivers/sh2_hal_stm32.cpp @@ -0,0 +1,124 @@ +#include "sh2_hal_stm32.h" + +#include "I2C_STM32.h" + +#include "stm32g4xx_hal.h" +#include "stm32g4xx_hal_i2c.h" + +#include + +/////////////////////////////////////////////////////////////////////////////// +// Configuration +/////////////////////////////////////////////////////////////////////////////// + +#define BNO085_I2C_ADDR (0x4A << 1) + +/////////////////////////////////////////////////////////////////////////////// +// Forward declarations +/////////////////////////////////////////////////////////////////////////////// + +static int SH2_Open(sh2_Hal_t* self); +static void SH2_Close(sh2_Hal_t* self); + +static int SH2_Read(sh2_Hal_t* self, uint8_t* buffer, unsigned len, uint32_t* t_us); + +static int SH2_Write(sh2_Hal_t* self, uint8_t* buffer, unsigned len); + +static uint32_t SH2_GetTimeUs(sh2_Hal_t* self); + +/////////////////////////////////////////////////////////////////////////////// +// Static HAL object +/////////////////////////////////////////////////////////////////////////////// + +static sh2_Hal_t sh2Hal = {SH2_Open, SH2_Close, SH2_Read, SH2_Write, SH2_GetTimeUs}; + +/////////////////////////////////////////////////////////////////////////////// + +sh2_Hal_t* SH2_HAL_GetInstance(void) { + return &sh2Hal; +} + +/////////////////////////////////////////////////////////////////////////////// + +static int SH2_Open(sh2_Hal_t* self) { + (void)self; + + MX_I2C1_Init(); + + HAL_Delay(10); + + return 0; +} + +/////////////////////////////////////////////////////////////////////////////// + +static void SH2_Close(sh2_Hal_t* self) { + (void)self; + + // Nothing to do for now. +} + +/////////////////////////////////////////////////////////////////////////////// + +static int SH2_Write(sh2_Hal_t* self, uint8_t* buffer, unsigned len) { + (void)self; + + HAL_StatusTypeDef status = + HAL_I2C_Master_Transmit(&hi2c1, BNO085_I2C_ADDR, buffer, len, HAL_MAX_DELAY); + + if (status != HAL_OK) + return -1; + + return (int)len; +} + +/////////////////////////////////////////////////////////////////////////////// + +static int SH2_Read(sh2_Hal_t* self, uint8_t* buffer, unsigned len, uint32_t* t_us) { + (void)self; + + uint8_t header[4]; + + //------------------------------------------------------------- + // Read SHTP header + //------------------------------------------------------------- + + if (HAL_I2C_Master_Receive(&hi2c1, BNO085_I2C_ADDR, header, 4, HAL_MAX_DELAY) != HAL_OK) { + return -1; + } + + uint16_t packetLength = (header[0] | ((header[1] & 0x7F) << 8)); + + if (packetLength == 0) + return 0; + + if (packetLength > len) + packetLength = len; + + memcpy(buffer, header, 4); + + //------------------------------------------------------------- + // Read payload + //------------------------------------------------------------- + + if (packetLength > 4) { + if (HAL_I2C_Master_Receive(&hi2c1, BNO085_I2C_ADDR, buffer + 4, packetLength - 4, + HAL_MAX_DELAY) != HAL_OK) { + return -1; + } + } + + if (t_us) { + *t_us = HAL_GetTick() * 1000UL; + } + + return packetLength; +} + +/////////////////////////////////////////////////////////////////////////////// + +static uint32_t SH2_GetTimeUs(sh2_Hal_t* self) { + (void)self; + + return HAL_GetTick() * 1000UL; +} \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/sh2_hal_stm32.h b/embedded/STM32/src/Drivers/sh2_hal_stm32.h new file mode 100644 index 00000000..9f51e2c9 --- /dev/null +++ b/embedded/STM32/src/Drivers/sh2_hal_stm32.h @@ -0,0 +1,19 @@ +#ifndef SH2_HAL_STM32_H +#define SH2_HAL_STM32_H + +#include "sh2_hal.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Returns the SH-2 HAL instance used by the STM32 implementation. + */ +sh2_Hal_t* SH2_HAL_GetInstance(void); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/embedded/STM32/src/main.cpp b/embedded/STM32/src/main.cpp index d36029d0..aec09eaa 100644 --- a/embedded/STM32/src/main.cpp +++ b/embedded/STM32/src/main.cpp @@ -1,7 +1,9 @@ // Code for CAN implementation +#include "Drivers/UART_STM32.h" #include #include +#include "Drivers/BNO085.h" #include "Drivers/FDCAN_STM32.h" #include "Drivers/SysClock.h" #include "Drivers/TIM.h" @@ -26,6 +28,7 @@ FDCAN_FilterTypeDef sFilterConfig; FDCAN_TxHeaderTypeDef TxHeader; FDCAN_RxHeaderTypeDef RxHeader; +BNO085 imu; BLDCMotor motor = BLDCMotor(7); BLDCDriver3PWM driver = BLDCDriver3PWM(phaseA, phaseB, phaseC); @@ -54,13 +57,44 @@ void doLimit(char* cmd) { } void setup() { + // HAL_Init(); + // SystemClock_Config(); + + // Serial.begin(115200); + // while (!Serial) + // ; + // Serial.println("STM32 Serial OK!"); + HAL_Init(); SystemClock_Config(); + MX_LPUART1_Init(); + + UART_Print("...\r\n"); + UART_Print("STM32 UART test\r\n"); + UART_Print("If you can read this, COM3 is working\r\n"); + UART_Print("why is this so finicky?\r\n"); + Serial.begin(115200); - while (!Serial) - ; - Serial.println("STM32 Serial OK!"); + delay(1000); + + Serial.println("=== STM32 STARTING ==="); + + MX_FDCAN2_Init(); + MX_TIM4_Init(); + MX_I2C1_Init(); + + Serial.println("=== PERIPHERALS INITIALIZED ==="); + + if (!imu.begin()) { + Serial.println("=== BNO085 FAILED ==="); + + while (1) { + delay(1000); + } + } + + Serial.println("=== BNO085 INITIALIZED ==="); /* Configure the system clock */ @@ -74,6 +108,14 @@ void setup() { // MT6835_Init(&hspi1, MT_CS_GPIO, MT_CS_PIN); + MX_I2C1_Init(); + + if (!imu.begin()) { + while (1) { + // IMU initialization failed + } + } + /* USER CODE BEGIN 2 */ TxHeader.Identifier = 0x123; // Standard ID TxHeader.IdType = FDCAN_STANDARD_ID; @@ -205,6 +247,25 @@ void loop() { encoder.update(); Serial.println(encoder.getAngle()); + Serial.println("=== LOOP RUNNING ==="); + imu.update(); + + Quaternion q = imu.getQuaternion(); + + Serial.print("W: "); + Serial.print(q.w, 3); + + Serial.print(" X: "); + Serial.print(q.x, 3); + + Serial.print(" Y: "); + Serial.print(q.y, 3); + + Serial.print(" Z: "); + Serial.println(q.z, 3); + + delay(100); + // uint32_t mdeg = {1}; // TxData_C2_To_C3[0] = (mdeg >> 24) & 0xFF; // TxData_C2_To_C3[1] = (mdeg >> 16) & 0xFF; From ad317958d7e41e25225f3d7291a97a374886912c Mon Sep 17 00:00:00 2001 From: Michael Yu Date: Wed, 29 Jul 2026 20:17:41 -0400 Subject: [PATCH 2/4] fix build errors --- embedded/STM32/platformio.ini | 2 ++ embedded/STM32/src/Drivers/BNO085.cpp | 9 ++++++++- embedded/STM32/src/Drivers/BNO085.h | 1 + embedded/STM32/src/Drivers/FDCAN_STM32.cpp | 4 ++-- embedded/STM32/src/Drivers/FDCAN_STM32.h | 3 ++- embedded/STM32/src/Drivers/I2C_STM32.cpp | 6 +++--- embedded/STM32/src/Drivers/I2C_STM32.h | 1 + embedded/STM32/src/Drivers/SysClock.cpp | 4 ++-- embedded/STM32/src/Drivers/SysClock.h | 3 ++- embedded/STM32/src/Drivers/TIM.cpp | 14 +++++++------- embedded/STM32/src/Drivers/TIM.h | 3 ++- embedded/STM32/src/main.cpp | 9 ++++++++- 12 files changed, 40 insertions(+), 19 deletions(-) diff --git a/embedded/STM32/platformio.ini b/embedded/STM32/platformio.ini index 024b6f0e..07d70b0f 100644 --- a/embedded/STM32/platformio.ini +++ b/embedded/STM32/platformio.ini @@ -15,6 +15,7 @@ default_envs = app platform = ststm32 board = nucleo_g474re framework = arduino +lib_ignore = FreeRTOS-Kernel upload_protocol = stlink board_build.f_cpu = 170000000L monitor_speed = 115200 @@ -34,6 +35,7 @@ lib_deps = math simplefoc/SimpleFOCDrivers@^1.0.8 askuric/Simple FOC@^2.3.4 + adafruit/Adafruit BNO08x@^1.2.5 build_flags = -Wl,-Map,firmware.map diff --git a/embedded/STM32/src/Drivers/BNO085.cpp b/embedded/STM32/src/Drivers/BNO085.cpp index d7065afd..17d6f36e 100644 --- a/embedded/STM32/src/Drivers/BNO085.cpp +++ b/embedded/STM32/src/Drivers/BNO085.cpp @@ -58,7 +58,14 @@ bool BNO085::enableGyroscope(uint32_t interval_us) { } Quaternion BNO085::getQuaternion() const { - return rotationVector; + Quaternion q; + + q.w = rotationVector.un.rotationVector.real; + q.x = rotationVector.un.rotationVector.i; + q.y = rotationVector.un.rotationVector.j; + q.z = rotationVector.un.rotationVector.k; + + return q; } sh2_SensorValue_t BNO085::getAccelerometer() const { diff --git a/embedded/STM32/src/Drivers/BNO085.h b/embedded/STM32/src/Drivers/BNO085.h index f6a9fe68..5a8aa877 100644 --- a/embedded/STM32/src/Drivers/BNO085.h +++ b/embedded/STM32/src/Drivers/BNO085.h @@ -2,6 +2,7 @@ #define BNO085_H #include "sh2.h" +#include "sh2_err.h" #include "sh2_SensorValue.h" struct Quaternion { diff --git a/embedded/STM32/src/Drivers/FDCAN_STM32.cpp b/embedded/STM32/src/Drivers/FDCAN_STM32.cpp index 14abc870..5b7dd469 100644 --- a/embedded/STM32/src/Drivers/FDCAN_STM32.cpp +++ b/embedded/STM32/src/Drivers/FDCAN_STM32.cpp @@ -49,7 +49,7 @@ void MX_FDCAN2_Init(void) { hfdcan2.Init.ExtFiltersNbr = 0; hfdcan2.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; if (HAL_FDCAN_Init(&hfdcan2) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } /* USER CODE BEGIN FDCAN2_Init 2 */ @@ -72,7 +72,7 @@ void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef* fdcanHandle) { PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; PeriphClkInit.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } /* FDCAN2 clock enable */ diff --git a/embedded/STM32/src/Drivers/FDCAN_STM32.h b/embedded/STM32/src/Drivers/FDCAN_STM32.h index be9c0268..c5f1de03 100644 --- a/embedded/STM32/src/Drivers/FDCAN_STM32.h +++ b/embedded/STM32/src/Drivers/FDCAN_STM32.h @@ -1,4 +1,4 @@ -#include +#include #ifdef __cplusplus extern "C" { @@ -7,6 +7,7 @@ void HAL_FDCAN_ErrorStatusCallback(FDCAN_HandleTypeDef* hfdcan, uint32_t ErrorSt void MX_FDCAN2_Init(void); void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef* fdcanHandle); void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef* fdcanHandle); +void AppError_Handler(void); extern FDCAN_HandleTypeDef hfdcan2; #ifdef __cplusplus diff --git a/embedded/STM32/src/Drivers/I2C_STM32.cpp b/embedded/STM32/src/Drivers/I2C_STM32.cpp index a1c8d8cc..2f0900b8 100644 --- a/embedded/STM32/src/Drivers/I2C_STM32.cpp +++ b/embedded/STM32/src/Drivers/I2C_STM32.cpp @@ -50,14 +50,14 @@ void MX_I2C1_Init(void) { hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; if (HAL_I2C_Init(&hi2c1) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } } \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/I2C_STM32.h b/embedded/STM32/src/Drivers/I2C_STM32.h index 510b1878..9f42e9d2 100644 --- a/embedded/STM32/src/Drivers/I2C_STM32.h +++ b/embedded/STM32/src/Drivers/I2C_STM32.h @@ -9,6 +9,7 @@ extern "C" { extern I2C_HandleTypeDef hi2c1; void MX_I2C1_Init(void); +void AppError_Handler(void); #ifdef __cplusplus } diff --git a/embedded/STM32/src/Drivers/SysClock.cpp b/embedded/STM32/src/Drivers/SysClock.cpp index c18284a2..b0b87dab 100644 --- a/embedded/STM32/src/Drivers/SysClock.cpp +++ b/embedded/STM32/src/Drivers/SysClock.cpp @@ -28,7 +28,7 @@ void SystemClock_Config(void) { RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2; RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } /** Initializes the CPU, AHB and APB buses clocks @@ -48,6 +48,6 @@ void SystemClock_Config(void) { HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit); if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } } diff --git a/embedded/STM32/src/Drivers/SysClock.h b/embedded/STM32/src/Drivers/SysClock.h index f071efd0..0c9c5389 100644 --- a/embedded/STM32/src/Drivers/SysClock.h +++ b/embedded/STM32/src/Drivers/SysClock.h @@ -1,11 +1,12 @@ // STM32G474CEU6 WEACT STUDIO Board -#include +#include #ifdef __cplusplus extern "C" { #endif void SystemClock_Config(void); +void AppError_Handler(void); #ifdef __cplusplus } diff --git a/embedded/STM32/src/Drivers/TIM.cpp b/embedded/STM32/src/Drivers/TIM.cpp index 197c4da2..8cac5d4a 100644 --- a/embedded/STM32/src/Drivers/TIM.cpp +++ b/embedded/STM32/src/Drivers/TIM.cpp @@ -49,32 +49,32 @@ void MX_TIM4_Init(void) { htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_Base_Init(&htim4) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } if (HAL_TIM_PWM_Init(&htim4) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } /* USER CODE BEGIN TIM4_Init 2 */ diff --git a/embedded/STM32/src/Drivers/TIM.h b/embedded/STM32/src/Drivers/TIM.h index 7aa280f7..c0f49d65 100644 --- a/embedded/STM32/src/Drivers/TIM.h +++ b/embedded/STM32/src/Drivers/TIM.h @@ -1,4 +1,4 @@ -#include +#include #ifdef __cplusplus extern "C" { @@ -6,6 +6,7 @@ extern "C" { void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle); void MX_TIM4_Init(void); +void AppError_Handler(void); extern TIM_HandleTypeDef htim4; #ifdef __cplusplus diff --git a/embedded/STM32/src/main.cpp b/embedded/STM32/src/main.cpp index aec09eaa..ecb82a52 100644 --- a/embedded/STM32/src/main.cpp +++ b/embedded/STM32/src/main.cpp @@ -5,6 +5,7 @@ #include "Drivers/BNO085.h" #include "Drivers/FDCAN_STM32.h" +#include "Drivers/I2C_STM32.h" #include "Drivers/SysClock.h" #include "Drivers/TIM.h" @@ -45,6 +46,12 @@ uint8_t RxData_C3[8]; volatile int txDone = 0; const static char motor_id = 'M'; +void AppError_Handler(void) { + __disable_irq(); + while (1) { + } +} + Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); @@ -219,7 +226,7 @@ void loop() { } prevTime = currTime; if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan2, &TxHeader, TxData_C2_To_C3) != HAL_OK) { - Error_Handler(); + AppError_Handler(); } while (HAL_FDCAN_IsTxBufferMessagePending(&hfdcan2, FDCAN_TX_BUFFER0)) { From 4f4577b527f25242eac98c5e1a68d3b5080f1663 Mon Sep 17 00:00:00 2001 From: michaely07 Date: Fri, 21 Aug 2026 14:54:08 -0400 Subject: [PATCH 3/4] Kalman Filter implementation --- embedded/STM32/src/EKF/ekf_ahrs.c | 270 ++++++++++++++++++++++++++++ embedded/STM32/src/EKF/ekf_ahrs.h | 85 +++++++++ embedded/STM32/src/EKF/quaternion.c | 127 +++++++++++++ embedded/STM32/src/EKF/quaternion.h | 71 ++++++++ 4 files changed, 553 insertions(+) create mode 100644 embedded/STM32/src/EKF/ekf_ahrs.c create mode 100644 embedded/STM32/src/EKF/ekf_ahrs.h create mode 100644 embedded/STM32/src/EKF/quaternion.c create mode 100644 embedded/STM32/src/EKF/quaternion.h diff --git a/embedded/STM32/src/EKF/ekf_ahrs.c b/embedded/STM32/src/EKF/ekf_ahrs.c new file mode 100644 index 00000000..4a9e0ea5 --- /dev/null +++ b/embedded/STM32/src/EKF/ekf_ahrs.c @@ -0,0 +1,270 @@ +#include "ekf_ahrs.h" +#include +#include + +/* ---------- small fixed-size matrix helpers (no malloc) ---------- */ + +static void mat6x6_mult(const float A[6][6], const float B[6][6], float C[6][6]) { + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + float s = 0.0f; + for (int k = 0; k < 6; k++) s += A[i][k] * B[k][j]; + C[i][j] = s; + } + } +} + +static void mat6x6_transpose(const float A[6][6], float At[6][6]) { + for (int i = 0; i < 6; i++) + for (int j = 0; j < 6; j++) + At[i][j] = A[j][i]; +} + +static void mat6x6_add(const float A[6][6], const float B[6][6], float C[6][6]) { + for (int i = 0; i < 6; i++) + for (int j = 0; j < 6; j++) + C[i][j] = A[i][j] + B[i][j]; +} + +/* 3x3 inverse via adjugate/determinant (fine for well-conditioned S) */ +static int mat3_inverse(const float M[3][3], float Minv[3][3]) { + float det = + M[0][0] * (M[1][1] * M[2][2] - M[1][2] * M[2][1]) - + M[0][1] * (M[1][0] * M[2][2] - M[1][2] * M[2][0]) + + M[0][2] * (M[1][0] * M[2][1] - M[1][1] * M[2][0]); + + if (fabsf(det) < 1e-12f) return 0; /* singular, bail out */ + float inv_det = 1.0f / det; + + Minv[0][0] = (M[1][1] * M[2][2] - M[1][2] * M[2][1]) * inv_det; + Minv[0][1] = -(M[0][1] * M[2][2] - M[0][2] * M[2][1]) * inv_det; + Minv[0][2] = (M[0][1] * M[1][2] - M[0][2] * M[1][1]) * inv_det; + + Minv[1][0] = -(M[1][0] * M[2][2] - M[1][2] * M[2][0]) * inv_det; + Minv[1][1] = (M[0][0] * M[2][2] - M[0][2] * M[2][0]) * inv_det; + Minv[1][2] = -(M[0][0] * M[1][2] - M[0][2] * M[1][0]) * inv_det; + + Minv[2][0] = (M[1][0] * M[2][1] - M[1][1] * M[2][0]) * inv_det; + Minv[2][1] = -(M[0][0] * M[2][1] - M[0][1] * M[2][0]) * inv_det; + Minv[2][2] = (M[0][0] * M[1][1] - M[0][1] * M[1][0]) * inv_det; + + return 1; +} + +/* ---------- init ---------- */ + +void ekf_ahrs_init(ekf_ahrs_t *ekf, const float mag_ref_nav[3]) { + + + memset(ekf, 0, sizeof(*ekf)); + ekf->q = quat_identity(); + + for (int i = 0; i < 6; i++) { + ekf->P[i][i] = (i < 3) ? 0.05f /* initial attitude uncertainty, rad^2 */ + : 0.01f; /* initial gyro bias uncertainty, (rad/s)^2 */ + } + + /* --- default noise parameters: TUNE THESE for your sensors --- */ + ekf->gyro_noise_var = 3e-4f; /* gyro white noise, (rad/s)^2 -- from datasheet or Allan variance */ + ekf->gyro_bias_var = 1e-7f; /* gyro bias random walk, (rad/s)^2/s */ + ekf->accel_noise_var = 5e-2f; /* accel direction noise -- raise this if vehicle moves/vibrates a lot */ + ekf->mag_noise_var = 5e-2f; /* mag direction noise -- raise near magnetic interference */ + + ekf->accel_ref[0] = 0.0f; + ekf->accel_ref[1] = 0.0f; + ekf->accel_ref[2] = 1.0f; /* "up" in nav frame */ + + ekf->mag_ref[0] = mag_ref_nav[0]; + ekf->mag_ref[1] = mag_ref_nav[1]; + ekf->mag_ref[2] = mag_ref_nav[2]; + + vec3_normalize(ekf->mag_ref); +} + +/* ---------- predict ---------- */ + +void ekf_ahrs_predict(ekf_ahrs_t *ekf, const float gyro[3], float dt) { + if (dt <= 0.0f) return; + + float w[3] = { + gyro[0] - ekf->bias[0], + gyro[1] - ekf->bias[1], + gyro[2] - ekf->bias[2] + }; + + /* --- propagate attitude exactly (exponential map), not just linearized --- */ + quat_t dq = quat_from_gyro_delta(w, dt); + ekf->q = quat_normalize(quat_mult(ekf->q, dq)); + + /* --- propagate error-state covariance --- + * F = [ I - skew(w)*dt -I*dt ] + * [ 0 I ] + * (first-order discretization; fine for small dt e.g. <= 10-20 ms) + */ + float Sw[3][3]; + skew3(w, Sw); + + float F[6][6] = {0}; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + F[i][j] = -Sw[i][j] * dt; + } + F[i][i] += 1.0f; + F[i][i + 3] = -dt; + F[i + 3][i + 3] = 1.0f; + } + + float Q[6][6] = {0}; + for (int i = 0; i < 3; i++) { + Q[i][i] = ekf->gyro_noise_var * dt * dt; + Q[i + 3][i + 3] = ekf->gyro_bias_var * dt; + } + + float Ft[6][6], FP[6][6], FPFt[6][6], Pnew[6][6]; + mat6x6_transpose(F, Ft); + mat6x6_mult(F, ekf->P, FP); + mat6x6_mult(FP, Ft, FPFt); + mat6x6_add(FPFt, Q, Pnew); + memcpy(ekf->P, Pnew, sizeof(Pnew)); +} + +/* ---------- generic 3-dof vector-observation correction ---------- + * Shared logic for accel and mag updates: both are "measure a known + * nav-frame reference direction, rotated into the body frame" updates, + * differing only in the reference vector and measurement noise used. + */ +static void vector_update(ekf_ahrs_t *ekf, const float meas_body_in[3], + const float ref_nav[3], float meas_noise_var) { + float meas_body[3] = {meas_body_in[0], meas_body_in[1], meas_body_in[2]}; + vec3_normalize(meas_body); + + float R[3][3], Rt[3][3]; + quat_to_rotmat(ekf->q, R); + mat3_transpose(R, Rt); + + /* predicted body-frame direction of the reference vector */ + float h[3]; + mat3_vec_mult(Rt, ref_nav, h); + vec3_normalize(h); + + /* innovation */ + float y[3] = { + meas_body[0] - h[0], + meas_body[1] - h[1], + meas_body[2] - h[2] + }; + + /* H = [ skew(h), 0 ] (3x6) -- Jacobian of predicted measurement + * w.r.t. the attitude-error part of the state */ + float Sh[3][3]; + skew3(h, Sh); + float H[3][6] = {0}; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + H[i][j] = Sh[i][j]; + + /* S = H P H^T + R (3x3) */ + float HP[3][6] = {0}; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 6; j++) { + float s = 0.0f; + for (int k = 0; k < 6; k++) s += H[i][k] * ekf->P[k][j]; + HP[i][j] = s; + } + + float S[3][3] = {0}; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) { + float s = 0.0f; + for (int k = 0; k < 6; k++) s += HP[i][k] * H[j][k]; /* H^T */ + S[i][j] = s; + } + S[0][0] += meas_noise_var; + S[1][1] += meas_noise_var; + S[2][2] += meas_noise_var; + + float Sinv[3][3]; + if (!mat3_inverse(S, Sinv)) return; /* skip update if S is singular */ + + /* K = P H^T S^-1 (6x3) */ + float PHt[6][3] = {0}; + for (int i = 0; i < 6; i++) + for (int j = 0; j < 3; j++) { + float s = 0.0f; + for (int k = 0; k < 6; k++) s += ekf->P[i][k] * H[j][k]; /* H^T */ + PHt[i][j] = s; + } + + float K[6][3] = {0}; + for (int i = 0; i < 6; i++) + for (int j = 0; j < 3; j++) { + float s = 0.0f; + for (int k = 0; k < 3; k++) s += PHt[i][k] * Sinv[k][j]; + K[i][j] = s; + } + + /* error-state correction dx = K * y (6x1) */ + float dx[6] = {0}; + for (int i = 0; i < 6; i++) { + float s = 0.0f; + for (int k = 0; k < 3; k++) s += K[i][k] * y[k]; + dx[i] = s; + } + + /* apply attitude correction, reset bias, reset attitude error to 0 */ + float da[3] = {dx[0], dx[1], dx[2]}; + quat_t dq = quat_from_small_angle(da); + ekf->q = quat_normalize(quat_mult(ekf->q, dq)); + + ekf->bias[0] += dx[3]; + ekf->bias[1] += dx[4]; + ekf->bias[2] += dx[5]; + + /* covariance update, Joseph form for numerical stability: + * P = (I - K H) P (I - K H)^T + K R K^T + */ + float KH[6][6] = {0}; + for (int i = 0; i < 6; i++) + for (int j = 0; j < 6; j++) { + float s = 0.0f; + for (int k = 0; k < 3; k++) s += K[i][k] * H[k][j]; + KH[i][j] = s; + } + + float IKH[6][6]; + for (int i = 0; i < 6; i++) + for (int j = 0; j < 6; j++) + IKH[i][j] = ((i == j) ? 1.0f : 0.0f) - KH[i][j]; + + float IKHt[6][6]; + mat6x6_transpose(IKH, IKHt); + + float term1[6][6], term1b[6][6]; + mat6x6_mult(IKH, ekf->P, term1); + mat6x6_mult(term1, IKHt, term1b); + + float KRKt[6][6] = {0}; + for (int i = 0; i < 6; i++) + for (int j = 0; j < 6; j++) { + float s = 0.0f; + for (int k = 0; k < 3; k++) + s += K[i][k] * (k == 0 || k == 1 || k == 2 ? meas_noise_var : 0.0f) * K[j][k]; + KRKt[i][j] = s; + } + + float Pnew[6][6]; + mat6x6_add(term1b, KRKt, Pnew); + memcpy(ekf->P, Pnew, sizeof(Pnew)); +} + +void ekf_ahrs_update_accel(ekf_ahrs_t *ekf, const float accel[3]) { + vector_update(ekf, accel, ekf->accel_ref, ekf->accel_noise_var); +} + +void ekf_ahrs_update_mag(ekf_ahrs_t *ekf, const float mag[3]) { + vector_update(ekf, mag, ekf->mag_ref, ekf->mag_noise_var); +} + +void ekf_ahrs_get_euler(const ekf_ahrs_t *ekf, float *roll, float *pitch, float *yaw) { + quat_to_euler(ekf->q, roll, pitch, yaw); +} diff --git a/embedded/STM32/src/EKF/ekf_ahrs.h b/embedded/STM32/src/EKF/ekf_ahrs.h new file mode 100644 index 00000000..1f25c981 --- /dev/null +++ b/embedded/STM32/src/EKF/ekf_ahrs.h @@ -0,0 +1,85 @@ +#ifndef EKF_AHRS_H +#define EKF_AHRS_H + +#include "quaternion.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Error-state (multiplicative) EKF for 9-axis AHRS. + * + * State carried between calls: + * - q : unit quaternion, body->nav attitude + * - bias : gyro bias estimate (rad/s), body frame + * + * Kalman filter operates internally on a 6-element error state: + * [ delta_theta (3) ] small-angle attitude error (rad) + * [ delta_bias (3) ] gyro bias error (rad/s) + * which is reset to zero after every correction (standard MEKF pattern). + * + * Usage per control loop: + * ekf_ahrs_predict(&ekf, gyro, dt); // every loop, using gyro (rad/s) + * ekf_ahrs_update_accel(&ekf, accel); // whenever new accel sample (m/s^2 or g, any consistent unit) + * ekf_ahrs_update_mag(&ekf, mag); // whenever new mag sample (any consistent unit) + * ekf_ahrs_get_euler(&ekf, &roll, &pitch, &yaw); + * + * Notes: + * - accel and mag vectors are normalized internally, so units don't need + * to match the reference vectors exactly, only directions matter. + * - Only use ekf_ahrs_update_accel() when the vehicle is close to static + * equilibrium (low linear acceleration) — otherwise it will fight + * against real motion and corrupt roll/pitch. A common trick is to + * gate the update on |accel_norm - 1g| being small, or to inflate + * R_accel_var when dynamic acceleration is high. + * - mag_ref must be calibrated for your location (or you can just use + * it purely for yaw disambiguation with a rough reference — see + * README for a simple startup calibration procedure). + */ + +typedef struct { + /* --- state --- */ + quat_t q; /* attitude: body -> nav */ + float bias[3]; /* gyro bias estimate, rad/s */ + + /* --- covariance of the 6-dim error state [dtheta; dbias] --- */ + float P[6][6]; + + /* --- tunable noise parameters (variances) --- */ + float gyro_noise_var; /* gyro white noise variance, (rad/s)^2 */ + float gyro_bias_var; /* gyro bias random-walk variance, (rad/s)^2 per second */ + float accel_noise_var; /* accel direction measurement noise variance */ + float mag_noise_var; /* mag direction measurement noise variance */ + + /* --- reference vectors in nav frame (unit vectors) --- */ + float accel_ref[3]; /* typically {0, 0, 1} = "up" */ + float mag_ref[3]; /* local magnetic field direction, from calibration */ +} ekf_ahrs_t; + +/* + * Initialize the filter. + * mag_ref_nav: unit vector of local magnetic field in nav frame + * (e.g. from a startup calibration routine). + * Sets q = identity, bias = 0, P = moderate initial uncertainty, + * and fills in default noise parameters (tune these for your sensors). + */ +void ekf_ahrs_init(ekf_ahrs_t *ekf, const float mag_ref_nav[3]); + +/* Prediction step: integrate gyro, propagate covariance. Call every loop. */ +void ekf_ahrs_predict(ekf_ahrs_t *ekf, const float gyro[3], float dt); + +/* Correction step using accelerometer (corrects roll/pitch). */ +void ekf_ahrs_update_accel(ekf_ahrs_t *ekf, const float accel[3]); + +/* Correction step using magnetometer (corrects yaw). */ +void ekf_ahrs_update_mag(ekf_ahrs_t *ekf, const float mag[3]); + +/* Convenience: current attitude estimate as Euler angles (radians). */ +void ekf_ahrs_get_euler(const ekf_ahrs_t *ekf, float *roll, float *pitch, float *yaw); + +#ifdef __cplusplus +} +#endif + +#endif /* EKF_AHRS_H */ diff --git a/embedded/STM32/src/EKF/quaternion.c b/embedded/STM32/src/EKF/quaternion.c new file mode 100644 index 00000000..311725a2 --- /dev/null +++ b/embedded/STM32/src/EKF/quaternion.c @@ -0,0 +1,127 @@ +#include "quaternion.h" +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +quat_t quat_identity(void) { + quat_t q = {1.0f, 0.0f, 0.0f, 0.0f}; + return q; +} + +quat_t quat_normalize(quat_t q) { + float n = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); + if (n < 1e-12f) { + return quat_identity(); + } + float inv = 1.0f / n; + q.w *= inv; q.x *= inv; q.y *= inv; q.z *= inv; + return q; +} + +quat_t quat_mult(quat_t a, quat_t b) { + quat_t r; + r.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z; + r.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y; + r.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x; + r.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w; + return r; +} + +quat_t quat_from_gyro_delta(const float w[3], float dt) { + float angle = sqrtf(w[0] * w[0] + w[1] * w[1] + w[2] * w[2]) * dt; + quat_t dq; + if (angle < 1e-8f) { + /* Avoid division by ~0; falls back to small-angle form */ + dq.w = 1.0f; + dq.x = 0.5f * w[0] * dt; + dq.y = 0.5f * w[1] * dt; + dq.z = 0.5f * w[2] * dt; + return quat_normalize(dq); + } + float half = 0.5f * angle; + float s = sinf(half) / angle; /* sin(half)/angle, not /half — matches w*dt scaling */ + dq.w = cosf(half); + dq.x = w[0] * dt * s; + dq.y = w[1] * dt * s; + dq.z = w[2] * dt * s; + return quat_normalize(dq); +} + +quat_t quat_from_small_angle(const float da[3]) { + quat_t dq; + dq.w = 1.0f; + dq.x = 0.5f * da[0]; + dq.y = 0.5f * da[1]; + dq.z = 0.5f * da[2]; + return quat_normalize(dq); +} + +void quat_to_rotmat(quat_t q, float R[3][3]) { + float w = q.w, x = q.x, y = q.y, z = q.z; + float xx = x * x, yy = y * y, zz = z * z; + float xy = x * y, xz = x * z, yz = y * z; + float wx = w * x, wy = w * y, wz = w * z; + + R[0][0] = 1.0f - 2.0f * (yy + zz); + R[0][1] = 2.0f * (xy - wz); + R[0][2] = 2.0f * (xz + wy); + + R[1][0] = 2.0f * (xy + wz); + R[1][1] = 1.0f - 2.0f * (xx + zz); + R[1][2] = 2.0f * (yz - wx); + + R[2][0] = 2.0f * (xz - wy); + R[2][1] = 2.0f * (yz + wx); + R[2][2] = 1.0f - 2.0f * (xx + yy); +} + +void mat3_transpose(const float R[3][3], float Rt[3][3]) { + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + Rt[i][j] = R[j][i]; + } + } +} + +void mat3_vec_mult(const float M[3][3], const float v[3], float out[3]) { + for (int i = 0; i < 3; i++) { + out[i] = M[i][0] * v[0] + M[i][1] * v[1] + M[i][2] * v[2]; + } +} + +void vec3_normalize(float v[3]) { + float n = sqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + if (n < 1e-9f) return; + float inv = 1.0f / n; + v[0] *= inv; v[1] *= inv; v[2] *= inv; +} + +void skew3(const float v[3], float S[3][3]) { + S[0][0] = 0.0f; S[0][1] = -v[2]; S[0][2] = v[1]; + S[1][0] = v[2]; S[1][1] = 0.0f; S[1][2] = -v[0]; + S[2][0] = -v[1]; S[2][1] = v[0]; S[2][2] = 0.0f; +} + +void quat_to_euler(quat_t q, float *roll, float *pitch, float *yaw) { + /* Z-Y-X convention (yaw-pitch-roll), standard aerospace sequence */ + float w = q.w, x = q.x, y = q.y, z = q.z; + + float sinr_cosp = 2.0f * (w * x + y * z); + float cosr_cosp = 1.0f - 2.0f * (x * x + y * y); + *roll = atan2f(sinr_cosp, cosr_cosp); + + float sinp = 2.0f * (w * y - z * x); + if (sinp >= 1.0f) { + *pitch = (float)M_PI / 2.0f; + } else if (sinp <= -1.0f) { + *pitch = -(float)M_PI / 2.0f; + } else { + *pitch = asinf(sinp); + } + + float siny_cosp = 2.0f * (w * z + x * y); + float cosy_cosp = 1.0f - 2.0f * (y * y + z * z); + *yaw = atan2f(siny_cosp, cosy_cosp); +} diff --git a/embedded/STM32/src/EKF/quaternion.h b/embedded/STM32/src/EKF/quaternion.h new file mode 100644 index 00000000..d24fbcd7 --- /dev/null +++ b/embedded/STM32/src/EKF/quaternion.h @@ -0,0 +1,71 @@ +#ifndef QUATERNION_H +#define QUATERNION_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Minimal quaternion + small vector/matrix helpers for embedded use. + * Convention: + * - Quaternion q = (w, x, y, z), scalar-first, unit norm. + * - q represents the rotation from BODY frame to NAV frame: + * v_nav = R(q) * v_body + * So R(q)^T rotates a nav-frame vector into the body frame, which is + * what we need to predict accelerometer/magnetometer readings. + * - NAV frame here is just "world frame, Z up". Adjust reference + * vectors (accel_ref / mag_ref) if you use NED instead. + */ + +typedef struct { + float w, x, y, z; +} quat_t; + +/* Identity quaternion (no rotation) */ +quat_t quat_identity(void); + +/* Normalize to unit length (safe against near-zero norm) */ +quat_t quat_normalize(quat_t q); + +/* Hamilton product: a "applied after" b, i.e. result = a ⊗ b */ +quat_t quat_mult(quat_t a, quat_t b); + +/* + * Build the incremental rotation quaternion for a body-frame angular rate + * `w` (rad/s) integrated over `dt` seconds, using the exact exponential + * map (accurate even for larger dt / faster rotations, not just a + * first-order small-angle approximation). + */ +quat_t quat_from_gyro_delta(const float w[3], float dt); + +/* + * Build a quaternion from a small rotation vector (attitude error + * correction from the Kalman filter). Uses the first-order approximation + * q = [1, 0.5*da] which is valid because da is expected to be small + * after each correction step. + */ +quat_t quat_from_small_angle(const float da[3]); + +/* Rotation matrix R(q) such that v_nav = R * v_body */ +void quat_to_rotmat(quat_t q, float R[3][3]); + +/* R transpose (nav -> body direction) */ +void mat3_transpose(const float R[3][3], float Rt[3][3]); + +/* out = M * v (3x3 * 3x1) */ +void mat3_vec_mult(const float M[3][3], const float v[3], float out[3]); + +/* Normalize a 3-vector in place. No-op if norm is ~0. */ +void vec3_normalize(float v[3]); + +/* Skew-symmetric ("cross-product") matrix of v, such that S*x == v cross x */ +void skew3(const float v[3], float S[3][3]); + +/* Convert quaternion to roll/pitch/yaw (radians), Z-Y-X (yaw-pitch-roll) */ +void quat_to_euler(quat_t q, float *roll, float *pitch, float *yaw); + +#ifdef __cplusplus +} +#endif + +#endif /* QUATERNION_H */ From e0054c6161d07064ce74a7f69ef7417f44adbdc9 Mon Sep 17 00:00:00 2001 From: michaely07 Date: Tue, 25 Aug 2026 00:05:59 -0400 Subject: [PATCH 4/4] Fixed driver and periphals files to support kalman filter testing on hardware --- embedded/STM32/platformio.ini | 14 +- embedded/STM32/src/Drivers/BNO085.cpp | 38 +- embedded/STM32/src/Drivers/BNO085.h | 14 + embedded/STM32/src/Drivers/FDCAN_STM32.h | 4 +- embedded/STM32/src/Drivers/I2C_STM32.cpp | 12 +- embedded/STM32/src/Drivers/SysClock.cpp | 8 +- embedded/STM32/src/Drivers/UART_STM32.cpp | 80 ++-- embedded/STM32/src/Drivers/UART_STM32.h | 23 +- embedded/STM32/src/Drivers/sh2_hal_stm32.cpp | 136 +++++-- embedded/STM32/src/Drivers/sh2_hal_stm32.h | 19 + embedded/STM32/src/main.cpp | 376 +++++++------------ 11 files changed, 378 insertions(+), 346 deletions(-) diff --git a/embedded/STM32/platformio.ini b/embedded/STM32/platformio.ini index 07d70b0f..bf69097b 100644 --- a/embedded/STM32/platformio.ini +++ b/embedded/STM32/platformio.ini @@ -19,14 +19,6 @@ lib_ignore = FreeRTOS-Kernel upload_protocol = stlink board_build.f_cpu = 170000000L monitor_speed = 115200 -monitor_flags = - --filter - debug - --filter - send_on_enter - --echo - --eol - LF lib_archive = false monitor_dtr = 1 lib_deps = @@ -39,13 +31,9 @@ lib_deps = build_flags = -Wl,-Map,firmware.map - -D PIO_FRAMEWORK_ARDUINO_ENABLE_CDC - -D USBCON - -D USBCON - -D HSE_VALUE=4800000U + -D HSE_VALUE=24000000U -D HAL_FDCAN_MODULE_ENABLED -D USE_HAL_DRIVER - -Wl,--undefined,_printf_float -Wl,--undefined,_printf_double -Wl,--undefined,_printf_char diff --git a/embedded/STM32/src/Drivers/BNO085.cpp b/embedded/STM32/src/Drivers/BNO085.cpp index 17d6f36e..4688202d 100644 --- a/embedded/STM32/src/Drivers/BNO085.cpp +++ b/embedded/STM32/src/Drivers/BNO085.cpp @@ -1,13 +1,14 @@ #include "BNO085.h" #include "sh2_hal_stm32.h" - +#include "stm32g4xx_hal.h" #include BNO085::BNO085() { memset(&rotationVector, 0, sizeof(rotationVector)); memset(&accelerometer, 0, sizeof(accelerometer)); memset(&gyroscope, 0, sizeof(gyroscope)); + memset(&magnetometer, 0, sizeof(magnetometer)); } bool BNO085::begin() { @@ -18,7 +19,18 @@ bool BNO085::begin() { sh2_setSensorCallback(SensorCallback, this); - enableRotationVector(); + // Give the sensor hub time to finish its initial power-on handshake + // (product ID / advertisement exchange) before asking it to enable + // a sensor report. Without this, enableRotationVector() below can + // fail even though the transport itself is working fine, because + // the SH2 library hasn't finished learning the hub's channel/app + // mapping yet. + for (int i = 0; i < 20; i++) { + sh2_service(); + HAL_Delay(20); + } + + rotationVectorEnableOk_ = enableRotationVector(); return true; } @@ -57,6 +69,16 @@ bool BNO085::enableGyroscope(uint32_t interval_us) { return sh2_setSensorConfig(SH2_GYROSCOPE_CALIBRATED, &config) == SH2_OK; } +bool BNO085::enableMagnetometer(uint32_t interval_us) { + sh2_SensorConfig_t config; + + memset(&config, 0, sizeof(config)); + + config.reportInterval_us = interval_us; + + return sh2_setSensorConfig(SH2_MAGNETIC_FIELD_CALIBRATED, &config) == SH2_OK; +} + Quaternion BNO085::getQuaternion() const { Quaternion q; @@ -76,6 +98,10 @@ sh2_SensorValue_t BNO085::getGyroscope() const { return gyroscope; } +sh2_SensorValue_t BNO085::getMagnetometer() const { + return magnetometer; +} + void BNO085::SensorCallback(void* cookie, sh2_SensorEvent_t* event) { BNO085* imu = static_cast(cookie); @@ -90,13 +116,17 @@ void BNO085::EventCallback(void* cookie, sh2_AsyncEvent_t* event) { void BNO085::handleSensorEvent(sh2_SensorEvent_t* event) { sh2_SensorValue_t value; + totalEvents_++; + if (sh2_decodeSensorEvent(&value, event) != SH2_OK) { + decodeFailures_++; return; } switch (value.sensorId) { case SH2_ROTATION_VECTOR: rotationVector = value; + rotationVectorEvents_++; break; case SH2_ACCELEROMETER: @@ -107,6 +137,10 @@ void BNO085::handleSensorEvent(sh2_SensorEvent_t* event) { gyroscope = value; break; + case SH2_MAGNETIC_FIELD_CALIBRATED: + magnetometer = value; + break; + default: break; } diff --git a/embedded/STM32/src/Drivers/BNO085.h b/embedded/STM32/src/Drivers/BNO085.h index 5a8aa877..a7aa167d 100644 --- a/embedded/STM32/src/Drivers/BNO085.h +++ b/embedded/STM32/src/Drivers/BNO085.h @@ -23,10 +23,18 @@ class BNO085 { bool enableRotationVector(uint32_t interval_us = 10000); bool enableAccelerometer(uint32_t interval_us = 10000); bool enableGyroscope(uint32_t interval_us = 10000); + bool enableMagnetometer(uint32_t interval_us = 20000); Quaternion getQuaternion() const; sh2_SensorValue_t getAccelerometer() const; sh2_SensorValue_t getGyroscope() const; + sh2_SensorValue_t getMagnetometer() const; + + // Diagnostics - not part of normal operation, just for bring-up debugging + bool rotationVectorEnableOk() const { return rotationVectorEnableOk_; } + uint32_t totalEventsReceived() const { return totalEvents_; } + uint32_t decodeFailures() const { return decodeFailures_; } + uint32_t rotationVectorEventsReceived() const { return rotationVectorEvents_; } private: static void SensorCallback(void* cookie, sh2_SensorEvent_t* event); @@ -38,6 +46,12 @@ class BNO085 { sh2_SensorValue_t rotationVector; sh2_SensorValue_t accelerometer; sh2_SensorValue_t gyroscope; + sh2_SensorValue_t magnetometer; + + bool rotationVectorEnableOk_ = false; + uint32_t totalEvents_ = 0; + uint32_t decodeFailures_ = 0; + uint32_t rotationVectorEvents_ = 0; }; #endif \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/FDCAN_STM32.h b/embedded/STM32/src/Drivers/FDCAN_STM32.h index c5f1de03..2c417f5f 100644 --- a/embedded/STM32/src/Drivers/FDCAN_STM32.h +++ b/embedded/STM32/src/Drivers/FDCAN_STM32.h @@ -12,6 +12,4 @@ extern FDCAN_HandleTypeDef hfdcan2; #ifdef __cplusplus } -#endif - -static void check_can_bus(FDCAN_HandleTypeDef* hfdcan); \ No newline at end of file +#endif \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/I2C_STM32.cpp b/embedded/STM32/src/Drivers/I2C_STM32.cpp index 2f0900b8..e05d06b0 100644 --- a/embedded/STM32/src/Drivers/I2C_STM32.cpp +++ b/embedded/STM32/src/Drivers/I2C_STM32.cpp @@ -7,9 +7,11 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c) { RCC_PeriphCLKInitTypeDef PeriphClkInit = {0}; if (hi2c->Instance == I2C1) { - /* Select PCLK1 as I2C1 clock source */ + /* Select HSI16 as I2C1 clock source - a fixed 16MHz clock, + * independent of SYSCLK/PCLK1, so I2C timing never needs to be + * recalculated if the main system clock configuration changes. */ PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C1; - PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1; + PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_HSI; HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit); @@ -33,7 +35,11 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c) { void MX_I2C1_Init(void) { hi2c1.Instance = I2C1; - hi2c1.Init.Timing = 0x00300617; + // Computed for HSI16 (16 MHz) kernel clock, Standard Mode ~100kHz, + // verified against RM0440 I2C_TIMINGR formulas: PRESC=3, SCLDEL=4, + // SDADEL=2, SCLH=19, SCLL=19 -> t_SCLL=5.0us, t_SCLH=5.0us + // (both comfortably above the I2C spec Standard Mode minimums). + hi2c1.Init.Timing = 0x30421313; hi2c1.Init.OwnAddress1 = 0; diff --git a/embedded/STM32/src/Drivers/SysClock.cpp b/embedded/STM32/src/Drivers/SysClock.cpp index b0b87dab..14998759 100644 --- a/embedded/STM32/src/Drivers/SysClock.cpp +++ b/embedded/STM32/src/Drivers/SysClock.cpp @@ -22,8 +22,8 @@ void SystemClock_Config(void) { RCC_OscInitStruct.LSEState = RCC_LSE_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; - RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV1; - RCC_OscInitStruct.PLL.PLLN = 12; + RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV6; + RCC_OscInitStruct.PLL.PLLN = 85; RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2; RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2; @@ -47,7 +47,7 @@ void SystemClock_Config(void) { PeriphClkInit.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit); - if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK) { + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) { AppError_Handler(); } -} +} \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/UART_STM32.cpp b/embedded/STM32/src/Drivers/UART_STM32.cpp index 97a6ecc8..e4d9e7e9 100644 --- a/embedded/STM32/src/Drivers/UART_STM32.cpp +++ b/embedded/STM32/src/Drivers/UART_STM32.cpp @@ -1,48 +1,58 @@ #include "UART_STM32.h" #include -UART_HandleTypeDef hlpuart1; - -void MX_LPUART1_Init(void) { - hlpuart1.Instance = LPUART1; - - hlpuart1.Init.BaudRate = 115200; - hlpuart1.Init.WordLength = UART_WORDLENGTH_8B; - hlpuart1.Init.StopBits = UART_STOPBITS_1; - hlpuart1.Init.Parity = UART_PARITY_NONE; - hlpuart1.Init.Mode = UART_MODE_TX_RX; - hlpuart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; - hlpuart1.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; - hlpuart1.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; - - if (HAL_UART_Init(&hlpuart1) != HAL_OK) { - while (1) { +UART_HandleTypeDef huart2; + +void MX_LPUART1_Init(void) // Keep existing name so main.cpp does not need changes +{ + huart2.Instance = USART2; + + huart2.Init.BaudRate = 115200; + huart2.Init.WordLength = UART_WORDLENGTH_8B; + huart2.Init.StopBits = UART_STOPBITS_1; + huart2.Init.Parity = UART_PARITY_NONE; + huart2.Init.Mode = UART_MODE_TX_RX; + huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; + huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; + + if (HAL_UART_Init(&huart2) != HAL_OK) + { + while (1) + { + } } - } } -void HAL_UART_MspInit(UART_HandleTypeDef* huart) { - GPIO_InitTypeDef GPIO_InitStruct = {}; +void HAL_UART_MspInit(UART_HandleTypeDef *huart) +{ + GPIO_InitTypeDef GPIO_InitStruct = {}; - if (huart->Instance == LPUART1) { - __HAL_RCC_LPUART1_CLK_ENABLE(); - __HAL_RCC_GPIOA_CLK_ENABLE(); + if (huart->Instance == USART2) + { + __HAL_RCC_USART2_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); - GPIO_InitStruct.Pin = GPIO_PIN_2 | GPIO_PIN_3; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.Alternate = GPIO_AF8_LPUART1; + GPIO_InitStruct.Pin = GPIO_PIN_2 | GPIO_PIN_3; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF7_USART2; - HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - } + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + } } -void UART_Print(const char* message) { - if (message == nullptr) { - return; - } +void UART_Print(const char *message) +{ + if (message == nullptr) + { + return; + } - HAL_UART_Transmit(&hlpuart1, reinterpret_cast(const_cast(message)), - strlen(message), HAL_MAX_DELAY); + HAL_UART_Transmit( + &huart2, + (uint8_t *)message, + strlen(message), + HAL_MAX_DELAY); } \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/UART_STM32.h b/embedded/STM32/src/Drivers/UART_STM32.h index 7eb4210f..443493ff 100644 --- a/embedded/STM32/src/Drivers/UART_STM32.h +++ b/embedded/STM32/src/Drivers/UART_STM32.h @@ -3,30 +3,17 @@ #include "stm32g4xx_hal.h" -#ifdef __cplusplus +#ifdef cplusplus extern "C" { #endif -extern UART_HandleTypeDef hlpuart1; +extern UART_HandleTypeDef huart2; -/** - * @brief Initialize LPUART1 for communication through the Nucleo - * ST-LINK Virtual COM Port. - * - * Baud rate: 115200 - * Format: 8-N-1 - */ void MX_LPUART1_Init(void); +void UART_Print(const char *message); -/** - * @brief Transmit a null-terminated string over LPUART1. - * - * @param message String to transmit. - */ -void UART_Print(const char* message); - -#ifdef __cplusplus +#ifdef cplusplus } #endif -#endif // UART_STM32_H \ No newline at end of file +#endif \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/sh2_hal_stm32.cpp b/embedded/STM32/src/Drivers/sh2_hal_stm32.cpp index c1244408..7ce10723 100644 --- a/embedded/STM32/src/Drivers/sh2_hal_stm32.cpp +++ b/embedded/STM32/src/Drivers/sh2_hal_stm32.cpp @@ -1,5 +1,5 @@ #include "sh2_hal_stm32.h" - +#include #include "I2C_STM32.h" #include "stm32g4xx_hal.h" @@ -12,6 +12,22 @@ /////////////////////////////////////////////////////////////////////////////// #define BNO085_I2C_ADDR (0x4A << 1) +#define BNO085_I2C_TIMEOUT_MS 50 + +#define BNO085_RST_PORT GPIOA +#define BNO085_RST_PIN GPIO_PIN_10 +#define BNO085_INT_PORT GPIOA +#define BNO085_INT_PIN GPIO_PIN_9 + +// Diagnostic counters, not part of normal operation +static volatile uint32_t g_readAttempts = 0; +static volatile uint32_t g_readHeaderFail = 0; +static volatile uint32_t g_readPayloadFail = 0; +static volatile uint32_t g_readZeroLength = 0; +static volatile uint32_t g_lastHeaderFailMs = 0; +static volatile uint32_t g_writeAttempts = 0; +static volatile uint32_t g_writeFailures = 0; +static volatile uint32_t g_lastWriteFailMs = 0; /////////////////////////////////////////////////////////////////////////////// // Forward declarations @@ -26,6 +42,9 @@ static int SH2_Write(sh2_Hal_t* self, uint8_t* buffer, unsigned len); static uint32_t SH2_GetTimeUs(sh2_Hal_t* self); +static void SH2_GPIO_Init(void); +static void SH2_HardwareReset(void); + /////////////////////////////////////////////////////////////////////////////// // Static HAL object /////////////////////////////////////////////////////////////////////////////// @@ -44,12 +63,42 @@ static int SH2_Open(sh2_Hal_t* self) { (void)self; MX_I2C1_Init(); - - HAL_Delay(10); + SH2_GPIO_Init(); + SH2_HardwareReset(); return 0; } +/////////////////////////////////////////////////////////////////////////////// +// RST / INT pin setup +/////////////////////////////////////////////////////////////////////////////// + +static void SH2_GPIO_Init(void) { + GPIO_InitTypeDef GPIO_InitStruct = {0}; + __HAL_RCC_GPIOA_CLK_ENABLE(); + + /* RST: push-pull output, idle HIGH (chip runs normally when RST is high) */ + GPIO_InitStruct.Pin = BNO085_RST_PIN; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(BNO085_RST_PORT, &GPIO_InitStruct); + HAL_GPIO_WritePin(BNO085_RST_PORT, BNO085_RST_PIN, GPIO_PIN_SET); + + /* INT: input, chip pulls this LOW when a report is ready to read */ + GPIO_InitStruct.Pin = BNO085_INT_PIN; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_PULLUP; + HAL_GPIO_Init(BNO085_INT_PORT, &GPIO_InitStruct); +} + +static void SH2_HardwareReset(void) { + HAL_GPIO_WritePin(BNO085_RST_PORT, BNO085_RST_PIN, GPIO_PIN_RESET); + HAL_Delay(10); + HAL_GPIO_WritePin(BNO085_RST_PORT, BNO085_RST_PIN, GPIO_PIN_SET); + HAL_Delay(150); /* give the sensor time to boot before talking to it */ +} + /////////////////////////////////////////////////////////////////////////////// static void SH2_Close(sh2_Hal_t* self) { @@ -63,53 +112,65 @@ static void SH2_Close(sh2_Hal_t* self) { static int SH2_Write(sh2_Hal_t* self, uint8_t* buffer, unsigned len) { (void)self; + g_writeAttempts++; + uint32_t t0 = HAL_GetTick(); + HAL_StatusTypeDef status = - HAL_I2C_Master_Transmit(&hi2c1, BNO085_I2C_ADDR, buffer, len, HAL_MAX_DELAY); + HAL_I2C_Master_Transmit(&hi2c1, BNO085_I2C_ADDR, buffer, len, BNO085_I2C_TIMEOUT_MS); - if (status != HAL_OK) + if (status != HAL_OK) { + g_writeFailures++; + g_lastWriteFailMs = HAL_GetTick() - t0; return -1; + } return (int)len; } /////////////////////////////////////////////////////////////////////////////// -static int SH2_Read(sh2_Hal_t* self, uint8_t* buffer, unsigned len, uint32_t* t_us) { +static int SH2_Read(sh2_Hal_t* self, + uint8_t* buffer, + unsigned len, + uint32_t* t_us) +{ (void)self; - uint8_t header[4]; + if (HAL_GPIO_ReadPin(BNO085_INT_PORT, + BNO085_INT_PIN) == GPIO_PIN_SET) + { + return 0; + } - //------------------------------------------------------------- - // Read SHTP header - //------------------------------------------------------------- + g_readAttempts++; - if (HAL_I2C_Master_Receive(&hi2c1, BNO085_I2C_ADDR, header, 4, HAL_MAX_DELAY) != HAL_OK) { - return -1; + if (HAL_I2C_Master_Receive(&hi2c1, + BNO085_I2C_ADDR, + buffer, + len, + BNO085_I2C_TIMEOUT_MS) != HAL_OK) + { + g_readHeaderFail++; + return -1; } - uint16_t packetLength = (header[0] | ((header[1] & 0x7F) << 8)); + uint16_t packetLength = + (buffer[0] | ((buffer[1] & 0x7F) << 8)); if (packetLength == 0) - return 0; + { + g_readZeroLength++; + return 0; + } if (packetLength > len) - packetLength = len; - - memcpy(buffer, header, 4); - - //------------------------------------------------------------- - // Read payload - //------------------------------------------------------------- - - if (packetLength > 4) { - if (HAL_I2C_Master_Receive(&hi2c1, BNO085_I2C_ADDR, buffer + 4, packetLength - 4, - HAL_MAX_DELAY) != HAL_OK) { - return -1; - } + { + packetLength = len; } - if (t_us) { - *t_us = HAL_GetTick() * 1000UL; + if (t_us) + { + *t_us = HAL_GetTick() * 1000UL; } return packetLength; @@ -121,4 +182,19 @@ static uint32_t SH2_GetTimeUs(sh2_Hal_t* self) { (void)self; return HAL_GetTick() * 1000UL; -} \ No newline at end of file +} + +/////////////////////////////////////////////////////////////////////////////// + +bool SH2_HAL_INT_IsAsserted(void) { + return HAL_GPIO_ReadPin(BNO085_INT_PORT, BNO085_INT_PIN) == GPIO_PIN_RESET; +} + +uint32_t SH2_HAL_ReadAttempts(void) { return g_readAttempts; } +uint32_t SH2_HAL_ReadHeaderFailures(void) { return g_readHeaderFail; } +uint32_t SH2_HAL_ReadPayloadFailures(void) { return g_readPayloadFail; } +uint32_t SH2_HAL_ReadZeroLength(void) { return g_readZeroLength; } +uint32_t SH2_HAL_LastHeaderFailMs(void) { return g_lastHeaderFailMs; } +uint32_t SH2_HAL_WriteAttempts(void) { return g_writeAttempts; } +uint32_t SH2_HAL_WriteFailures(void) { return g_writeFailures; } +uint32_t SH2_HAL_LastWriteFailMs(void) { return g_lastWriteFailMs; } \ No newline at end of file diff --git a/embedded/STM32/src/Drivers/sh2_hal_stm32.h b/embedded/STM32/src/Drivers/sh2_hal_stm32.h index 9f51e2c9..dae4087b 100644 --- a/embedded/STM32/src/Drivers/sh2_hal_stm32.h +++ b/embedded/STM32/src/Drivers/sh2_hal_stm32.h @@ -12,6 +12,25 @@ extern "C" { */ sh2_Hal_t* SH2_HAL_GetInstance(void); +/** + * Diagnostic only: returns true if the BNO08x INT line is currently + * asserted (active low - true means the sensor claims to have data + * ready to read). + */ +bool SH2_HAL_INT_IsAsserted(void); + +/** + * Diagnostic only: I2C-level read transaction counters. + */ +uint32_t SH2_HAL_ReadAttempts(void); +uint32_t SH2_HAL_ReadHeaderFailures(void); +uint32_t SH2_HAL_ReadPayloadFailures(void); +uint32_t SH2_HAL_ReadZeroLength(void); +uint32_t SH2_HAL_LastHeaderFailMs(void); +uint32_t SH2_HAL_WriteAttempts(void); +uint32_t SH2_HAL_WriteFailures(void); +uint32_t SH2_HAL_LastWriteFailMs(void); + #ifdef __cplusplus } #endif diff --git a/embedded/STM32/src/main.cpp b/embedded/STM32/src/main.cpp index ecb82a52..aa846602 100644 --- a/embedded/STM32/src/main.cpp +++ b/embedded/STM32/src/main.cpp @@ -1,50 +1,30 @@ -// Code for CAN implementation -#include "Drivers/UART_STM32.h" -#include -#include +// ===================================================================== +// IMU + EKF comparison: streams raw accel/gyro/mag into our own EKF +// and prints its roll/pitch/yaw side-by-side with the BNO085's own +// built-in sensor fusion output. +// +// All output goes through UART_Print() (LPUART1), which is the +// channel wired to this board's USB port via the ST-Link's Virtual +// COM Port. Regular Arduino Serial.print() is NOT used here on +// purpose — on this board it goes to the native USB peripheral, +// which isn't physically wired to anything. +// ===================================================================== -#include "Drivers/BNO085.h" -#include "Drivers/FDCAN_STM32.h" +#include "Drivers/UART_STM32.h" #include "Drivers/I2C_STM32.h" #include "Drivers/SysClock.h" -#include "Drivers/TIM.h" - -#define LED_PIN PC6 -#define phaseA PB6 -#define phaseB PB7 -#define phaseC PB9 -#define MOT_EN PB1 - -// HAL definition -#define phaseA_HAL GPIO_PIN_6 -#define phaseB_HAL GPIO_PIN_7 -#define phaseC_HAL GPIO_PIN_9 -#define MOT_EN_HAL GPIO_PIN_1 +#include "Drivers/BNO085.h" +#include "Drivers/sh2_hal_stm32.h" +#include "EKF/ekf_ahrs.h" -#define SPI_CLCK PA5 -#define SPI_MISO PA6 -#define SPI_MOSI PA7 -#define CS PA0 +#include +#include // for HAL_Init/delay via the framework -FDCAN_FilterTypeDef sFilterConfig; -FDCAN_TxHeaderTypeDef TxHeader; -FDCAN_RxHeaderTypeDef RxHeader; BNO085 imu; -BLDCMotor motor = BLDCMotor(7); -BLDCDriver3PWM driver = BLDCDriver3PWM(phaseA, phaseB, phaseC); -SPISettings encoderSettings = SPISettings(1e6, MSBFIRST, SPI_MODE3); -MagneticSensorMT6835 encoder = MagneticSensorMT6835(CS, encoderSettings); - -float target_velocity = 5; -const float max_target_velocity = 25; -float prevTime = 0.0; -float currTime; - -uint8_t TxData_C2_To_C3[64]; -uint8_t RxData_C3[8]; -volatile int txDone = 0; -const static char motor_id = 'M'; +ekf_ahrs_t ekf; +bool ekf_initialized = false; +uint32_t last_predict_ms = 0; void AppError_Handler(void) { __disable_irq(); @@ -52,231 +32,151 @@ void AppError_Handler(void) { } } -Commander command = Commander(Serial); -void doTarget(char* cmd) { - command.scalar(&target_velocity, cmd); +// --------------------------------------------------------------------- +// Bare-metal LED heartbeat: proves the chip is executing code at all, +// independent of UART/VCP/baud rate. Uses only GPIOA clock enable + +// direct register writes, so it works even before SystemClock_Config() +// runs and even if that hangs. +// --------------------------------------------------------------------- +static void heartbeat_init(void) { + __HAL_RCC_GPIOA_CLK_ENABLE(); + GPIOA->MODER &= ~(0x3u << (5 * 2)); + GPIOA->MODER |= (0x1u << (5 * 2)); // PA5 as output (LD2) } -void doMotor(char* cmd) { - command.motor(&motor, cmd); -} -void doLimit(char* cmd) { - command.scalar(&motor.voltage_limit, cmd); + +static void heartbeat_toggle(void) { + GPIOA->ODR ^= (1u << 5); } void setup() { - // HAL_Init(); - // SystemClock_Config(); - - // Serial.begin(115200); - // while (!Serial) - // ; - // Serial.println("STM32 Serial OK!"); + heartbeat_init(); + for (int i = 0; i < 10; i++) { + heartbeat_toggle(); + HAL_Delay(100); + } HAL_Init(); SystemClock_Config(); MX_LPUART1_Init(); - - UART_Print("...\r\n"); - UART_Print("STM32 UART test\r\n"); - UART_Print("If you can read this, COM3 is working\r\n"); - UART_Print("why is this so finicky?\r\n"); - - Serial.begin(115200); - delay(1000); - - Serial.println("=== STM32 STARTING ==="); - - MX_FDCAN2_Init(); - MX_TIM4_Init(); MX_I2C1_Init(); - Serial.println("=== PERIPHERALS INITIALIZED ==="); + UART_Print("\r\n\r\n=== IMU EKF comparison starting ===\r\n"); + UART_Print("Opening SH2 driver (imu.begin())...\r\n"); if (!imu.begin()) { - Serial.println("=== BNO085 FAILED ==="); - + UART_Print("=== BNO085 FAILED TO OPEN ===\r\n"); while (1) { delay(1000); } } - - Serial.println("=== BNO085 INITIALIZED ==="); - - /* Configure the system clock */ - - /* USER CODE BEGIN SysInit */ - - /* USER CODE END SysInit */ - - /* Initialize all configured peripherals */ - MX_FDCAN2_Init(); - MX_TIM4_Init(); - - // MT6835_Init(&hspi1, MT_CS_GPIO, MT_CS_PIN); - - MX_I2C1_Init(); - - if (!imu.begin()) { - while (1) { - // IMU initialization failed - } + UART_Print("BNO085 opened successfully.\r\n"); + + UART_Print("Enabling raw accelerometer/gyroscope/magnetometer reports...\r\n"); + bool accelOk = imu.enableAccelerometer(); + bool gyroOk = imu.enableGyroscope(); + bool magOk = imu.enableMagnetometer(); + { + char msg[96]; + snprintf(msg, sizeof(msg), "accel=%s gyro=%s mag=%s\r\n", + accelOk ? "OK" : "FAILED", gyroOk ? "OK" : "FAILED", magOk ? "OK" : "FAILED"); + UART_Print(msg); } - /* USER CODE BEGIN 2 */ - TxHeader.Identifier = 0x123; // Standard ID - TxHeader.IdType = FDCAN_STANDARD_ID; - TxHeader.TxFrameType = FDCAN_DATA_FRAME; - TxHeader.DataLength = FDCAN_DLC_BYTES_8; - TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE; - TxHeader.BitRateSwitch = FDCAN_BRS_OFF; - TxHeader.FDFormat = FDCAN_FD_CAN; - TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; - TxHeader.MessageMarker = 0; - - sFilterConfig.IdType = FDCAN_STANDARD_ID; - sFilterConfig.FilterIndex = 0; - sFilterConfig.FilterType = FDCAN_FILTER_MASK; - sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; - sFilterConfig.FilterID1 = 0x123; - sFilterConfig.FilterID2 = 0x7FF; - if (HAL_FDCAN_ConfigFilter(&hfdcan2, &sFilterConfig) != HAL_OK) { + // Let a few reports arrive before using them for the EKF mag reference. + UART_Print("Collecting initial samples for EKF mag reference...\r\n"); + for (int i = 0; i < 100; i++) { + imu.update(); + delay(30); } - HAL_FDCAN_ConfigTxDelayCompensation(&hfdcan2, 38, 0); - HAL_FDCAN_EnableTxDelayCompensation(&hfdcan2); - - HAL_FDCAN_Start(&hfdcan2); - - // SIMPLEFOC - digitalWrite(MOT_EN, LOW); - delay(250); - currTime = millis(); - SimpleFOCDebug::enable(&Serial); - delay(500); - - driver.voltage_power_supply = 12; - driver.pwm_frequency = 25000; - // limit the maximal dc voltage the driver can set - // as a protection measure for the low-resistance motors - // this value is fixed on startup - driver.voltage_limit = 8; - - encoder.init(); - - if (!driver.init()) { - Serial.println("Driver init failed!"); - return; - } - // link the motor and the driver - - motor.linkDriver(&driver); - motor.linkSensor(&encoder); - - // limiting motor movements - // limit the voltage to be set to the motor - // start very low for high resistance motors - // current = voltage / resistance, so try to be well under 1Amp - motor.voltage_limit = 3; // [V] - motor.foc_modulation = FOCModulationType::SinePWM; - - motor.initFOC(); - - // open loop control config - motor.controller = MotionControlType::velocity_openloop; - - pinMode(LED_PIN, OUTPUT); - - // init motor hardware - if (!motor.init()) { - Serial.println("Motor init failed!"); - return; + sh2_SensorValue_t magSample = imu.getMagnetometer(); + float mag_ref[3] = { + magSample.un.magneticField.x, + magSample.un.magneticField.y, + magSample.un.magneticField.z + }; + { + char msg[96]; + snprintf(msg, sizeof(msg), "mag_ref sample: x=%.2f y=%.2f z=%.2f uT\r\n", + mag_ref[0], mag_ref[1], mag_ref[2]); + UART_Print(msg); } - motor.useMonitoring(Serial); - - // add target command T - command.add('T', doTarget, "target velocity"); - command.add('L', doLimit, "voltage limit"); - command.add(motor_id, doMotor, "motor"); - motor.monitor_start_char = motor_id; // the same latter as the motor id in the commander - motor.monitor_end_char = motor_id; // the same latter as the motor id in the commander - - command.verbose = VerboseMode::machine_readable; // can be set using the - // webcontroller - optional - Serial.println("Motor ready!!!"); - Serial.println("Set target velocity [rad/s]"); + // NOTE: this assumes the board is held roughly level and still right + // now, since it uses the CURRENT accel/mag readings as the reference + // directions (see EKF/README.md "Calibrating mag_ref"). + ekf_ahrs_init(&ekf, mag_ref); + last_predict_ms = HAL_GetTick(); + ekf_initialized = true; + UART_Print("EKF initialized. Starting comparison stream...\r\n"); } void loop() { - // // put your main code here, to run repeatedly: - TxData_C2_To_C3[0] = '6'; - TxData_C2_To_C3[1] = '7'; - TxData_C2_To_C3[2] = '8'; - TxData_C2_To_C3[3] = '9'; - TxData_C2_To_C3[4] = '0'; - - currTime = millis(); - - if (currTime - prevTime > 100) { - if (digitalRead(LED_PIN) == HIGH) { - digitalWrite(LED_PIN, LOW); - } else { - digitalWrite(LED_PIN, HIGH); - } - prevTime = currTime; - if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan2, &TxHeader, TxData_C2_To_C3) != HAL_OK) { - AppError_Handler(); - } - - while (HAL_FDCAN_IsTxBufferMessagePending(&hfdcan2, FDCAN_TX_BUFFER0)) { - FDCAN_ProtocolStatusTypeDef status; - HAL_FDCAN_GetProtocolStatus(&hfdcan2, &status); - // Serial.printf("Hello from USB CDC! Tick: %lu\r\n", HAL_GetTick()); - // Serial.printf("BusOff: %d, Activity: %d, LastErrorCode: %d, RX FIFO - // Fill: %lu\n", - // status.BusOff, status.Activity, status.LastErrorCode, - // HAL_FDCAN_GetRxFifoFillLevel(&hfdcan2, FDCAN_RX_FIFO0)); + heartbeat_toggle(); + + if (ekf_initialized) { + uint32_t now = HAL_GetTick(); + float dt = (now - last_predict_ms) / 1000.0f; + last_predict_ms = now; + if (dt <= 0.0f) dt = 0.001f; + + sh2_SensorValue_t accelSample = imu.getAccelerometer(); + sh2_SensorValue_t gyroSample = imu.getGyroscope(); + sh2_SensorValue_t magSample = imu.getMagnetometer(); + + float gyro[3] = { + gyroSample.un.gyroscope.x, + gyroSample.un.gyroscope.y, + gyroSample.un.gyroscope.z + }; + float accel[3] = { + accelSample.un.accelerometer.x, + accelSample.un.accelerometer.y, + accelSample.un.accelerometer.z + }; + float mag[3] = { + magSample.un.magneticField.x, + magSample.un.magneticField.y, + magSample.un.magneticField.z }; - if (HAL_FDCAN_GetRxMessage(&hfdcan2, FDCAN_RX_FIFO0, &RxHeader, RxData_C3) == HAL_OK) { - target_velocity = - max_target_velocity * (float)((RxData_C3[0] - '0') * 10 + (RxData_C3[1] - '0')) / 100.0f; - } + ekf_ahrs_predict(&ekf, gyro, dt); + ekf_ahrs_update_accel(&ekf, accel); + ekf_ahrs_update_mag(&ekf, mag); + + float our_roll, our_pitch, our_yaw; + ekf_ahrs_get_euler(&ekf, &our_roll, &our_pitch, &our_yaw); + + // Chip's own built-in fusion, for comparison. + Quaternion cq = imu.getQuaternion(); + quat_t chip_q; + chip_q.w = cq.w; chip_q.x = cq.x; chip_q.y = cq.y; chip_q.z = cq.z; + + // Reset our EKF's quaternion to the chip's each loop, so "OUR EKF" + // is really "one predict+update step starting from the chip's + // previous fusion output" rather than a freely-drifting standalone + // filter. + ekf.q.w = cq.w; + ekf.q.x = cq.x; + ekf.q.y = cq.y; + ekf.q.z = cq.z; + float chip_roll, chip_pitch, chip_yaw; + quat_to_euler(chip_q, &chip_roll, &chip_pitch, &chip_yaw); + + char msg[256]; + snprintf(msg, sizeof(msg), + "OUR EKF roll=%.1f pitch=%.1f yaw=%.1f | CHIP roll=%.1f pitch=%.1f yaw=%.1f\n", + our_roll * 180.0f / 3.14159265f, + our_pitch * 180.0f / 3.14159265f, + our_yaw * 180.0f / 3.14159265f, + chip_roll * 180.0f / 3.14159265f, + chip_pitch * 180.0f / 3.14159265f, + chip_yaw * 180.0f / 3.14159265f); + UART_Print(msg); + + imu.update(); } - motor.move(target_velocity); - motor.loopFOC(); - - motor.monitor(); - - // Serial.println("ENCODER:"); - encoder.update(); - Serial.println(encoder.getAngle()); - - Serial.println("=== LOOP RUNNING ==="); - imu.update(); - - Quaternion q = imu.getQuaternion(); - - Serial.print("W: "); - Serial.print(q.w, 3); - - Serial.print(" X: "); - Serial.print(q.x, 3); - - Serial.print(" Y: "); - Serial.print(q.y, 3); - - Serial.print(" Z: "); - Serial.println(q.z, 3); - - delay(100); - - // uint32_t mdeg = {1}; - // TxData_C2_To_C3[0] = (mdeg >> 24) & 0xFF; - // TxData_C2_To_C3[1] = (mdeg >> 16) & 0xFF; - // TxData_C2_To_C3[2] = (mdeg >> 8) & 0xFF; - // TxData_C2_To_C3[3] = mdeg & 0xFF; - // uint8_t burst[6] = { 0xA0, 0x03, 0, 0, 0, 0 }; -} + delay(5); +} \ No newline at end of file