seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

feat: RC/autonomous mode manager with smooth handoff

Browse Source 
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>
This commit is contained in:
sl-controls 2026-02-28 21:06:20 -05:00
parent 76dc03db1b
commit 8abd88e05d
4 changed files with 262 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
include/config.h
View File

@ -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

74
include/mode_manager.h Normal file
View File

@ -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

48
src/main.c
View File

@ -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: 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) */
@ -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)

129
src/mode_manager.c Normal file
View File

@ -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;
}