Kalman filter - #204
Open
michaely07 wants to merge 4 commits into
Open
Conversation
Contributor
There was a problem hiding this comment.
Pull request overview
This PR replaces the previous motor/CAN test application with an IMU bring-up + attitude-estimation comparison app for the BNO085, adding a small embedded MEKF/EKF AHRS implementation and STM32 I2C/UART SH2 transport glue.
Changes:
- Add BNO085 driver + STM32 SH2 HAL over I2C, plus UART output helper for streaming telemetry over ST-Link VCP.
- Add minimal quaternion math utilities and an error-state EKF AHRS (gyro bias + accel/mag updates).
- Update system clock configuration and unify error handling calls to
AppError_Handler()across STM32 drivers.
Real Hardware Safety Review (mandatory checklist)
- Velocity limits: N/A (no actuator/joint velocity commands in this PR)
- Zero position / homing: N/A (no actuator position control in this PR)
- Joint position limits: N/A
- Torque limits: N/A
- Emergency stop: N/A in code (no motor control path here)
⚠️ HARDWARE E-STOP REMINDER: Before running anything that can actuate the real arm, confirm you have a physical E-stop wired in series with motor power; software stops are not sufficient. - CAN bus watchdog: N/A (CAN control removed from main app in this PR)
- Startup sequence ramp-up: N/A
- Sim-to-real verification: N/A
- Thermal monitoring: N/A
- Structural integrity checks: Manual (always required before any powered hardware test)
Reviewed changes
Copilot reviewed 20 out of 20 changed files in this pull request and generated 6 comments.
Show a summary per file
| File | Description |
|---|---|
| embedded/STM32/src/main.cpp | New IMU+EKF comparison app + UART streaming + heartbeat + centralized error handler. |
| embedded/STM32/src/EKF/quaternion.h | Adds quaternion API used by EKF + Euler conversion helper. |
| embedded/STM32/src/EKF/quaternion.c | Implements quaternion math, rotmats, and Euler conversion. |
| embedded/STM32/src/EKF/ekf_ahrs.h | Defines MEKF-style AHRS interface/state. |
| embedded/STM32/src/EKF/ekf_ahrs.c | Implements predict + accel/mag updates and covariance propagation. |
| embedded/STM32/src/Drivers/UART_STM32.h | UART helper interface for printing via HAL UART. |
| embedded/STM32/src/Drivers/UART_STM32.cpp | Implements USART2 init + UART_Print(). |
| embedded/STM32/src/Drivers/I2C_STM32.h | I2C1 init interface for SH2 transport. |
| embedded/STM32/src/Drivers/I2C_STM32.cpp | Implements I2C1 init with fixed HSI timing. |
| embedded/STM32/src/Drivers/sh2_hal_stm32.h | Declares STM32 SH2 HAL getter + diagnostics. |
| embedded/STM32/src/Drivers/sh2_hal_stm32.cpp | Implements SH2 HAL open/read/write/time + reset/INT handling. |
| embedded/STM32/src/Drivers/SysClock.h | SysClock header include cleanup + AppError_Handler() declaration. |
| embedded/STM32/src/Drivers/SysClock.cpp | Updates PLL + flash latency and routes errors to AppError_Handler(). |
| embedded/STM32/src/Drivers/TIM.h | Include cleanup + AppError_Handler() declaration. |
| embedded/STM32/src/Drivers/TIM.cpp | Routes TIM init errors to AppError_Handler(). |
| embedded/STM32/src/Drivers/FDCAN_STM32.h | Include cleanup + AppError_Handler() declaration. |
| embedded/STM32/src/Drivers/FDCAN_STM32.cpp | Routes FDCAN init/periph clock errors to AppError_Handler(). |
| embedded/STM32/src/Drivers/BNO085.h | Adds BNO085 wrapper API for SH2 sensor values/quaternion. |
| embedded/STM32/src/Drivers/BNO085.cpp | Implements SH2 open/service, report enables, and callbacks. |
| embedded/STM32/platformio.ini | Adds Adafruit BNO08x dep, adjusts HSE value, ignores FreeRTOS kernel. |
💡 Configure MCP servers for context-aware, tailored reviews. Learn more in the docs.
Comment on lines
+29
to
+33
| void AppError_Handler(void) { | ||
| __disable_irq(); | ||
| while (1) { | ||
| } | ||
| } |
Comment on lines
+6
to
+10
| // 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. |
Comment on lines
+6
to
+17
| #ifdef cplusplus | ||
| extern "C" { | ||
| #endif | ||
|
|
||
| extern UART_HandleTypeDef huart2; | ||
|
|
||
| void MX_LPUART1_Init(void); | ||
| void UART_Print(const char *message); | ||
|
|
||
| #ifdef cplusplus | ||
| } | ||
| #endif |
Comment on lines
+115
to
+125
| g_writeAttempts++; | ||
| uint32_t t0 = HAL_GetTick(); | ||
|
|
||
| HAL_StatusTypeDef status = | ||
| HAL_I2C_Master_Transmit(&hi2c1, BNO085_I2C_ADDR, buffer, len, BNO085_I2C_TIMEOUT_MS); | ||
|
|
||
| if (status != HAL_OK) { | ||
| g_writeFailures++; | ||
| g_lastWriteFailMs = HAL_GetTick() - t0; | ||
| return -1; | ||
| } |
Comment on lines
+147
to
+155
| if (HAL_I2C_Master_Receive(&hi2c1, | ||
| BNO085_I2C_ADDR, | ||
| buffer, | ||
| len, | ||
| BNO085_I2C_TIMEOUT_MS) != HAL_OK) | ||
| { | ||
| g_readHeaderFail++; | ||
| return -1; | ||
| } |
Comment on lines
+166
to
+169
| if (packetLength > len) | ||
| { | ||
| packetLength = len; | ||
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Driver and kalman filter for BNO085 IMU