Kalman filter

embedded/STM32/platformio.ini

@@ -15,17 +15,10 @@ 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
-monitor_flags = 
-	--filter
-	debug
-	--filter
-	send_on_enter
-	--echo
-	--eol
-	LF
 lib_archive = false
 monitor_dtr = 1
 lib_deps = 
@@ -34,16 +27,13 @@ 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
-	-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

embedded/STM32/src/Drivers/BNO085.cpp

@@ -0,0 +1,147 @@
+#include "BNO085.h"
+
+#include "sh2_hal_stm32.h"
+#include "stm32g4xx_hal.h"
+#include <cstring>
+
+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() {
+  int status = sh2_open(SH2_HAL_GetInstance(), EventCallback, this);
+
+  if (status != SH2_OK)
+    return false;
+
+  sh2_setSensorCallback(SensorCallback, this);
+
+  // 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;
+}
+
+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;
+}
+
+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;
+
+  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 {
+  return accelerometer;
+}
+
+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<BNO085*>(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;
+
+  totalEvents_++;
+
+  if (sh2_decodeSensorEvent(&value, event) != SH2_OK) {
+    decodeFailures_++;
+    return;
+  }
+
+  switch (value.sensorId) {
+  case SH2_ROTATION_VECTOR:
+    rotationVector = value;
+    rotationVectorEvents_++;
+    break;
+
+  case SH2_ACCELEROMETER:
+    accelerometer = value;
+    break;
+
+  case SH2_GYROSCOPE_CALIBRATED:
+    gyroscope = value;
+    break;
+
+  case SH2_MAGNETIC_FIELD_CALIBRATED:
+    magnetometer = value;
+    break;
+
+  default:
+    break;
+  }
+}

embedded/STM32/src/Drivers/BNO085.h

@@ -0,0 +1,57 @@
+#ifndef BNO085_H
+#define BNO085_H
+
+#include "sh2.h"
+#include "sh2_err.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);
+  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);
+
+  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;
+  sh2_SensorValue_t magnetometer;
+
+  bool rotationVectorEnableOk_ = false;
+  uint32_t totalEvents_ = 0;
+  uint32_t decodeFailures_ = 0;
+  uint32_t rotationVectorEvents_ = 0;
+};
+
+#endif

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 */

embedded/STM32/src/Drivers/FDCAN_STM32.h

@@ -1,4 +1,4 @@
-#include <stm32g4xx_hal_fdcan.h>
+#include <stm32g4xx_hal.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -7,10 +7,9 @@ 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
 }
-#endif
-
-static void check_can_bus(FDCAN_HandleTypeDef* hfdcan);
+#endif

embedded/STM32/src/Drivers/I2C_STM32.cpp

@@ -0,0 +1,69 @@
+#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 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_HSI;
+
+    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;
+
+  // 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;
+
+  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) {
+    AppError_Handler();
+  }
+
+  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK) {
+    AppError_Handler();
+  }
+
+  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK) {
+    AppError_Handler();
+  }
+}

embedded/STM32/src/Drivers/I2C_STM32.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "stm32g4xx_hal.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern I2C_HandleTypeDef hi2c1;
+
+void MX_I2C1_Init(void);
+void AppError_Handler(void);
+
+#ifdef __cplusplus
+}
+#endif