sl-controls
de4d1bbe3a
- Mamba (STM32): add ORIN_CAN_ID_PID_SET (0x305) handler in orin_can.c. Receives kp/ki/kd as uint16*100 (BE), applies to running balance loop, replies with FC_PID_ACK (0x405) echoing clamped gains. Gains persist in RAM until reboot; not saved to flash. - Jetson: expose pid/kp, pid/ki, pid/kd as ROS2 parameters in can_bridge_node. Parameter changes trigger encode_pid_set_cmd() and send CAN frame 0x305 immediately. ACK frame 0x405 logged at DEBUG. - mamba_protocol.py: add ORIN_CAN_ID_PID_SET / FC_PID_ACK IDs, PidGains dataclass, encode_pid_set_cmd(), decode_pid_ack(). Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
192 lines
8.3 KiB
C
192 lines
8.3 KiB
C
#ifndef ORIN_CAN_H
|
||
#define ORIN_CAN_H
|
||
|
||
#include <stdint.h>
|
||
#include <stdbool.h>
|
||
|
||
/*
|
||
* orin_can — Orin↔FC CAN protocol driver (Issue #674).
|
||
*
|
||
* Standard 11-bit CAN IDs on CAN2, FIFO0.
|
||
*
|
||
* Orin → FC commands:
|
||
* 0x300 HEARTBEAT : uint32 sequence counter (4 bytes)
|
||
* 0x301 DRIVE : int16 speed (−1000..+1000), int16 steer (−1000..+1000)
|
||
* 0x302 MODE : uint8 mode (0=RC_MANUAL, 1=ASSISTED, 2=AUTONOMOUS)
|
||
* 0x303 ESTOP : uint8 action (1=ESTOP, 0=CLEAR)
|
||
*
|
||
* FC → Orin telemetry (broadcast at ORIN_TLM_HZ):
|
||
* 0x400 FC_STATUS : int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv,
|
||
* uint8 balance_state, uint8 flags [bit0=estop, bit1=armed]
|
||
* 0x401 FC_VESC : int16 left_rpm_x10 (RPM/10), int16 right_rpm_x10,
|
||
* int16 left_current_x10, int16 right_current_x10
|
||
*
|
||
* Balance independence: if no Orin heartbeat for ORIN_HB_TIMEOUT_MS, the FC
|
||
* continues balancing in-place — Orin commands are simply not injected.
|
||
* The balance PID loop runs entirely on Mamba and never depends on Orin.
|
||
*/
|
||
|
||
/* ---- Orin → FC command IDs ---- */
|
||
#define ORIN_CAN_ID_HEARTBEAT 0x300u
|
||
#define ORIN_CAN_ID_DRIVE 0x301u
|
||
#define ORIN_CAN_ID_MODE 0x302u
|
||
#define ORIN_CAN_ID_ESTOP 0x303u
|
||
#define ORIN_CAN_ID_LED_CMD 0x304u /* LED pattern override (Issue #685) */
|
||
#define ORIN_CAN_ID_PID_SET 0x305u /* PID gain update: kp/ki/kd (Issue #693) */
|
||
|
||
/* ---- FC → Orin telemetry IDs ---- */
|
||
#define ORIN_CAN_ID_FC_STATUS 0x400u /* balance state + pitch + vbat at 10 Hz */
|
||
#define ORIN_CAN_ID_FC_VESC 0x401u /* VESC RPM + current at 10 Hz */
|
||
#define ORIN_CAN_ID_FC_IMU 0x402u /* full IMU angles + cal status at 50 Hz (Issue #680) */
|
||
#define ORIN_CAN_ID_FC_BARO 0x403u /* barometer pressure/temp/altitude at 1 Hz (Issue #672) */
|
||
#define ORIN_CAN_ID_FC_BTN 0x404u /* button event on-demand (Issue #682) */
|
||
#define ORIN_CAN_ID_FC_PID_ACK 0x405u /* PID gain ACK: echoes applied kp/ki/kd (Issue #693) */
|
||
|
||
/* ---- Timing ---- */
|
||
#define ORIN_HB_TIMEOUT_MS 500u /* Orin offline after 500 ms without any frame */
|
||
#define ORIN_TLM_HZ 10u /* FC_STATUS + FC_VESC broadcast rate (Hz) */
|
||
#define ORIN_IMU_TLM_HZ 50u /* FC_IMU broadcast rate (Hz) */
|
||
#define ORIN_BARO_TLM_HZ 1u /* FC_BARO broadcast rate (Hz) */
|
||
|
||
/* ---- Volatile state updated by orin_can_on_frame(), read by main loop ---- */
|
||
typedef struct {
|
||
volatile int16_t speed; /* DRIVE: −1000..+1000 */
|
||
volatile int16_t steer; /* DRIVE: −1000..+1000 */
|
||
volatile uint8_t mode; /* MODE: robot_mode_t value */
|
||
volatile uint8_t drive_updated; /* set on DRIVE, cleared by main */
|
||
volatile uint8_t mode_updated; /* set on MODE, cleared by main */
|
||
volatile uint8_t estop_req; /* set on ESTOP(1), cleared by main */
|
||
volatile uint8_t estop_clear_req; /* set on ESTOP(0), cleared by main */
|
||
volatile uint32_t last_rx_ms; /* HAL_GetTick() of last received frame */
|
||
/* PID_SET (Issue #693) -- set by orin_can_on_frame(), consumed by main */
|
||
volatile uint8_t pid_updated; /* set on PID_SET, cleared by main */
|
||
volatile uint16_t pid_kp_x100; /* Kp * 100 (0..50000) */
|
||
volatile uint16_t pid_ki_x100; /* Ki * 100 (0..5000) */
|
||
volatile uint16_t pid_kd_x100; /* Kd * 100 (0..5000) */
|
||
} OrinCanState;
|
||
|
||
extern volatile OrinCanState orin_can_state;
|
||
|
||
/* ---- FC → Orin broadcast payloads (packed, 8 bytes each) ---- */
|
||
|
||
typedef struct __attribute__((packed)) {
|
||
int16_t pitch_x10; /* pitch degrees × 10 */
|
||
int16_t motor_cmd; /* balance PID output −1000..+1000 */
|
||
uint16_t vbat_mv; /* battery voltage (mV) */
|
||
uint8_t balance_state; /* BalanceState: 0=DISARMED,1=ARMED,2=TILT_FAULT */
|
||
uint8_t flags; /* bit0=estop_active, bit1=armed */
|
||
} orin_can_fc_status_t; /* 8 bytes */
|
||
|
||
typedef struct __attribute__((packed)) {
|
||
int16_t left_rpm_x10; /* left wheel RPM / 10 (±32767 × 10 = ±327k RPM) */
|
||
int16_t right_rpm_x10; /* right wheel RPM / 10 */
|
||
int16_t left_current_x10; /* left phase current × 10 (A) */
|
||
int16_t right_current_x10; /* right phase current × 10 (A) */
|
||
} orin_can_fc_vesc_t; /* 8 bytes */
|
||
|
||
/* FC_IMU (0x402) — full IMU angles at 50 Hz (Issue #680) */
|
||
typedef struct __attribute__((packed)) {
|
||
int16_t pitch_x10; /* pitch degrees × 10 (mount-offset corrected) */
|
||
int16_t roll_x10; /* roll degrees × 10 (mount-offset corrected) */
|
||
int16_t yaw_x10; /* yaw degrees × 10 (gyro-integrated, drifts) */
|
||
uint8_t cal_status; /* 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */
|
||
uint8_t balance_state; /* BalanceState: 0=DISARMED, 1=ARMED, 2=TILT_FAULT */
|
||
} orin_can_fc_imu_t; /* 8 bytes */
|
||
|
||
/* FC_BARO (0x403) — barometer at 1 Hz (Issue #672) */
|
||
typedef struct __attribute__((packed)) {
|
||
int32_t pressure_pa; /* barometric pressure in Pa */
|
||
int16_t temp_x10; /* temperature × 10 (°C) */
|
||
int16_t alt_cm; /* altitude in cm (reference = pressure at boot) */
|
||
} orin_can_fc_baro_t; /* 8 bytes */
|
||
|
||
/* FC_BTN (0x404) — button event, sent on demand (Issue #682)
|
||
* event_id: 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED (pitch too large) */
|
||
typedef struct __attribute__((packed)) {
|
||
uint8_t event_id; /* 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED */
|
||
uint8_t balance_state; /* balance_state_t value after the event */
|
||
} orin_can_fc_btn_t; /* 2 bytes */
|
||
|
||
/* LED_CMD (0x304) — Orin → FC LED pattern override (Issue #685)
|
||
* duration_ms = 0: hold until next state change; >0: revert after duration */
|
||
typedef struct __attribute__((packed)) {
|
||
uint8_t pattern; /* 0=state_auto, 1=solid, 2=slow_blink, 3=fast_blink, 4=pulse */
|
||
uint8_t brightness; /* 0-255 (0 = both LEDs off) */
|
||
uint16_t duration_ms; /* override duration; 0 = permanent until state change */
|
||
} orin_can_led_cmd_t; /* 4 bytes */
|
||
|
||
/* LED override state (updated by orin_can_on_frame, read by main loop) */
|
||
extern volatile orin_can_led_cmd_t orin_can_led_override;
|
||
extern volatile uint8_t orin_can_led_updated;
|
||
|
||
/* ---- API ---- */
|
||
|
||
/*
|
||
* orin_can_init() — zero state, register orin_can_on_frame as std_cb with
|
||
* can_driver. Call after can_driver_init().
|
||
*/
|
||
void orin_can_init(void);
|
||
|
||
/*
|
||
* orin_can_on_frame(std_id, data, len) — dispatched by can_driver for each
|
||
* standard-ID frame in FIFO0. Updates orin_can_state.
|
||
*/
|
||
void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len);
|
||
|
||
/*
|
||
* orin_can_is_alive(now_ms) — true if a frame from Orin arrived within
|
||
* ORIN_HB_TIMEOUT_MS of now_ms.
|
||
*/
|
||
bool orin_can_is_alive(uint32_t now_ms);
|
||
|
||
/*
|
||
* orin_can_broadcast(now_ms, status, vesc) — rate-limited broadcast of
|
||
* FC_STATUS (0x400) and FC_VESC (0x401) at ORIN_TLM_HZ (10 Hz).
|
||
* Safe to call every ms; internally rate-limited.
|
||
*/
|
||
void orin_can_broadcast(uint32_t now_ms,
|
||
const orin_can_fc_status_t *status,
|
||
const orin_can_fc_vesc_t *vesc);
|
||
|
||
/*
|
||
* orin_can_broadcast_imu(now_ms, imu_tlm) — rate-limited broadcast of
|
||
* FC_IMU (0x402) at ORIN_IMU_TLM_HZ (50 Hz).
|
||
* Safe to call every ms; internally rate-limited. Issue #680.
|
||
*/
|
||
void orin_can_broadcast_imu(uint32_t now_ms,
|
||
const orin_can_fc_imu_t *imu_tlm);
|
||
|
||
/*
|
||
* orin_can_broadcast_baro(now_ms, baro_tlm) — rate-limited broadcast of
|
||
* FC_BARO (0x403) at ORIN_BARO_TLM_HZ (1 Hz).
|
||
* Pass NULL to skip transmission. Issue #672.
|
||
*/
|
||
void orin_can_broadcast_baro(uint32_t now_ms,
|
||
const orin_can_fc_baro_t *baro_tlm);
|
||
|
||
/*
|
||
* orin_can_send_btn_event(event_id, balance_state) — send FC_BTN (0x404)
|
||
* immediately. Call on button park/unpark events. Issue #682.
|
||
* event_id: 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED.
|
||
*/
|
||
void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state);
|
||
|
||
/* orin_can_send_pid_ack() -- send FC_PID_ACK (0x405). Issue #693. */
|
||
void orin_can_send_pid_ack(float kp, float ki, float kd);
|
||
|
||
/* PID_SET (0x305) -- 6-byte payload: kp*100, ki*100, kd*100 (uint16 BE each) */
|
||
typedef struct __attribute__((packed)) {
|
||
uint16_t kp_x100;
|
||
uint16_t ki_x100;
|
||
uint16_t kd_x100;
|
||
} orin_can_pid_set_t;
|
||
|
||
/* FC_PID_ACK (0x405) -- FC -> Orin echo of applied gains */
|
||
typedef struct __attribute__((packed)) {
|
||
uint16_t kp_x100;
|
||
uint16_t ki_x100;
|
||
uint16_t kd_x100;
|
||
} orin_can_fc_pid_ack_t;
|
||
|
||
#endif /* ORIN_CAN_H */
|