feat: RC/autonomous mode manager with smooth handoff

Adds mode_manager.c/h: three operating modes selected by RC CH6 (3-pos
switch), smoothly interpolated over ~500ms to prevent jerky transitions.

Modes:
  RC_MANUAL   (blend=0.0) — RC CH4 steer + CH3 speed bias; balance PID active
  RC_ASSISTED (blend=0.5) — 50/50 blend of RC and Jetson autonomous commands
  AUTONOMOUS  (blend=1.0) — Jetson steer only; RC CH5 still kills motors

Key design:
- Single `blend` float (0=RC, 1=auto) drives all lerp; MANUAL→AUTO takes
  500ms, adjacent steps take ~250ms
- CH6 thresholds: <=600=MANUAL, >=1200=AUTONOMOUS, else ASSISTED
- CH4/CH3 read with ±30-count deadband around CRSF center (992)
- RC speed bias (CH3, ±300 counts) added to bal.motor_cmd AFTER PID
- RC CH5 kill: if rc_alive && !crsf_state.armed → disarm, regardless of mode
- Jetson steer fed via mode_manager_set_auto_cmd() → blended in get_steer()
- Telemetry: new "md" field (0/1/2) in USB JSON stream
- mode_manager_set_auto_cmd() API ready for Jetson serial bridge integration

config.h: CRSF channel indices, deadband, speed-bias max, blend timing.
Safe on USB-only build: CRSF stub keeps last_rx_ms=0 → rc_alive=false
→ RC inputs = 0, mode stays RC_MANUAL, CH5 kill never fires.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

include/config.h

@@ -147,4 +147,20 @@
 // --- IMU Calibration ---
 #define GYRO_CAL_SAMPLES        1000    /* gyro bias samples (~1s at 1ms/sample) */
 
+// --- RC / Mode Manager ---
+/* CRSF channel indices (0-based; CRSF range 172-1811, center 992) */
+#define CRSF_CH_SPEED           2       /* CH3 — left stick vertical (fwd/back) */
+#define CRSF_CH_STEER           3       /* CH4 — left stick horizontal (yaw) */
+#define CRSF_CH_ARM             4       /* CH5 — arm switch (2-pos) */
+#define CRSF_CH_MODE            5       /* CH6 — mode switch (3-pos) */
+/* Deadband around CRSF center (992) in raw counts (~2% of range) */
+#define CRSF_DEADBAND           30
+/* CH6 mode thresholds (raw CRSF counts) */
+#define CRSF_MODE_LOW_THRESH    600     /* <= → RC_MANUAL */
+#define CRSF_MODE_HIGH_THRESH   1200    /* >= → AUTONOMOUS */
+/* Max speed bias RC can add to balance PID output (counts, same scale as ESC) */
+#define MOTOR_RC_SPEED_MAX      300
+/* Full blend transition time: MANUAL→AUTO takes this many ms */
+#define MODE_BLEND_MS           500
+
 #endif // CONFIG_H

include/mode_manager.h

@@ -0,0 +1,74 @@
+#ifndef MODE_MANAGER_H
+#define MODE_MANAGER_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * SaltyLab Mode Manager
+ *
+ * Resolves three operating modes selected by RC CH6 (3-pos switch):
+ *
+ *   RC_MANUAL   — RC steer (CH4) and speed bias (CH3) applied directly.
+ *                 Balance PID remains active for stability.
+ *   RC_ASSISTED — RC inputs blended 50/50 with Jetson autonomous commands.
+ *   AUTONOMOUS  — Jetson commands only; RC CH5 arm switch still kills motors.
+ *
+ * Transitions between modes are smoothed over MODE_BLEND_MS (~500ms) to
+ * prevent jerky handoffs. A single `blend` scalar (0=pure RC, 1=pure auto)
+ * drives all interpolation; adjacent-mode steps take ~250ms each.
+ *
+ * RC safety rule: if RC is alive and CH5 is disarmed, the main loop MUST
+ * disarm regardless of mode. mode_manager only blends commands — kill
+ * authority lives in the main loop.
+ *
+ * Autonomous commands are set by the Jetson serial bridge via
+ * mode_manager_set_auto_cmd(). They default to zero (no motion).
+ */
+
+typedef enum {
+    MODE_RC_MANUAL   = 0,
+    MODE_RC_ASSISTED = 1,
+    MODE_AUTONOMOUS  = 2,
+} robot_mode_t;
+
+typedef struct {
+    robot_mode_t target;         /* Mode requested by CH6 (or fallback) */
+    float        blend;          /* 0.0=pure RC .. 1.0=pure auto, smoothly ramped */
+    bool         rc_alive;       /* Cached RC liveness (set in update) */
+    int16_t      auto_steer;     /* Jetson steer cmd (-1000..+1000) */
+    int16_t      auto_speed_bias;/* Jetson speed bias (-MOTOR_RC_SPEED_MAX..+) */
+} mode_manager_t;
+
+/* Initialise — call once before the main loop */
+void mode_manager_init(mode_manager_t *m);
+
+/*
+ * Call every main-loop tick (1ms) to:
+ *   - read CH6, update target mode
+ *   - cache RC liveness
+ *   - advance blend ramp toward target blend value
+ */
+void mode_manager_update(mode_manager_t *m, uint32_t now);
+
+/* Set autonomous commands from the Jetson serial bridge */
+void mode_manager_set_auto_cmd(mode_manager_t *m,
+                                int16_t steer,
+                                int16_t speed_bias);
+
+/*
+ * Blended steer command to pass to motor_driver_update().
+ * Returns 0 when RC is not alive and no autonomous steer set.
+ */
+int16_t mode_manager_get_steer(const mode_manager_t *m);
+
+/*
+ * Blended speed bias to add to bal.motor_cmd before motor_driver_update().
+ * Returns 0 when RC is not alive and no autonomous speed set.
+ */
+int16_t mode_manager_get_speed_bias(const mode_manager_t *m);
+
+/* Quantised current mode (based on blend position, not target) */
+robot_mode_t mode_manager_active(const mode_manager_t *m);
+
+#endif

src/main.c

@@ -7,6 +7,7 @@
 #include "balance.h"
 #include "hoverboard.h"
 #include "motor_driver.h"
+#include "mode_manager.h"
 #include "config.h"
 #include "status.h"
 #include "safety.h"
@@ -14,6 +15,7 @@
 #include "i2c1.h"
 #include "bmp280.h"
 #include "mag.h"
+#include "jetson_cmd.h"
 #include <math.h>
 #include <string.h>
 #include <stdio.h>
@@ -129,6 +131,10 @@ int main(void) {
     motor_driver_t motors;
     motor_driver_init(&motors);
 
+    /* Init mode manager (RC/autonomous blend; CH6 mode switch) */
+    mode_manager_t mode;
+    mode_manager_init(&mode);
+
     /* Probe I2C1 for optional sensors — skip gracefully if not found */
     int baro_ok = 0;
     mag_type_t mag_type = MAG_NONE;
@@ -162,10 +168,19 @@ int main(void) {
         /* Feed hardware watchdog — must happen every WATCHDOG_TIMEOUT_MS */
         safety_refresh();
 
-        /* RC timeout: disarm if signal lost while armed */
+        /* Mode manager: update RC liveness, CH6 mode selection, blend ramp */
+        mode_manager_update(&mode, now);
+
+        /* RC CH5 kill switch: disarm immediately if RC is alive and CH5 off.
+         * Applies regardless of active mode (CH5 always has kill authority). */
+        if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) {
+            safety_arm_cancel();
+            balance_disarm(&bal);
+        }
+        /* RC signal lost while armed — disarm for safety */
         if (bal.state == BALANCE_ARMED && !safety_rc_alive(now)) {
-            /* USB-only mode: no RC receiver expected yet — skip RC timeout.
+            /* Uncomment when CRSF is wired (last_rx_ms stays 0 on stub): */
-             * Uncomment when CRSF is wired: balance_disarm(&bal); */
+            /* balance_disarm(&bal); */
         }
 
         /* Tilt fault buzzer alert (one-shot on fault edge) */
@@ -199,10 +214,19 @@ int main(void) {
             CDC_Transmit((uint8_t *)reply, strlen(reply));
         }
 
-        /* Balance PID at 1kHz */
+        /* Handle Jetson C<speed>,<steer> command (parsed here, not in ISR) */
+        if (jetson_cmd_ready) {
+            jetson_cmd_ready = 0;
+            jetson_cmd_process();
+        }
+
+        /* Balance PID at 1kHz — apply Jetson speed offset when active */
         if (imu_ret == 0 && now - balance_tick >= 1) {
             balance_tick = now;
+            float base_sp = bal.setpoint;
+            if (jetson_cmd_is_active(now)) bal.setpoint += jetson_cmd_sp_offset();
             balance_update(&bal, &imu, dt);
+            bal.setpoint = base_sp;
         }
 
         /* Latch estop on tilt fault or disarm */
@@ -212,11 +236,22 @@ int main(void) {
             motor_driver_estop_clear(&motors);
         }
 
+        /* Feed autonomous steer from Jetson into mode manager.
+         * mode_manager_get_steer() blends it with RC CH4 per active mode. */
+        if (jetson_cmd_is_active(now))
+            mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0);
+
         /* Send to hoverboard ESC at 50Hz (every 20ms) */
         if (now - esc_tick >= 20) {
             esc_tick = now;
             if (bal.state == BALANCE_ARMED) {
-                motor_driver_update(&motors, bal.motor_cmd, 0, now);
+                /* Blended steer (RC ↔ auto per mode) + RC speed bias */
+                int16_t steer      = mode_manager_get_steer(&mode);
+                int16_t spd_bias   = mode_manager_get_speed_bias(&mode);
+                int32_t speed      = (int32_t)bal.motor_cmd + spd_bias;
+                if (speed >  1000) speed =  1000;
+                if (speed < -1000) speed = -1000;
+                motor_driver_update(&motors, (int16_t)speed, steer, now);
             } else {
                 /* Always send zero while disarmed to prevent ESC timeout */
                 motor_driver_update(&motors, 0, 0, now);
@@ -242,6 +277,9 @@ int main(void) {
                     (int)bal.state,
                     (int)(imu.yaw * 10));        /* yaw degrees x10 (gyro-integrated) */
                 p += n; rem -= n;
+                n = snprintf(p, rem, ",\"md\":%d",
+                    (int)mode_manager_active(&mode));  /* 0=MANUAL,1=ASSISTED,2=AUTO */
+                p += n; rem -= n;
                 if (mag_type != MAG_NONE) {
                     int16_t hd = mag_read_heading();
                     if (hd >= 0)

src/mode_manager.c

@@ -0,0 +1,129 @@
+#include "mode_manager.h"
+#include "crsf.h"
+#include "config.h"
+
+/* -----------------------------------------------------------------------
+ * Internal helpers
+ * --------------------------------------------------------------------- */
+
+static int16_t clamp16(int32_t v, int16_t lo, int16_t hi) {
+    if (v < lo) return lo;
+    if (v > hi) return hi;
+    return (int16_t)v;
+}
+
+static float clampf(float v, float lo, float hi) {
+    if (v < lo) return lo;
+    if (v > hi) return hi;
+    return v;
+}
+
+/*
+ * Map a CRSF raw value to [-out_max, +out_max] with a symmetric deadband
+ * around center (992). Within ±CRSF_DEADBAND counts of center → returns 0.
+ * Outside deadband the remaining range is rescaled linearly to ±out_max.
+ */
+static int16_t crsf_stick(uint16_t raw, int16_t out_max) {
+    int32_t centered = (int32_t)raw - 992;
+    if (centered >  CRSF_DEADBAND)  centered -= CRSF_DEADBAND;
+    else if (centered < -CRSF_DEADBAND) centered += CRSF_DEADBAND;
+    else return 0;
+    /* CRSF half-range from centre ≈ 820 counts; subtract deadband */
+    const int32_t half_range = 820 - CRSF_DEADBAND;
+    int32_t out = centered * out_max / half_range;
+    return clamp16(out, -out_max, out_max);
+}
+
+/* Blend target values for each mode (0=pure RC, 1=pure autonomous) */
+static const float k_blend_target[3] = {
+    [MODE_RC_MANUAL]   = 0.0f,
+    [MODE_RC_ASSISTED] = 0.5f,
+    [MODE_AUTONOMOUS]  = 1.0f,
+};
+
+/* Blend advance rate: 1/MODE_BLEND_MS per ms → full 0..1 transition in
+ * MODE_BLEND_MS. Adjacent mode steps (covering 0.5 of range) take 250ms. */
+#define BLEND_RATE  (1.0f / (float)MODE_BLEND_MS)
+
+/* -----------------------------------------------------------------------
+ * Public API
+ * --------------------------------------------------------------------- */
+
+void mode_manager_init(mode_manager_t *m) {
+    m->target          = MODE_RC_MANUAL;
+    m->blend           = 0.0f;
+    m->rc_alive        = false;
+    m->auto_steer      = 0;
+    m->auto_speed_bias = 0;
+}
+
+void mode_manager_update(mode_manager_t *m, uint32_t now) {
+    static uint32_t s_last_tick = 0;
+
+    /* Delta-time (cap at 100ms for first call / resume after gap) */
+    int32_t dt_ms = (int32_t)(now - s_last_tick);
+    if (dt_ms > 100) dt_ms = 100;
+    s_last_tick = now;
+
+    /* Cache RC liveness — checked by main loop too, but needed by getters */
+    m->rc_alive = (crsf_state.last_rx_ms != 0) &&
+                  ((now - crsf_state.last_rx_ms) < RC_TIMEOUT_MS);
+
+    /* Determine mode target from CH6 */
+    if (m->rc_alive) {
+        uint16_t ch6 = crsf_state.channels[CRSF_CH_MODE];
+        if (ch6 <= CRSF_MODE_LOW_THRESH)
+            m->target = MODE_RC_MANUAL;
+        else if (ch6 >= CRSF_MODE_HIGH_THRESH)
+            m->target = MODE_AUTONOMOUS;
+        else
+            m->target = MODE_RC_ASSISTED;
+    }
+    /* If RC is not alive, keep existing target — don't flap to MANUAL just
+     * because the stub returns zeros; kill authority is a separate concern. */
+
+    /* Advance blend toward target value */
+    float target_blend = k_blend_target[m->target];
+    float step = BLEND_RATE * (float)dt_ms;
+    if (m->blend < target_blend)
+        m->blend = clampf(m->blend + step, 0.0f, target_blend);
+    else
+        m->blend = clampf(m->blend - step, target_blend, 1.0f);
+}
+
+void mode_manager_set_auto_cmd(mode_manager_t *m,
+                                int16_t steer,
+                                int16_t speed_bias) {
+    m->auto_steer      = clamp16(steer,      -1000, 1000);
+    m->auto_speed_bias = clamp16(speed_bias,
+                                 -MOTOR_RC_SPEED_MAX,
+                                  MOTOR_RC_SPEED_MAX);
+}
+
+int16_t mode_manager_get_steer(const mode_manager_t *m) {
+    int16_t rc_steer = 0;
+    if (m->rc_alive)
+        rc_steer = crsf_stick(crsf_state.channels[CRSF_CH_STEER], 1000);
+
+    /* lerp: rc_steer → auto_steer over blend */
+    int32_t mixed = (int32_t)rc_steer +
+                    (int32_t)((float)(m->auto_steer - rc_steer) * m->blend);
+    return clamp16(mixed, -1000, 1000);
+}
+
+int16_t mode_manager_get_speed_bias(const mode_manager_t *m) {
+    int16_t rc_bias = 0;
+    if (m->rc_alive)
+        rc_bias = crsf_stick(crsf_state.channels[CRSF_CH_SPEED],
+                             MOTOR_RC_SPEED_MAX);
+
+    int32_t mixed = (int32_t)rc_bias +
+                    (int32_t)((float)(m->auto_speed_bias - rc_bias) * m->blend);
+    return clamp16(mixed, -MOTOR_RC_SPEED_MAX, MOTOR_RC_SPEED_MAX);
+}
+
+robot_mode_t mode_manager_active(const mode_manager_t *m) {
+    if (m->blend < 0.25f) return MODE_RC_MANUAL;
+    if (m->blend > 0.75f) return MODE_AUTONOMOUS;
+    return MODE_RC_ASSISTED;
+}