sl-controls
5f03e4cbef
Adds a slow-adapting terrain slope estimator (IIR tau=5s) that decouples the robot's balance offset from genuine ground incline. The balance controller subtracts the slope estimate from measured pitch so the PID balances around the slope surface rather than absolute vertical. - include/slope_estimator.h + src/slope_estimator.c: first-order IIR filter clamped to ±15°; JLINK_TLM_SLOPE (0x88) telemetry at 1 Hz - include/jlink.h + src/jlink.c: add JLINK_TLM_SLOPE (0x88), jlink_tlm_slope_t (4 bytes), jlink_send_slope_tlm() - include/balance.h + src/balance.c: integrate slope_estimator into balance_t; update, reset on tilt-fault and disarm - test/test_slope_estimator.c: 35 unit tests, all passing Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
299 lines
15 KiB
C
299 lines
15 KiB
C
#ifndef JLINK_H
|
|
#define JLINK_H
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include "pid_flash.h" /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */
|
|
|
|
/*
|
|
* JLink -- Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX).
|
|
*
|
|
* Issue #120: replaces jetson_cmd ASCII-over-USB-CDC with a dedicated
|
|
* hardware UART at 921600 baud using DMA circular RX and IDLE interrupt.
|
|
*
|
|
* Frame format (both directions):
|
|
* [STX=0x02][LEN][CMD][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03]
|
|
*
|
|
* STX : frame start sentinel (0x02)
|
|
* LEN : count of CMD + PAYLOAD bytes (1 + payload_len)
|
|
* CMD : command/telemetry type byte
|
|
* PAYLOAD: 0..N bytes depending on CMD
|
|
* CRC16 : CRC16-XModem over CMD+PAYLOAD (poly 0x1021, init 0), big-endian
|
|
* ETX : frame end sentinel (0x03)
|
|
*
|
|
* Jetson to STM32 commands:
|
|
* 0x01 HEARTBEAT - no payload; refreshes heartbeat timer
|
|
* 0x02 DRIVE - int16 speed (-1000..+1000), int16 steer (-1000..+1000)
|
|
* 0x03 ARM - no payload; request arm (same interlock as CDC 'A')
|
|
* 0x04 DISARM - no payload; disarm immediately
|
|
* 0x05 PID_SET - float kp, float ki, float kd (12 bytes, IEEE-754 LE)
|
|
* 0x06 DFU_ENTER - no payload; request OTA DFU reboot (denied while armed)
|
|
* 0x07 ESTOP - no payload; engage emergency stop
|
|
* 0x08 AUDIO - int16 PCM samples (up to 126 samples)
|
|
* 0x09 SLEEP - no payload; request STOP-mode sleep
|
|
* 0x0A PID_SAVE - no payload; save current Kp/Ki/Kd to flash (Issue #531)
|
|
* 0x0B GIMBAL_POS - int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547)
|
|
* 0x0C SCHED_GET - no payload; reply with TLM_SCHED (Issue #550)
|
|
* 0x0D SCHED_SET - uint8 num_bands + N*16-byte pid_sched_entry_t (Issue #550)
|
|
* 0x0E SCHED_SAVE - float kp, ki, kd (12 bytes); save sched+single to flash (Issue #550)
|
|
* 0x0F FAULT_LOG_GET - no payload; reply with TLM_FAULT_LOG (Issue #565)
|
|
*
|
|
* STM32 to Jetson telemetry:
|
|
* 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
|
|
* 0x81 POWER - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ
|
|
* 0x82 BATTERY - jlink_tlm_battery_t (10 bytes, Issue #533)
|
|
* 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
|
|
* 0x84 GIMBAL_STATE - jlink_tlm_gimbal_state_t (10 bytes, Issue #547)
|
|
* 0x85 SCHED - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550)
|
|
* 0x86 MOTOR_CURRENT - jlink_tlm_motor_current_t (8 bytes, Issue #584)
|
|
* 0x87 FAULT_LOG - jlink_tlm_fault_log_t (20 bytes), sent on boot + FAULT_LOG_GET (Issue #565)
|
|
*
|
|
* Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
|
|
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
|
|
* RC_ASSISTED modes the Jetson speed offset and steer are injected via
|
|
* mode_manager_set_auto_cmd() and blended per the existing blend ramp.
|
|
*
|
|
* Heartbeat: if no valid frame arrives within JLINK_HB_TIMEOUT_MS (1000ms),
|
|
* jlink_is_active() returns false and the main loop clears the auto command.
|
|
*/
|
|
|
|
/* ---- Frame constants ---- */
|
|
#define JLINK_STX 0x02u
|
|
#define JLINK_ETX 0x03u
|
|
|
|
/* ---- Command IDs (Jetson to STM32) ---- */
|
|
#define JLINK_CMD_HEARTBEAT 0x01u
|
|
#define JLINK_CMD_DRIVE 0x02u
|
|
#define JLINK_CMD_ARM 0x03u
|
|
#define JLINK_CMD_DISARM 0x04u
|
|
#define JLINK_CMD_PID_SET 0x05u
|
|
#define JLINK_CMD_DFU_ENTER 0x06u
|
|
#define JLINK_CMD_ESTOP 0x07u
|
|
#define JLINK_CMD_AUDIO 0x08u /* PCM audio chunk: int16 samples, up to 126 */
|
|
#define JLINK_CMD_SLEEP 0x09u /* no payload; request STOP-mode sleep */
|
|
#define JLINK_CMD_PID_SAVE 0x0Au /* no payload; save Kp/Ki/Kd to flash (Issue #531) */
|
|
#define JLINK_CMD_GIMBAL_POS 0x0Bu /* int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) */
|
|
#define JLINK_CMD_SCHED_GET 0x0Cu /* no payload; reply TLM_SCHED (Issue #550) */
|
|
#define JLINK_CMD_SCHED_SET 0x0Du /* uint8 num_bands + N*16-byte entries (Issue #550) */
|
|
#define JLINK_CMD_SCHED_SAVE 0x0Eu /* float kp,ki,kd; save sched+single to flash (Issue #550) */
|
|
#define JLINK_CMD_FAULT_LOG_GET 0x0Fu /* no payload; reply TLM_FAULT_LOG (Issue #565) */
|
|
|
|
/* ---- Telemetry IDs (STM32 to Jetson) ---- */
|
|
#define JLINK_TLM_STATUS 0x80u
|
|
#define JLINK_TLM_POWER 0x81u /* jlink_tlm_power_t (11 bytes) */
|
|
#define JLINK_TLM_BATTERY 0x82u /* jlink_tlm_battery_t (10 bytes, Issue #533) */
|
|
#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */
|
|
#define JLINK_TLM_GIMBAL_STATE 0x84u /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */
|
|
#define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */
|
|
#define JLINK_TLM_MOTOR_CURRENT 0x86u /* jlink_tlm_motor_current_t (8 bytes, Issue #584) */
|
|
#define JLINK_TLM_FAULT_LOG 0x87u /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */
|
|
#define JLINK_TLM_SLOPE 0x88u /* jlink_tlm_slope_t (4 bytes, Issue #600) */
|
|
|
|
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
|
|
typedef struct __attribute__((packed)) {
|
|
int16_t pitch_x10; /* pitch degrees x10 */
|
|
int16_t roll_x10; /* roll degrees x10 */
|
|
int16_t yaw_x10; /* yaw degrees x10 (gyro-integrated) */
|
|
int16_t motor_cmd; /* ESC output -1000..+1000 */
|
|
uint16_t vbat_mv; /* battery millivolts */
|
|
int8_t rssi_dbm; /* CRSF RSSI (dBm, negative) */
|
|
uint8_t link_quality; /* CRSF LQ 0-100 */
|
|
uint8_t balance_state; /* 0=DISARMED, 1=ARMED, 2=TILT_FAULT */
|
|
uint8_t rc_armed; /* crsf_state.armed (1=armed) */
|
|
uint8_t mode; /* robot_mode_t: 0=RC_MANUAL,1=ASSISTED,2=AUTONOMOUS */
|
|
uint8_t estop; /* EstopSource value */
|
|
uint8_t soc_pct; /* state-of-charge 0-100, 255=unknown */
|
|
uint8_t fw_major;
|
|
uint8_t fw_minor;
|
|
uint8_t fw_patch;
|
|
} jlink_tlm_status_t; /* 20 bytes */
|
|
|
|
/* ---- Telemetry POWER payload (11 bytes, packed) ---- */
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t power_state; /* PowerState: 0=ACTIVE,1=SLEEP_PENDING,2=SLEEPING,3=WAKING */
|
|
uint16_t est_total_ma; /* estimated total current draw (mA) */
|
|
uint16_t est_audio_ma; /* estimated I2S3+amp current (mA); 0 if gated */
|
|
uint16_t est_osd_ma; /* estimated OSD SPI2 current (mA); 0 if gated */
|
|
uint32_t idle_ms; /* ms since last cmd_vel activity */
|
|
} jlink_tlm_power_t; /* 11 bytes */
|
|
|
|
/* ---- Telemetry BATTERY payload (10 bytes, packed) Issue #533 ---- */
|
|
typedef struct __attribute__((packed)) {
|
|
uint16_t vbat_mv; /* DMA-sampled LPF-filtered Vbat (mV) */
|
|
int16_t ibat_ma; /* DMA-sampled LPF-filtered Ibat (mA, + = discharge) */
|
|
uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV) */
|
|
uint8_t flags; /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready */
|
|
int8_t cal_offset; /* vbat_offset_mv / 4 (+-127 -> +-508 mV) */
|
|
uint8_t lpf_shift; /* IIR shift factor (alpha = 1/2^lpf_shift) */
|
|
uint8_t soc_pct; /* voltage-based SoC 0-100, 255 = unknown */
|
|
} jlink_tlm_battery_t; /* 10 bytes */
|
|
|
|
/* ---- Telemetry PID_RESULT payload (13 bytes, packed) Issue #531 ---- */
|
|
/* Sent after JLINK_CMD_PID_SAVE is processed; confirms gains written to flash. */
|
|
typedef struct __attribute__((packed)) {
|
|
float kp; /* Kp saved */
|
|
float ki; /* Ki saved */
|
|
float kd; /* Kd saved */
|
|
uint8_t saved_ok; /* 1 = flash write verified, 0 = write failed */
|
|
} jlink_tlm_pid_result_t; /* 13 bytes */
|
|
|
|
/* ---- Telemetry GIMBAL_STATE payload (10 bytes, packed) Issue #547 ---- */
|
|
/* Sent at GIMBAL_TLM_HZ (50 Hz); reports measured pan/tilt and speed. */
|
|
typedef struct __attribute__((packed)) {
|
|
int16_t pan_x10; /* Measured pan angle, deg x10 */
|
|
int16_t tilt_x10; /* Measured tilt angle, deg x10 */
|
|
uint16_t pan_speed_raw; /* Present speed register, pan servo */
|
|
uint16_t tilt_speed_raw; /* Present speed register, tilt servo */
|
|
uint8_t torque_en; /* 1 = torque enabled */
|
|
uint8_t rx_err_pct; /* bus error rate 0-100% (rx_err*100/(rx_ok+rx_err)) */
|
|
} jlink_tlm_gimbal_state_t; /* 10 bytes */
|
|
|
|
/* ---- Telemetry SCHED payload (1 + N*16 bytes, packed) Issue #550 ---- */
|
|
/* Sent in response to JLINK_CMD_SCHED_GET; N = num_bands (1..PID_SCHED_MAX_BANDS). */
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t num_bands; /* number of valid entries */
|
|
pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* up to 6 x 16 = 96 bytes */
|
|
} jlink_tlm_sched_t; /* 1 + 96 = 97 bytes max */
|
|
|
|
/* ---- Telemetry MOTOR_CURRENT payload (8 bytes, packed) Issue #584 ---- */
|
|
/* Published at MOTOR_CURR_TLM_HZ; reports measured current and protection state. */
|
|
typedef struct __attribute__((packed)) {
|
|
int32_t current_ma; /* filtered battery/motor current (mA, + = discharge) */
|
|
uint8_t limit_pct; /* soft-limit reduction applied: 0=none, 100=full cutoff */
|
|
uint8_t state; /* MotorCurrentState: 0=NORMAL,1=SOFT_LIMIT,2=COOLDOWN */
|
|
uint8_t fault_count; /* lifetime hard-cutoff trips (saturates at 255) */
|
|
uint8_t _pad; /* reserved */
|
|
} jlink_tlm_motor_current_t; /* 8 bytes */
|
|
|
|
/* ---- Telemetry SLOPE payload (4 bytes, packed) Issue #600 ---- */
|
|
/* Published at SLOPE_TLM_HZ (1 Hz); reports slow-adapting terrain slope estimate. */
|
|
typedef struct __attribute__((packed)) {
|
|
int16_t slope_x100; /* slope estimate degrees x100 (0.01° resolution) */
|
|
uint8_t active; /* 1 = slope compensation enabled */
|
|
uint8_t _pad; /* reserved */
|
|
} jlink_tlm_slope_t; /* 4 bytes */
|
|
|
|
/* ---- Telemetry FAULT_LOG payload (20 bytes, packed) Issue #565 ---- */
|
|
/* Sent on boot (if last fault != NONE) and in response to FAULT_LOG_GET. */
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t fault_type; /* FaultType of most recent entry */
|
|
uint8_t entry_count; /* number of valid entries in flash log (0-8) */
|
|
uint8_t reset_count; /* lifetime reset counter */
|
|
uint8_t _pad;
|
|
uint32_t timestamp_ms; /* HAL_GetTick() at fault */
|
|
uint32_t pc; /* faulting PC */
|
|
uint32_t lr; /* link register at fault */
|
|
uint32_t cfsr; /* SCB->CFSR */
|
|
uint32_t hfsr; /* SCB->HFSR */
|
|
} jlink_tlm_fault_log_t; /* 20 bytes */
|
|
|
|
/* ---- Volatile state (read from main loop) ---- */
|
|
typedef struct {
|
|
/* Drive command - updated on JLINK_CMD_DRIVE */
|
|
volatile int16_t speed; /* -1000..+1000 */
|
|
volatile int16_t steer; /* -1000..+1000 */
|
|
|
|
/* Heartbeat timer - updated on any valid frame */
|
|
volatile uint32_t last_rx_ms; /* HAL_GetTick() of last valid frame; 0=none */
|
|
|
|
/* One-shot request flags - set by parser, cleared by main loop */
|
|
volatile uint8_t arm_req;
|
|
volatile uint8_t disarm_req;
|
|
volatile uint8_t estop_req;
|
|
|
|
/* PID update - set by parser, cleared by main loop */
|
|
volatile uint8_t pid_updated;
|
|
volatile float pid_kp;
|
|
volatile float pid_ki;
|
|
volatile float pid_kd;
|
|
|
|
/* DFU reboot request - set by parser, cleared by main loop */
|
|
volatile uint8_t dfu_req;
|
|
/* Sleep request - set by JLINK_CMD_SLEEP, cleared by main loop */
|
|
volatile uint8_t sleep_req;
|
|
/* PID save request - set by JLINK_CMD_PID_SAVE, cleared by main loop (Issue #531) */
|
|
volatile uint8_t pid_save_req;
|
|
|
|
/* Gimbal position command - set by JLINK_CMD_GIMBAL_POS (Issue #547) */
|
|
volatile uint8_t gimbal_updated; /* set by parser, cleared by main loop */
|
|
volatile int16_t gimbal_pan_x10; /* pan angle deg x10 */
|
|
volatile int16_t gimbal_tilt_x10; /* tilt angle deg x10 */
|
|
volatile uint16_t gimbal_speed; /* servo speed 0-4095 (0=max) */
|
|
|
|
/* PID schedule commands (Issue #550) - set by parser, cleared by main loop */
|
|
volatile uint8_t sched_get_req; /* SCHED_GET: main loop calls jlink_send_sched_telemetry() */
|
|
volatile uint8_t sched_save_req; /* SCHED_SAVE: main loop calls pid_schedule_flash_save() */
|
|
volatile float sched_save_kp; /* kp for single-PID record in SCHED_SAVE */
|
|
volatile float sched_save_ki;
|
|
volatile float sched_save_kd;
|
|
|
|
/* Fault log request (Issue #565) - set by JLINK_CMD_FAULT_LOG_GET, cleared by main loop */
|
|
volatile uint8_t fault_log_req;
|
|
} JLinkState;
|
|
|
|
extern volatile JLinkState jlink_state;
|
|
|
|
/* ---- SCHED_SET receive buffer -- Issue #550 ---- */
|
|
/*
|
|
* Populated by the parser on JLINK_CMD_SCHED_SET. Main loop reads via
|
|
* jlink_get_sched_set() and calls pid_schedule_set_table() before clearing.
|
|
*/
|
|
typedef struct {
|
|
volatile uint8_t ready; /* set by parser, cleared by main loop */
|
|
volatile uint8_t num_bands;
|
|
pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* copied from frame */
|
|
} JLinkSchedSetBuf;
|
|
|
|
/* ---- API ---- */
|
|
|
|
void jlink_init(void);
|
|
bool jlink_is_active(uint32_t now_ms);
|
|
void jlink_process(void);
|
|
void jlink_send_telemetry(const jlink_tlm_status_t *status);
|
|
void jlink_send_power_telemetry(const jlink_tlm_power_t *power);
|
|
void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt);
|
|
void jlink_send_pid_result(const jlink_tlm_pid_result_t *result);
|
|
|
|
/*
|
|
* jlink_send_gimbal_state(state) - transmit JLINK_TLM_GIMBAL_STATE (0x84)
|
|
* frame (16 bytes) at GIMBAL_TLM_HZ (50 Hz). Issue #547.
|
|
*/
|
|
void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state);
|
|
|
|
/*
|
|
* jlink_send_sched_telemetry(tlm) - transmit JLINK_TLM_SCHED (0x85) in
|
|
* response to SCHED_GET. tlm->num_bands determines actual frame size.
|
|
* Issue #550.
|
|
*/
|
|
void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm);
|
|
|
|
/*
|
|
* jlink_get_sched_set() - return pointer to the most-recently received
|
|
* SCHED_SET payload buffer (static storage in jlink.c). Main loop calls
|
|
* pid_schedule_set_table() from this buffer, then clears ready. Issue #550.
|
|
*/
|
|
JLinkSchedSetBuf *jlink_get_sched_set(void);
|
|
|
|
/*
|
|
* jlink_send_motor_current_tlm(tlm) - transmit JLINK_TLM_MOTOR_CURRENT (0x86)
|
|
* frame (14 bytes total) to Jetson. Issue #584.
|
|
* Rate-limiting is handled by motor_current_send_tlm(); call from there only.
|
|
*/
|
|
void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm);
|
|
|
|
/*
|
|
* jlink_send_slope_tlm(tlm) - transmit JLINK_TLM_SLOPE (0x88) frame
|
|
* (10 bytes total) to Jetson. Issue #600.
|
|
* Rate-limiting is handled by slope_estimator_send_tlm(); call from there only.
|
|
*/
|
|
void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm);
|
|
|
|
/*
|
|
* jlink_send_fault_log(fl) - transmit JLINK_TLM_FAULT_LOG (0x87) frame
|
|
* (26 bytes) on boot (if fault log non-empty) and in response to
|
|
* FAULT_LOG_GET. Issue #565.
|
|
*/
|
|
void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl);
|
|
|
|
#endif /* JLINK_H */
|