diff --git a/include/config.h b/include/config.h index b05641f..2c09564 100644 --- a/include/config.h +++ b/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 diff --git a/include/mode_manager.h b/include/mode_manager.h new file mode 100644 index 0000000..4b8194c --- /dev/null +++ b/include/mode_manager.h @@ -0,0 +1,74 @@ +#ifndef MODE_MANAGER_H +#define MODE_MANAGER_H + +#include +#include + +/* + * 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 diff --git a/src/main.c b/src/main.c index 45e19f4..c5f87d5 100644 --- a/src/main.c +++ b/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 #include #include @@ -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_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */ mag_type_t mag_type = MAG_NONE; @@ -163,10 +169,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: balance_disarm(&bal); */ + /* Uncomment when CRSF is wired (last_rx_ms stays 0 on stub): */ + /* balance_disarm(&bal); */ } /* Tilt fault buzzer alert (one-shot on fault edge) */ @@ -200,10 +215,19 @@ int main(void) { CDC_Transmit((uint8_t *)reply, strlen(reply)); } - /* Balance PID at 1kHz */ + /* Handle Jetson C, 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 */ @@ -213,11 +237,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); @@ -243,6 +278,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) diff --git a/src/mode_manager.c b/src/mode_manager.c new file mode 100644 index 0000000..81be333 --- /dev/null +++ b/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; +}