Feat / Add driver for mmwave sensor + reimplement DMA + tests

.vscode/launch.json

@@ -0,0 +1,48 @@
+{
+    "configurations": [
+        {
+            "name": "FC - Debug",
+            "cwd": "${workspaceFolder}/embedded-software/firmware/ulysses-flight-controller",
+            "executable": "${workspaceFolder}/embedded-software/firmware/ulysses-flight-controller/build/debug/ulysses.elf",
+            "request": "launch",
+            "type": "cortex-debug",
+            "servertype": "openocd",
+            "serverpath": "${env:STM32_OPENOCD_PATH}",
+            "configFiles": [
+                "interface/stlink-dap.cfg",
+                "target/stm32l4x.cfg"
+            ],
+            "svdFile": "${workspaceFolder}/embedded-software/firmware/ulysses-flight-controller/platform/svd/STM32L476.svd",
+            "searchDir": ["${env:STM32_OPENOCD_SCRIPTS_PATH}"],
+            "runToEntryPoint": "main",
+            "showDevDebugOutput": "raw"
+        },
+        {
+            "name": "C/C++: clang build and debug active file",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "${fileDirname}/${fileBasenameNoExtension}",
+            "args": [],
+            "stopAtEntry": false,
+            "cwd": "${fileDirname}",
+            "environment": [],
+            "externalConsole": false,
+            "MIMode": "lldb",
+            "preLaunchTask": "C/C++: clang build active file"
+        },
+        {
+            "name": "C/C++: arm-none-eabi-gcc build and debug active file",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "${fileDirname}/${fileBasenameNoExtension}",
+            "args": [],
+            "stopAtEntry": false,
+            "cwd": "${fileDirname}",
+            "environment": [],
+            "externalConsole": false,
+            "MIMode": "lldb",
+            "preLaunchTask": "C/C++: arm-none-eabi-gcc build active file"
+        }
+    ],
+    "version": "2.0.0"
+}

embedded-software/firmware/.vscode/launch.json

@@ -12,9 +12,9 @@
       "serverpath": "${env:STM32_OPENOCD_PATH}",
       "configFiles": [
         "interface/stlink-dap.cfg",
-        "target/stm32h5x.cfg"
+        "target/stm32l4x.cfg"
       ],
-      "svdFile": "${workspaceFolder}/ulysses-flight-controller/platform/svd/STM32H563.svd",
+      "svdFile": "${workspaceFolder}/ulysses-flight-controller/platform/svd/STM32L476.svd",
       "searchDir": ["${env:STM32_OPENOCD_SCRIPTS_PATH}"],
       "runToEntryPoint": "main",
       "showDevDebugOutput": "raw",
@@ -30,9 +30,9 @@
       "serverpath": "${env:STM32_OPENOCD_PATH}",
       "configFiles": [
         "interface/stlink-dap.cfg",
-        "target/stm32h5x.cfg"
+        "target/stm32l4x.cfg"
       ],
-      "svdFile": "${workspaceFolder}/ulysses-flight-controller/platform/svd/STM32H563.svd",
+      "svdFile": "${workspaceFolder}/ulysses-flight-controller/platform/svd/STM32L476.svd",
       "searchDir": ["${env:STM32_OPENOCD_SCRIPTS_PATH}"],
       "runToEntryPoint": "main",
       "showDevDebugOutput": "raw",

embedded-software/firmware/libs/controls/include/controls/pwm.h

@@ -0,0 +1,13 @@
+#ifndef LIB_CONTROLS_PWM_H
+#define LIB_CONTROLS_PWM_H
+
+#include <stdint.h>
+
+typedef struct {
+  uint16_t upper_motor_us; /**< PWM for the upper motor in microseconds */
+  uint16_t lower_motor_us;
+} pwm_setpoint_t;
+
+pwm_setpoint_t pwm_setpoint_from_forces(double thrust, double torque);
+
+#endif // LIB_CONTROLS_PWM_H

embedded-software/firmware/libs/controls/tests/test_pwm.c

@@ -0,0 +1,40 @@
+#include "unity.h"
+#include "controls/pwm.h"
+#include <math.h>
+
+#define TOL 20 /**< Tolerance for floating-point comparisons */
+
+void test_pwm_zeroes(void)
+{
+    pwm_setpoint_t setpoint_pwm = pwm_from_setpoint(0,0);
+    TEST_ASSERT_EQUAL_UINT16(1000, setpoint_pwm.upper_motor_us, TOL);
+    TEST_ASSERT_EQUAL_UINT16(1000, setpoint_pwm.lower_motor_us, TOL);
+}
+
+void test_pwm_midpoint(void)
+{
+    pwm_setpoint_t setpoint_pwm = pwm_from_setpoint(500,0);
+    TEST_ASSERT_EQUAL_UINT16(1531.9, setpoint_pwm.upper_motor_us, TOL);
+    TEST_ASSERT_EQUAL_UINT16(1559.4, setpoint_pwm.lower_motor_us, TOL);
+}
+
+void test_pwm_max_thrust(void)
+{
+    pwm_setpoint_t setpoint_pwm = pwm_from_setpoint(1594.56,0);
+    TEST_ASSERT_EQUAL_UINT16(1949.9, setpoint_pwm.upper_motor_us, TOL);
+    TEST_ASSERT_EQUAL_UINT16(1999.0, setpoint_pwm.lower_motor_us, TOL);
+}
+
+void test_pwm_max_torque_upper(void)
+{
+    pwm_setpoint_t setpoint_pwm = pwm_from_setpoint(782.16, 0.1952);
+    TEST_ASSERT_EQUAL_UINT16(1999.0, setpoint_pwm.upper_motor_us, TOL);
+    TEST_ASSERT_EQUAL_UINT16(1000.0, setpoint_pwm.lower_motor_us, TOL);
+}
+
+void test_pwm_max_torque_lower(void)
+{
+    pwm_setpoint_t setpoint_pwm = pwm_from_setpoint(887.44, -0.1765);
+    TEST_ASSERT_EQUAL_UINT16(1000.0, setpoint_pwm.upper_motor_us, TOL);
+    TEST_ASSERT_EQUAL_UINT16(1999.0, setpoint_pwm.lower_motor_us, TOL);
+}

embedded-software/firmware/libs/sensors/CMakeLists.txt

@@ -6,6 +6,7 @@ add_library(sensors STATIC
     src/bmi088_gyro.c
     src/ms5611_baro.c
     src/ms5607_baro.c
+    src/sen0306_mmwave.c
 )
 
 target_include_directories(sensors

embedded-software/firmware/libs/sensors/include/sensors/sen0306_mmwave.h

@@ -0,0 +1,144 @@
+/**
+ * @file sen0306_mmwave.h
+ * @brief Driver for the DFRobot SEN0306 mmWave radar (distance-only mode).
+ *
+ * Reception uses UART DMA (HAL_UARTEx_ReceiveToIdle_DMA) in DMA_NORMAL mode.
+ * The DMA channel (GPDMA2_Channel5) is configured in stm32h5xx_hal_msp.c and
+ * linked to huart4. Call sen0306_irq_handler() from HAL_UARTEx_RxEventCallback
+ * in main.c, then immediately re-arm with HAL_UARTEx_ReceiveToIdle_DMA.
+ *
+ * Frame format (8 bytes, distance-only mode):
+ *   [0xFF][0xFF][0xFF][DIST_H][DIST_L][0x00][0x00][0x00]
+ *   Distance is big-endian uint16 in centimetres.
+ *
+ * UBC Rocket, 2026
+ */
+
+#ifndef SEN0306_MMWAVE_H
+#define SEN0306_MMWAVE_H
+
+#include "stm32l4xx_hal.h"
+#include "sensors/spsc_queue.h"
+#include <stdbool.h>
+#include <stdint.h>
+
+/* -------------------------------------------------------------------------- */
+/* Configuration                                                               */
+/* -------------------------------------------------------------------------- */
+
+#define SEN0306_FRAME_SIZE   8   /**< Bytes per frame in distance-only mode */
+#define SEN0306_DMA_BUF_SIZE 64  /**< DMA reception buffer size (bytes).
+                                      Sized to hold 8 complete frames;
+                                      IDLE fires after each burst so the
+                                      buffer rarely fills completely. */
+#define SEN0306_QUEUE_LEN    16  /**< Output sample queue depth */
+
+/* -------------------------------------------------------------------------- */
+/* Data types                                                                  */
+/* -------------------------------------------------------------------------- */
+
+/** distance_cm value when the sensor sees no target. */
+#define SEN0306_NO_DETECTION 0xFFFFU
+
+/**
+ * @brief A single parsed radar sample.
+ */
+typedef struct {
+    uint16_t distance_cm;   ///< Target distance in cm, or SEN0306_NO_DETECTION
+    uint32_t timestamp_ms;  ///< HAL_GetTick() value at time of reception (ms)
+} sen0306_sample_t;
+
+/**
+ * @brief SPSC output queue for parsed samples.
+ *
+ * Producer: sen0306_irq_handler (ISR context via HAL_UARTEx_RxEventCallback).
+ * Consumer: application task via sen0306_get_latest().
+ * SENSORS_MEMORY_BARRIER() separates the sample write from the head update.
+ */
+typedef struct {
+    sen0306_sample_t samples[SEN0306_QUEUE_LEN];
+    volatile uint8_t head;  ///< Written by producer only
+    volatile uint8_t tail;  ///< Written by consumer only
+} sen0306_queue_t;
+
+/**
+ * @brief Driver context. One global instance: sen0306_ctx.
+ */
+typedef struct {
+    UART_HandleTypeDef *huart;  ///< UART handle passed to sen0306_init()
+    bool initialized;           ///< Set true by sen0306_init()
+
+    /* ── DMA reception ──────────────────────────────────────────────────── */
+    /** DMA destination buffer. HAL_UARTEx_ReceiveToIdle_DMA fills this from
+     *  index 0 on each arm. Bytes [0..size-1] are valid in the callback. */
+    uint8_t dma_buf[SEN0306_DMA_BUF_SIZE];
+
+    /* ── Frame parser ───────────────────────────────────────────────────── */
+    uint8_t  parser_buf[SEN0306_FRAME_SIZE]; ///< Sliding window accumulator
+    uint16_t parser_idx;                     ///< Valid bytes in parser_buf
+
+    /* ── Statistics (read-only from application) ────────────────────────── */
+    uint32_t samples_received;  ///< Total valid frames parsed
+    uint32_t parse_errors;      ///< Frames with invalid tail bytes
+    uint32_t queue_overflows;   ///< Samples dropped due to full queue
+
+    /* ── Output ─────────────────────────────────────────────────────────── */
+    sen0306_queue_t queue;
+} sen0306_context_t;
+
+extern sen0306_context_t sen0306_ctx;
+
+/* -------------------------------------------------------------------------- */
+/* Public API                                                                  */
+/* -------------------------------------------------------------------------- */
+
+/**
+ * @brief Initialise the driver context.
+ *
+ * Zeroes the context and stores the UART handle. Does not start hardware
+ * reception — call HAL_UARTEx_ReceiveToIdle_DMA() in main.c after sending
+ * the hex-mode command to the sensor.
+ *
+ * @param huart  UART handle connected to the SEN0306 (huart4 on this board).
+ */
+void sen0306_init(UART_HandleTypeDef *huart);
+
+/**
+ * @brief Process a DMA burst and feed bytes to the frame parser.
+ *
+ * Call this from HAL_UARTEx_RxEventCallback() in main.c, then immediately
+ * re-arm with HAL_UARTEx_ReceiveToIdle_DMA(). The DMA_NORMAL transfer always
+ * fills dma_buf from index 0, so @p size bytes starting at dma_buf[0] are
+ * the new data for this burst.
+ *
+ * @param huart  UART handle (guards against spurious calls for other UARTs).
+ * @param size   Number of bytes received into dma_buf since the last arm.
+ */
+void sen0306_irq_handler(UART_HandleTypeDef *huart, uint16_t size);
+
+/**
+ * @brief Peek at the most recently parsed sample without consuming it.
+ *
+ * Returns a pointer directly into the queue — valid until the next call to
+ * sen0306_irq_handler() that overwrites that slot (queue depth: SEN0306_QUEUE_LEN).
+ *
+ * @return Pointer to the latest sample, or NULL if the queue is empty.
+ */
+const sen0306_sample_t *sen0306_get_latest(void);
+
+/* -------------------------------------------------------------------------- */
+/* Queue helpers (inline, zero overhead)                                       */
+/* -------------------------------------------------------------------------- */
+
+/** @brief Returns true if no samples are available. */
+static inline bool sen0306_queue_empty(void) {
+    return sen0306_ctx.queue.head == sen0306_ctx.queue.tail;
+}
+
+/** @brief Returns the number of unread samples in the queue. */
+static inline uint8_t sen0306_queue_count(void) {
+    return (sen0306_ctx.queue.head - sen0306_ctx.queue.tail
+            + SEN0306_QUEUE_LEN) % SEN0306_QUEUE_LEN;
+}
+
+#endif /* SEN0306_MMWAVE_H */

embedded-software/firmware/libs/sensors/include/sensors/sensor_config.h

@@ -20,6 +20,7 @@
 #include "sensors/bmi088_gyro.h"
 #include "sensors/ms5611_baro.h"
 #include "sensors/ms5607_baro.h"
+#include "sensors/sen0306_mmwave.h"
 
 /* -------------------------------------------------------------------------- */
 /* BMI088 Accelerometer Configuration                                         */
@@ -194,6 +195,26 @@ typedef struct {
     .odr_hz = 100                   \
 }
 
+/* -------------------------------------------------------------------------- */
+/* SEN0306 mmWave Radar Configuration                                           */
+/* -------------------------------------------------------------------------- */
+
+/**
+ * @brief Configuration for SEN0306 mmWave radar.
+ */
+typedef struct {
+    uint32_t uart_baudrate; /**< UART baud rate for communication */
+} sen0306_config_t;
+
+/**
+ * @brief Default mmWave radar configuration.
+ *
+ * UART Baud Rate: 115200 (sufficient for 50 Hz updates with small frames)
+ */
+#define SEN0306_CONFIG_DEFAULT {     \
+    .uart_baudrate = 115200          \
+}
+
 /* -------------------------------------------------------------------------- */
 /* Complete System Configuration                                              */
 /* -------------------------------------------------------------------------- */
@@ -206,16 +227,18 @@ typedef struct {
     bmi088_gyro_config_t  gyro;
     ms5611_config_t       baro;
     ms5607_config_t       baro2;
+    sen0306_config_t      mmwave;
 } sensor_system_config_t;
 
 /**
  * @brief Default system sensor configuration.
  */
 #define SENSOR_SYSTEM_CONFIG_DEFAULT {          \
-    .accel = BMI088_ACCEL_CONFIG_DEFAULT,       \
-    .gyro  = BMI088_GYRO_CONFIG_DEFAULT,        \
-    .baro  = MS5611_CONFIG_DEFAULT,             \
-    .baro2 = MS5607_CONFIG_DEFAULT              \
+    .accel  = BMI088_ACCEL_CONFIG_DEFAULT,      \
+    .gyro   = BMI088_GYRO_CONFIG_DEFAULT,       \
+    .baro   = MS5611_CONFIG_DEFAULT,            \
+    .baro2  = MS5607_CONFIG_DEFAULT,            \
+    .mmwave = SEN0306_CONFIG_DEFAULT            \
 }
 
 #endif /* SENSOR_CONFIG_H */