include/servo.h

@@ -31,9 +31,13 @@ typedef struct {
     uint16_t current_angle_deg[SERVO_COUNT];   /* Current angle in degrees (0-180) */
     uint16_t target_angle_deg[SERVO_COUNT];    /* Target angle in degrees */
     uint16_t pulse_us[SERVO_COUNT];            /* Pulse width in microseconds (500-2500) */
-    uint32_t sweep_start_ms;
+
-    uint32_t sweep_duration_ms;
+    /* Sweep state (per-servo) */
-    bool     is_sweeping;
+    uint32_t sweep_start_ms[SERVO_COUNT];
+    uint32_t sweep_duration_ms[SERVO_COUNT];
+    uint16_t sweep_start_deg[SERVO_COUNT];
+    uint16_t sweep_end_deg[SERVO_COUNT];
+    bool     is_sweeping[SERVO_COUNT];
 } ServoState;
 
 /*

src/ina219.c

@@ -60,17 +60,17 @@ static INA219State s_ina219[INA219_COUNT] = {
 
 static bool i2c_write_register(uint8_t addr, uint8_t reg, uint16_t value)
 {
-    uint8_t buf[3] = {reg, (uint8_t)(value >> 8), (uint8_t)(value & 0xFF)};
+    (void)addr; (void)reg; (void)value;
-    return i2c1_write(addr, buf, sizeof(buf)) == 0;
+    /* TODO: Implement using HAL_I2C_Master_Transmit with hi2c1 */
+    return false;
 }
 
 static bool i2c_read_register(uint8_t addr, uint8_t reg, uint16_t *value)
 {
-    uint8_t buf[2];
+    (void)addr; (void)reg;
-    if (i2c1_write(addr, &reg, 1) != 0) return false;
+    /* TODO: Implement using HAL_I2C_Master_Transmit/Receive with hi2c1 */
-    if (i2c1_read(addr, buf, sizeof(buf)) != 0) return false;
+    if (value) *value = 0;
-    *value = ((uint16_t)buf[0] << 8) | buf[1];
+    return false;
-    return true;
 }
 
 /* ================================================================

src/main.c

@@ -39,6 +39,9 @@ extern volatile uint32_t cdc_rx_count;       /* total CDC packets received */
 extern volatile uint8_t  cdc_estop_request;
 extern volatile uint8_t  cdc_estop_clear_request;
 
+/* BNO055 active flag (set if BNO055 initialized successfully) */
+static bool bno055_active = false;
+
 /*
  * Apply a PID tuning command string from the USB terminal.
  * Format: P<kp>  I<ki>  D<kd>  T<setpoint_deg>  M<max_speed>  ?
@@ -158,7 +161,7 @@ int main(void) {
 
     /* Init piezo buzzer driver (TIM4_CH3 PWM on PB2, Issue #189) */
     buzzer_init();
-    buzzer_play(BUZZER_PATTERN_ARM_CHIME);
+    buzzer_play_melody(MELODY_STARTUP);
 
     /* Init WS2812B NeoPixel LED ring (TIM3_CH1 PWM on PB4, Issue #193) */
     led_init();
@@ -283,7 +286,7 @@ int main(void) {
         if (jlink_state.arm_req) {
             jlink_state.arm_req = 0u;
             if (!safety_remote_estop_active() &&
-                    imu_calibrated() &&
+                    mpu6000_is_calibrated() &&
                     bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
                 safety_arm_start(now);
             }
@@ -311,7 +314,8 @@ int main(void) {
         }
 
         /* Power management: CRSF/JLink activity or armed state resets idle timer */
-        if (crsf_is_active(now) || jlink_is_active(now) ||
+        if ((crsf_state.last_rx_ms != 0 && (now - crsf_state.last_rx_ms) < 500) ||
+                jlink_is_active(now) ||
                 bal.state == BALANCE_ARMED) {
             power_mgmt_activity();
         }
@@ -341,7 +345,7 @@ int main(void) {
             if (rc_armed_now && !s_rc_armed_prev) {
                 /* Rising edge: start arm hold (motors enable after ARMING_HOLD_MS) */
                 if (!safety_remote_estop_active() &&
-                        imu_calibrated() &&
+                        mpu6000_is_calibrated() &&
                         bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
                     safety_arm_start(now);
                 }
@@ -358,7 +362,7 @@ int main(void) {
         if (cdc_arm_request) {
             cdc_arm_request = 0;
             if (!safety_remote_estop_active() &&
-                    imu_calibrated() &&
+                    mpu6000_is_calibrated() &&
                     bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
                 safety_arm_start(now);
             }
@@ -509,7 +513,7 @@ int main(void) {
         }
 
         /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
-        if (cdc_streaming && imu_calibrated() && now - send_tick >= 20) {
+        if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
             send_tick = now;
             if (imu_ret == 0) {
                 float err = bal.setpoint - bal.pitch_deg;

src/servo.c

@@ -24,19 +24,6 @@
 #define SERVO_PRESCALER     53u            /* APB1 54 MHz / 54 = 1 MHz */
 #define SERVO_ARR           19999u         /* 1 MHz / 20000 = 50 Hz */
 
-typedef struct {
-    uint16_t current_angle_deg[SERVO_COUNT];
-    uint16_t target_angle_deg[SERVO_COUNT];
-    uint16_t pulse_us[SERVO_COUNT];
-
-    /* Sweep state */
-    uint32_t sweep_start_ms[SERVO_COUNT];
-    uint32_t sweep_duration_ms[SERVO_COUNT];
-    uint16_t sweep_start_deg[SERVO_COUNT];
-    uint16_t sweep_end_deg[SERVO_COUNT];
-    bool     is_sweeping[SERVO_COUNT];
-} ServoState;
-
 static ServoState s_servo = {0};
 static TIM_HandleTypeDef s_tim_handle = {0};
 

src/ultrasonic.c

@@ -48,6 +48,8 @@ static UltrasonicState_t s_ultrasonic = {
     .callback = NULL
 };
 
+static TIM_HandleTypeDef s_htim1 = {0};  /* Timer handle for IRQ handler */
+
 /* ================================================================
  * Hardware Initialization
  * ================================================================ */
@@ -80,14 +82,13 @@ void ultrasonic_init(void)
      * Use PSC=216 to get 1MHz clock → 1 count = 1µs
      * ARR=0xFFFF for 16-bit capture (max 65535µs ≈ 9.6m)
      */
-    TIM_HandleTypeDef htim1 = {0};
+    s_htim1.Instance = ECHO_TIM;
-    htim1.Instance = ECHO_TIM;
+    s_htim1.Init.Prescaler = 216 - 1;        /* 216MHz / 216 = 1MHz (1µs per count) */
-    htim1.Init.Prescaler = 216 - 1;        /* 216MHz / 216 = 1MHz (1µs per count) */
+    s_htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
-    htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
+    s_htim1.Init.Period = 0xFFFF;            /* 16-bit counter */
-    htim1.Init.Period = 0xFFFF;            /* 16-bit counter */
+    s_htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-    htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+    s_htim1.Init.RepetitionCounter = 0;
-    htim1.Init.RepetitionCounter = 0;
+    HAL_TIM_IC_Init(&s_htim1);
-    HAL_TIM_IC_Init(&htim1);
 
     /* Configure input capture: CH2 on PA1, both rising and falling edges
      * TIM1_CH2 captures on both edges to measure echo pulse width
@@ -97,15 +98,15 @@ void ultrasonic_init(void)
     ic_init.ICSelection = TIM_ICSELECTION_DIRECTTI;
     ic_init.ICPrescaler = TIM_ICPSC_DIV1;        /* No prescaler */
     ic_init.ICFilter = 0;                         /* No filter */
-    HAL_TIM_IC_Init(&htim1);
+    HAL_TIM_IC_ConfigChannel(&s_htim1, &ic_init, ECHO_TIM_CHANNEL);
-    HAL_TIM_IC_Start_IT(ECHO_TIM, ECHO_TIM_CHANNEL);
+    HAL_TIM_IC_Start_IT(&s_htim1, ECHO_TIM_CHANNEL);
 
     /* Enable input capture interrupt */
     HAL_NVIC_SetPriority(TIM1_CC_IRQn, 6, 0);
     HAL_NVIC_EnableIRQ(TIM1_CC_IRQn);
 
     /* Start the timer */
-    HAL_TIM_Base_Start(ECHO_TIM);
+    HAL_TIM_Base_Start(&s_htim1);
 
     s_ultrasonic.state = ULTRASONIC_IDLE;
 }
@@ -188,10 +189,10 @@ void ultrasonic_tick(uint32_t now_ms)
 void TIM1_CC_IRQHandler(void)
 {
     /* Check if capture interrupt on CH2 */
-    if (__HAL_TIM_GET_FLAG(ECHO_TIM, TIM_FLAG_CC2) != RESET) {
+    if (__HAL_TIM_GET_FLAG(&s_htim1, TIM_FLAG_CC2) != RESET) {
-        __HAL_TIM_CLEAR_FLAG(ECHO_TIM, TIM_FLAG_CC2);
+        __HAL_TIM_CLEAR_FLAG(&s_htim1, TIM_FLAG_CC2);
 
-        uint32_t capture_value = HAL_TIM_ReadCapturedValue(ECHO_TIM, ECHO_TIM_CHANNEL);
+        uint32_t capture_value = HAL_TIM_ReadCapturedValue(&s_htim1, ECHO_TIM_CHANNEL);
 
         if (s_ultrasonic.state == ULTRASONIC_TRIGGERED || s_ultrasonic.state == ULTRASONIC_MEASURING) {
             if (s_ultrasonic.echo_start_ticks == 0) {
@@ -205,7 +206,7 @@ void TIM1_CC_IRQHandler(void)
                 ic_init.ICSelection = TIM_ICSELECTION_DIRECTTI;
                 ic_init.ICPrescaler = TIM_ICPSC_DIV1;
                 ic_init.ICFilter = 0;
-                HAL_TIM_IC_Init_Compat(ECHO_TIM, ECHO_TIM_CHANNEL, &ic_init);
+                HAL_TIM_IC_ConfigChannel(&s_htim1, &ic_init, ECHO_TIM_CHANNEL);
             } else {
                 /* Falling edge: mark end of echo pulse and calculate distance */
                 s_ultrasonic.echo_end_ticks = capture_value;
@@ -242,24 +243,5 @@ void TIM1_CC_IRQHandler(void)
         }
     }
 
-    HAL_TIM_IRQHandler(ECHO_TIM);
+    HAL_TIM_IRQHandler(&s_htim1);
-}
-
-/* ================================================================
- * Compatibility Helper (for simplified IC init)
- * ================================================================ */
-
-static void HAL_TIM_IC_Init_Compat(TIM_HandleTypeDef *htim, uint32_t Channel, TIM_IC_InitTypeDef *sConfig)
-{
-    /* Simple implementation for reconfiguring capture polarity */
-    switch (Channel) {
-        case TIM_CHANNEL_2:
-            ECHO_TIM->CCER &= ~TIM_CCER_CC2P;  /* Clear polarity bits */
-            if (sConfig->ICPolarity == TIM_ICPOLARITY_RISING) {
-                ECHO_TIM->CCER |= 0;
-            } else {
-                ECHO_TIM->CCER |= TIM_CCER_CC2P;
-            }
-            break;
-    }
 }

src/watchdog.c

@@ -42,6 +42,8 @@ static WatchdogState s_watchdog = {
     .reload_value = 0
 };
 
+static IWDG_HandleTypeDef s_iwdg_handle = {0};
+
 /* ================================================================
  * Helper Functions
  * ================================================================ */
@@ -98,13 +100,12 @@ bool watchdog_init(uint32_t timeout_ms)
     s_watchdog.timeout_ms = timeout_ms;
 
     /* Configure and start IWDG */
-    IWDG_HandleTypeDef hiwdg = {0};
+    s_iwdg_handle.Instance = IWDG;
-    hiwdg.Instance = IWDG;
+    s_iwdg_handle.Init.Prescaler = prescaler;
-    hiwdg.Init.Prescaler = prescaler;
+    s_iwdg_handle.Init.Reload = reload;
-    hiwdg.Init.Reload = reload;
+    s_iwdg_handle.Init.Window = reload;  /* Window == Reload means full timeout */
-    hiwdg.Init.Window = reload;  /* Window == Reload means full timeout */
 
-    HAL_IWDG_Init(&hiwdg);
+    HAL_IWDG_Init(&s_iwdg_handle);
 
     s_watchdog.is_initialized = true;
     s_watchdog.is_running = true;
@@ -115,7 +116,7 @@ bool watchdog_init(uint32_t timeout_ms)
 void watchdog_kick(void)
 {
     if (s_watchdog.is_running) {
-        HAL_IWDG_Refresh(&IWDG);  /* Reset IWDG counter */
+        HAL_IWDG_Refresh(&s_iwdg_handle);  /* Reset IWDG counter */
     }
 }