sl-controls
ea5e2dac72
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>
75 lines
2.6 KiB
C
75 lines
2.6 KiB
C
#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
|