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>
56 lines
1.8 KiB
C
56 lines
1.8 KiB
C
#include "jetson_cmd.h"
|
|
#include <stdio.h>
|
|
|
|
/*
|
|
* Parsed drive state — updated by jetson_cmd_process() in the main loop.
|
|
* Raw fields are ints parsed from "C<speed>,<steer>\n".
|
|
*/
|
|
static volatile int16_t jcmd_speed = 0; /* -1000..+1000 */
|
|
static volatile int16_t jcmd_steer = 0; /* -1000..+1000 */
|
|
|
|
/* Clamp helper (avoids including math.h) */
|
|
static int16_t clamp16(int v, int lo, int hi) {
|
|
if (v < lo) return (int16_t)lo;
|
|
if (v > hi) return (int16_t)hi;
|
|
return (int16_t)v;
|
|
}
|
|
|
|
/*
|
|
* Called from main loop when jetson_cmd_ready is set.
|
|
* Parses jetson_cmd_buf — safe to use sscanf here (not in ISR).
|
|
* The ISR only copies bytes and sets the ready flag.
|
|
*/
|
|
void jetson_cmd_process(void) {
|
|
int speed = 0, steer = 0;
|
|
/* buf format: "C<speed>,<steer>" — skip leading 'C' */
|
|
if (sscanf((const char *)jetson_cmd_buf + 1, "%d,%d", &speed, &steer) == 2) {
|
|
jcmd_speed = clamp16(speed, -1000, 1000);
|
|
jcmd_steer = clamp16(steer, -1000, 1000);
|
|
}
|
|
/* If parse fails, keep previous values — don't zero-out mid-motion */
|
|
}
|
|
|
|
/*
|
|
* Returns true if the last heartbeat (H or C command) arrived within
|
|
* JETSON_HB_TIMEOUT_MS. jetson_hb_tick is updated in the ISR.
|
|
*/
|
|
bool jetson_cmd_is_active(uint32_t now_ms) {
|
|
/* jetson_hb_tick == 0 means we've never received any command */
|
|
if (jetson_hb_tick == 0) return false;
|
|
return (now_ms - jetson_hb_tick) < JETSON_HB_TIMEOUT_MS;
|
|
}
|
|
|
|
/* Steer command for motor_driver_update() */
|
|
int16_t jetson_cmd_steer(void) {
|
|
return jcmd_steer;
|
|
}
|
|
|
|
/*
|
|
* Convert speed command to balance setpoint offset (degrees).
|
|
* Positive speed → lean forward → robot moves forward.
|
|
* Scaled linearly: speed=1000 → +JETSON_SPEED_MAX_DEG degrees.
|
|
*/
|
|
float jetson_cmd_sp_offset(void) {
|
|
return ((float)jcmd_speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
|
|
}
|