sl-controls
cc0ffd1999
STM32F7 ADC driver for battery voltage/current monitoring using DMA-based continuous sampling, IIR low-pass filter, voltage divider calibration, and USART telemetry to Jetson. Integrates with power management for low-battery sleep (Issue #467). Implementation: - include/battery_adc.h: New driver header with calibration struct and public API (init, tick, get_voltage_mv, get_current_ma, calibrate, publish, check_pm, is_low, is_critical) - src/battery_adc.c: ADC3 continuous-scan DMA (DMA2_Stream0/Ch2), 4x hardware oversampling of both Vbat (PC1/IN11) and Ibat (PC3/IN13), IIR LPF (alpha=1/8, cutoff ~4 Hz at 100 Hz tick rate), calibration with ±500 mV offset clamp, 3S/4S auto-detection, 1 Hz USART publish - include/jlink.h + src/jlink.c: Add JLINK_TLM_BATTERY (0x82) telemetry type and jlink_tlm_battery_t (10-byte packed struct), implement jlink_send_battery_telemetry() using CRC16-XModem framing - include/power_mgmt.h + src/power_mgmt.c: Add power_mgmt_notify_battery() — triggers STOP-mode sleep when Vbat sustains critical level (Issue #467) - test/test_battery_adc.c: 27 unit tests (27/27 passing): voltage conversion, calibration offset/scale, IIR LPF convergence, SoC estimation (3S/4S), low/critical flags, PM notification timing, calibration reset, publish rate-limiting Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
172 lines
7.0 KiB
C
172 lines
7.0 KiB
C
#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 → 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
|
||
*
|
||
* STM32 → Jetson telemetry:
|
||
* 0x80 STATUS — jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
|
||
*
|
||
* 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 → 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 */
|
||
|
||
/* ---- Telemetry IDs (STM32 → 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) */
|
||
|
||
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
|
||
typedef struct __attribute__((packed)) {
|
||
int16_t pitch_x10; /* pitch degrees ×10 */
|
||
int16_t roll_x10; /* roll degrees ×10 */
|
||
int16_t yaw_x10; /* yaw degrees ×10 (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 (α = 1/2^lpf_shift) */
|
||
uint8_t soc_pct; /* voltage-based SoC 0–100, 255 = unknown */
|
||
} jlink_tlm_battery_t; /* 10 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;
|
||
} 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_battery_telemetry(batt) — build and transmit JLINK_TLM_BATTERY
|
||
* (0x82) frame (16 bytes) at BATTERY_ADC_PUBLISH_HZ (1 Hz).
|
||
* Called by battery_adc_publish(); not normally called directly.
|
||
*/
|
||
void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt);
|
||
|
||
#endif /* JLINK_H */
|