sl-jetson
22aaeb02cf
STM32 firmware (C): - include/jetson_cmd.h: protocol constants (HB_TIMEOUT_MS=500, SPEED_MAX_DEG=4°), API for jetson_cmd_process/is_active/steer/sp_offset - src/jetson_cmd.c: main-loop parser for buffered C<spd>,<str> frames; setpoint offset = speed/1000 * 4°; steer clamped ±1000 - lib/USB_CDC/src/usbd_cdc_if.c: add H (heartbeat) and C (drive cmd) to CDC_Receive ISR — follows existing pattern: H updates jetson_hb_tick in ISR, C copied to jetson_cmd_buf for main-loop sscanf (avoids sscanf in IRQ) - src/main.c: integrate jetson_cmd — process buffered frame, apply setpoint offset around balance_update(), inject steer into motor_driver_update() only when heartbeat alive (fallback: steer=0, setpoint unchanged) ROS2 (Python): - saltybot_cmd_node.py: full bidirectional node — owns serial port, handles telemetry RX → topics AND /cmd_vel TX → C<spd>,<str>\n + H\n heartbeat 200ms timer; sends C0,0\n on shutdown; speed/steer_scale configurable - serial_bridge_node.py: add write_serial() helper for extensibility - launch/bridge.launch.py: mode arg (bidirectional|rx_only) selects node - config/bridge_params.yaml: heartbeat_period, speed_scale, steer_scale docs - test/test_cmd.py: 13 tests — zero, full fwd/rev, turn clamping, combined - setup.py: saltybot_cmd_node entry point All 21 tests pass (8 parse + 13 cmd). Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
77 lines
2.8 KiB
C
77 lines
2.8 KiB
C
#ifndef JETSON_CMD_H
|
|
#define JETSON_CMD_H
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
|
|
/*
|
|
* Jetson→STM32 command protocol over USB CDC (bidirectional, same /dev/ttyACM0)
|
|
*
|
|
* Commands (newline-terminated ASCII, sent by Jetson):
|
|
* H\n — heartbeat (every 200ms). Must arrive within 500ms or
|
|
* jetson_cmd_is_active() returns false → steer reverts to 0.
|
|
* C<spd>,<str>\n — drive command: speed -1000..+1000, steer -1000..+1000.
|
|
* Also refreshes the heartbeat timer.
|
|
*
|
|
* Speed→setpoint:
|
|
* Speed is converted to a setpoint offset (degrees) before calling balance_update().
|
|
* Positive speed → forward tilt → robot moves forward.
|
|
* Max offset is ±JETSON_SPEED_MAX_DEG (see below).
|
|
*
|
|
* Steer:
|
|
* Passed directly to motor_driver_update() as steer_cmd.
|
|
* Motor driver ramps and clamps with balance headroom (see motor_driver.h).
|
|
*
|
|
* Integration pattern in main.c (after the cdc_cmd_ready block):
|
|
*
|
|
* // Process buffered C command (parsed here, not in ISR)
|
|
* if (jetson_cmd_ready) { jetson_cmd_ready = 0; jetson_cmd_process(); }
|
|
*
|
|
* // Apply setpoint offset and steer when active
|
|
* float base_sp = bal.setpoint;
|
|
* if (jetson_cmd_is_active(now)) bal.setpoint += jetson_cmd_sp_offset();
|
|
* balance_update(&bal, &imu, dt);
|
|
* bal.setpoint = base_sp;
|
|
*
|
|
* // Steer injection in 50Hz ESC block
|
|
* int16_t jsteer = jetson_cmd_is_active(now) ? jetson_cmd_steer() : 0;
|
|
* motor_driver_update(&motors, bal.motor_cmd, jsteer, now);
|
|
*/
|
|
|
|
/* Heartbeat timeout: if no H or C within this window, commands deactivate */
|
|
#define JETSON_HB_TIMEOUT_MS 500
|
|
|
|
/* Max setpoint offset from Jetson speed command (speed=1000 → +N degrees tilt) */
|
|
#define JETSON_SPEED_MAX_DEG 4.0f /* ±4° → enough for ~0.5 m/s */
|
|
|
|
/*
|
|
* jetson_cmd_process()
|
|
* Call from main loop (NOT ISR) when jetson_cmd_ready is set.
|
|
* Parses jetson_cmd_buf (the C<spd>,<str> frame) with sscanf.
|
|
*/
|
|
void jetson_cmd_process(void);
|
|
|
|
/*
|
|
* jetson_cmd_is_active(now)
|
|
* Returns true if a heartbeat (H or C command) arrived within JETSON_HB_TIMEOUT_MS.
|
|
* If false, main loop should fall back to RC or zero steer.
|
|
*/
|
|
bool jetson_cmd_is_active(uint32_t now_ms);
|
|
|
|
/* Current steer command after latest C frame, clamped to ±1000 */
|
|
int16_t jetson_cmd_steer(void);
|
|
|
|
/* Setpoint offset (degrees) derived from latest speed command. */
|
|
float jetson_cmd_sp_offset(void);
|
|
|
|
/*
|
|
* Externals — declared here, defined in usbd_cdc_if.c alongside the other
|
|
* CDC volatile flags (cdc_streaming, cdc_arm_request, etc.).
|
|
* Main loop checks jetson_cmd_ready; ISR sets it.
|
|
*/
|
|
extern volatile uint8_t jetson_cmd_ready; /* set by ISR on C frame */
|
|
extern volatile char jetson_cmd_buf[32]; /* C<spd>,<str>\0 from ISR */
|
|
extern volatile uint32_t jetson_hb_tick; /* HAL_GetTick() of last H or C */
|
|
|
|
#endif /* JETSON_CMD_H */
|