diff --git a/include/i2c1.h b/include/i2c1.h
index c4c7859..511fce4 100644
--- a/include/i2c1.h
+++ b/include/i2c1.h
@@ -14,4 +14,7 @@ extern I2C_HandleTypeDef hi2c1;
 
 int i2c1_init(void);
 
+int i2c1_write(uint8_t addr, const uint8_t *data, uint16_t len);
+int i2c1_read(uint8_t addr, uint8_t *data, uint16_t len);
+
 #endif /* I2C1_H */
diff --git a/src/i2c1.c b/src/i2c1.c
index 8e78273..301444c 100644
--- a/src/i2c1.c
+++ b/src/i2c1.c
@@ -31,3 +31,11 @@ int i2c1_init(void) {
 
     return (HAL_I2C_Init(&hi2c1) == HAL_OK) ? 0 : -1;
 }
+
+int i2c1_write(uint8_t addr, const uint8_t *data, uint16_t len) {
+    return (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)(addr << 1), (uint8_t *)data, len, 1000) == HAL_OK) ? 0 : -1;
+}
+
+int i2c1_read(uint8_t addr, uint8_t *data, uint16_t len) {
+    return (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)(addr << 1), data, len, 1000) == HAL_OK) ? 0 : -1;
+}
diff --git a/src/main.c b/src/main.c
index 951a7cc..294c19f 100644
--- a/src/main.c
+++ b/src/main.c
@@ -109,6 +109,23 @@ extern PCD_HandleTypeDef hpcd;
 void OTG_FS_IRQHandler(void) { HAL_PCD_IRQHandler(&hpcd); }
 void SysTick_Handler(void) { HAL_IncTick(); }
 
+/* Determine if BNO055 is active (vs MPU6000) */
+static bool bno055_active = false;
+
+/* Helper: Check if IMU is calibrated (MPU6000 gyro bias or BNO055 ready) */
+static bool imu_calibrated(void) {
+    if (bno055_active) {
+        return bno055_is_ready();
+    }
+    return mpu6000_is_calibrated();
+}
+
+/* Helper: Check if CRSF receiver has recent signal */
+static bool crsf_is_active(uint32_t now_ms) {
+    extern volatile CRSFState crsf_state;
+    return crsf_state.last_rx_ms > 0 && (now_ms - crsf_state.last_rx_ms) < 500;
+}
+
 int main(void) {
     SCB_EnableICache();
     /* DCache stays ON — MPU Region 0 in usbd_conf.c marks USB buffers non-cacheable. */
@@ -158,7 +175,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();
diff --git a/src/servo.c b/src/servo.c
index 59e42df..0e5da3e 100644
--- a/src/servo.c
+++ b/src/servo.c
@@ -24,7 +24,7 @@
 #define SERVO_PRESCALER     53u            /* APB1 54 MHz / 54 = 1 MHz */
 #define SERVO_ARR           19999u         /* 1 MHz / 20000 = 50 Hz */
 
-typedef struct {
+static struct {
     uint16_t current_angle_deg[SERVO_COUNT];
     uint16_t target_angle_deg[SERVO_COUNT];
     uint16_t pulse_us[SERVO_COUNT];
@@ -35,9 +35,7 @@ typedef struct {
     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};
+} s_servo = {0};
 static TIM_HandleTypeDef s_tim_handle = {0};
 
 /* ================================================================
diff --git a/src/ultrasonic.c b/src/ultrasonic.c
index 7201687..8966ccc 100644
--- a/src/ultrasonic.c
+++ b/src/ultrasonic.c
@@ -48,6 +48,9 @@ static UltrasonicState_t s_ultrasonic = {
     .callback = NULL
 };
 
+/* TIM1 handle for input capture (shared with interrupt handler) */
+static TIM_HandleTypeDef s_tim_handle = {0};
+
 /* ================================================================
  * Hardware Initialization
  * ================================================================ */
@@ -80,14 +83,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};
-    htim1.Instance = ECHO_TIM;
-    htim1.Init.Prescaler = 216 - 1;        /* 216MHz / 216 = 1MHz (1µs per count) */
-    htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
-    htim1.Init.Period = 0xFFFF;            /* 16-bit counter */
-    htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-    htim1.Init.RepetitionCounter = 0;
-    HAL_TIM_IC_Init(&htim1);
+    s_tim_handle.Instance = ECHO_TIM;
+    s_tim_handle.Init.Prescaler = 216 - 1;        /* 216MHz / 216 = 1MHz (1µs per count) */
+    s_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+    s_tim_handle.Init.Period = 0xFFFF;            /* 16-bit counter */
+    s_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+    s_tim_handle.Init.RepetitionCounter = 0;
+    HAL_TIM_IC_Init(&s_tim_handle);
 
     /* Configure input capture: CH2 on PA1, both rising and falling edges
      * TIM1_CH2 captures on both edges to measure echo pulse width
@@ -97,15 +99,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_Start_IT(ECHO_TIM, ECHO_TIM_CHANNEL);
+    HAL_TIM_IC_ConfigChannel(&s_tim_handle, &ic_init, ECHO_TIM_CHANNEL);
+    HAL_TIM_IC_Start_IT(&s_tim_handle, 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_tim_handle);
 
     s_ultrasonic.state = ULTRASONIC_IDLE;
 }
@@ -188,10 +190,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) {
-        __HAL_TIM_CLEAR_FLAG(ECHO_TIM, TIM_FLAG_CC2);
+    if (__HAL_TIM_GET_FLAG(&s_tim_handle, TIM_FLAG_CC2) != RESET) {
+        __HAL_TIM_CLEAR_FLAG(&s_tim_handle, TIM_FLAG_CC2);
 
-        uint32_t capture_value = HAL_TIM_ReadCapturedValue(ECHO_TIM, ECHO_TIM_CHANNEL);
+        uint32_t capture_value = HAL_TIM_ReadCapturedValue(&s_tim_handle, ECHO_TIM_CHANNEL);
 
         if (s_ultrasonic.state == ULTRASONIC_TRIGGERED || s_ultrasonic.state == ULTRASONIC_MEASURING) {
             if (s_ultrasonic.echo_start_ticks == 0) {
@@ -205,7 +207,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_tim_handle, &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 +244,5 @@ void TIM1_CC_IRQHandler(void)
         }
     }
 
-    HAL_TIM_IRQHandler(ECHO_TIM);
-}
-
-/* ================================================================
- * 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;
-    }
+    HAL_TIM_IRQHandler(&s_tim_handle);
 }
diff --git a/src/watchdog.c b/src/watchdog.c
index c6c65d1..c7e53de 100644
--- a/src/watchdog.c
+++ b/src/watchdog.c
@@ -32,6 +32,7 @@ typedef struct {
     uint32_t timeout_ms;        /* Configured timeout in milliseconds */
     uint8_t prescaler;          /* IWDG prescaler value */
     uint16_t reload_value;      /* IWDG reload register value */
+    IWDG_HandleTypeDef handle;  /* IWDG handle for refresh */
 } WatchdogState;
 
 static WatchdogState s_watchdog = {
@@ -98,13 +99,12 @@ bool watchdog_init(uint32_t timeout_ms)
     s_watchdog.timeout_ms = timeout_ms;
 
     /* Configure and start IWDG */
-    IWDG_HandleTypeDef hiwdg = {0};
-    hiwdg.Instance = IWDG;
-    hiwdg.Init.Prescaler = prescaler;
-    hiwdg.Init.Reload = reload;
-    hiwdg.Init.Window = reload;  /* Window == Reload means full timeout */
+    s_watchdog.handle.Instance = IWDG;
+    s_watchdog.handle.Init.Prescaler = prescaler;
+    s_watchdog.handle.Init.Reload = reload;
+    s_watchdog.handle.Init.Window = reload;  /* Window == Reload means full timeout */
 
-    HAL_IWDG_Init(&hiwdg);
+    HAL_IWDG_Init(&s_watchdog.handle);
 
     s_watchdog.is_initialized = true;
     s_watchdog.is_running = true;
@@ -115,7 +115,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_watchdog.handle);  /* Reset IWDG counter */
     }
 }