seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity
saltylab-firmware/include/jlink.h
sl-firmware 19a30a1c4f feat: PID auto-tune for balance mode (Issue #531) 
Implement Ziegler-Nichols relay feedback auto-tuning with flash persistence:

Firmware (STM32F722):
- pid_flash.c/h: erase+write Kp/Ki/Kd to flash sector 7 (0x0807FFC0),
  magic-validated; load on boot to restore saved tune
- jlink.h: add JLINK_CMD_PID_SAVE (0x0A) and JLINK_TLM_PID_RESULT (0x83)
  with jlink_tlm_pid_result_t struct and pid_save_req flag in JLinkState
- jlink.c: dispatch JLINK_CMD_PID_SAVE -> pid_save_req; add
  jlink_send_pid_result() to confirm flash write outcome over USART1
- main.c: load saved PID from flash after balance_init(); handle
  pid_save_req in main loop (disarmed-only, erase stalls CPU ~1s)

Jetson ROS2 (saltybot_pid_autotune):
- pid_autotune_node.py: add Ki to Ziegler-Nichols formula (ZN PID:
  Kp=0.6Ku, Ki=1.2Ku/Tu, Kd=0.075KuTu); add JLink serial client that
  sends JLINK_CMD_PID_SET + JLINK_CMD_PID_SAVE after tuning completes
- autotune_config.yaml: add jlink_serial_port and jlink_baud_rate params

Trigger: ros2 service call /saltybot/autotune_pid std_srvs/srv/Trigger

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-07 09:56:19 -05:00

175 lines
7.1 KiB
C
Raw Permalink Blame History

#ifndef JLINK_H
#define JLINK_H
#include <stdint.h>
#include <stdbool.h>
/*
* 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
* 0x0A PID_SAVE - no payload; save current Kp/Ki/Kd to flash (Issue #531)
*
* 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
* 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
*
* 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) */
/* ---- Telemetry IDs (STM32 to Jetson) ---- */
#define JLINK_TLM_STATUS 0x80u
#define JLINK_TLM_POWER 0x81u /* jlink_tlm_power_t (11 bytes) */
#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes) Issue #531 */
/* ---- 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 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 */
/* ---- 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;
} JLinkState;
extern volatile JLinkState jlink_state;
/* ---- API ---- */
/*
* jlink_init() - configure USART1 (PB6=TX, PB7=RX) at 921600 baud with
* DMA2_Stream2_Channel4 circular RX (128-byte buffer) and IDLE interrupt.
* Call once before safety_init().
*/
void jlink_init(void);
/*
* jlink_is_active(now_ms) - returns true if a valid frame arrived within
* JLINK_HB_TIMEOUT_MS. Returns false if no frame ever received.
*/
bool jlink_is_active(uint32_t now_ms);
/*
* jlink_send_telemetry(status) - build and transmit a JLINK_TLM_STATUS frame
* over USART1 TX (blocking, ~0.2ms at 921600). Call at JLINK_TLM_HZ.
*/
void jlink_send_telemetry(const jlink_tlm_status_t *status);
/*
* jlink_process() - drain DMA circular buffer and parse frames.
* Call from main loop every iteration (not ISR). Lightweight: O(bytes_pending).
*/
void jlink_process(void);
/*
* jlink_send_power_telemetry(power) - build and transmit a JLINK_TLM_POWER
* frame (17 bytes) at PM_TLM_HZ. Call from main loop when not in STOP mode.
*/
void jlink_send_power_telemetry(const jlink_tlm_power_t *power);
/*
* jlink_send_pid_result(result) - build and transmit a JLINK_TLM_PID_RESULT
* frame (19 bytes) to confirm PID flash save outcome (Issue #531).
*/
void jlink_send_pid_result(const jlink_tlm_pid_result_t *result);
#endif /* JLINK_H */
Reference in New Issue View Git Blame Copy Permalink