sl-firmware
9ed678ca35
Issue #680 — IMU mount angle calibration: - imu_cal_flash.h/.c: store pitch/roll offsets in flash sector 7 (0x0807FF00, 64 bytes; preserves PID records across sector erase) - mpu6000_set_mount_offset(): subtracts offsets from pitch/roll output - mpu6000_has_mount_offset(): reports cal_status=2 to Orin - 'O' CDC command: capture current pitch/roll → save to flash → ACK JSON - Load offsets on boot; report in printf log CAN telemetry correction (Tee: production has no USB to Orin): - FC_IMU (0x402): pitch/roll/yaw/cal_status/balance_state at 50 Hz - orin_can_broadcast_imu() rate-limited to ORIN_IMU_TLM_HZ (50 Hz) - FC_BARO (0x403): pressure_pa/temp_x10/alt_cm at 1 Hz (Issue #672) - orin_can_broadcast_baro() rate-limited to ORIN_BARO_TLM_HZ (1 Hz) Issue #685 — LED CAN override: - ORIN_CAN_ID_LED_CMD (0x304): pattern/brightness/duration_ms from Orin - orin_can_led_override volatile state + orin_can_led_updated flag - main.c: apply pattern to LED state machine on each LED_CMD received Orin side: - saltybot_can_node.py: production SocketCAN bridge — reads 0x400-0x403, publishes /saltybot/imu, /saltybot/balance_state, /saltybot/barometer; subscribes /cmd_vel → 0x301 DRIVE; /saltybot/leds → 0x304 LED_CMD; sends 0x300 HEARTBEAT at 5 Hz; sends 0x303 ESTOP on shutdown - setup.py: register saltybot_can_node entry point + uart_bridge launch Fix: re-apply --defsym __stack_end=_estack-0x1000 linker fix to branch Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
153 lines
6.6 KiB
C
153 lines
6.6 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) */
|
||
|
||
/* ---- 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) */
|
||
|
||
/* ---- 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 */
|
||
} 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 */
|
||
|
||
/* 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);
|
||
|
||
#endif /* ORIN_CAN_H */
|