seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

Merge pull request 'feat: Pan/tilt gimbal servo driver for ST3215 bus servos (Issue #547)' (#559) from sl-firmware/issue-547-gimbal-servo into main

Browse Source
This commit is contained in:
sl-jetson 2026-03-14 11:40:02 -04:00
commit 6409360428
8 changed files with 690 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
include/config.h
View File

@ -244,4 +244,17 @@
#define AUDIO_BUF_HALF 441u // DMA half-buffer: 20ms at 22050 Hz
#define AUDIO_VOLUME_DEFAULT 80u // default volume 0-100
// --- Gimbal Servo Bus (ST3215, USART3 half-duplex, Issue #547) ---
// Half-duplex single-wire on PB10 (USART3_TX, AF7) at 1 Mbps.
// USART3 is available: not assigned to any active subsystem.
#define SERVO_BUS_UART USART3
#define SERVO_BUS_PORT GPIOB
#define SERVO_BUS_PIN GPIO_PIN_10 // USART3_TX, AF7
#define SERVO_BUS_BAUD 1000000u // 1 Mbps (ST3215 default)
#define GIMBAL_PAN_ID 1u // ST3215 servo ID for pan
#define GIMBAL_TILT_ID 2u // ST3215 servo ID for tilt
#define GIMBAL_TLM_HZ 50u // position feedback rate (Hz)
#define GIMBAL_PAN_LIMIT_DEG 180.0f // pan soft limit (deg each side)
#define GIMBAL_TILT_LIMIT_DEG 90.0f // tilt soft limit (deg each side)
#endif // CONFIG_H

72
include/gimbal.h Normal file
View File

@ -0,0 +1,72 @@
#ifndef GIMBAL_H
#define GIMBAL_H
#include <stdint.h>
#include <stdbool.h>
/*
* gimbal.h — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547)
*
* Manages dual ST3215 serial bus servos:
* Pan servo: ID GIMBAL_PAN_ID (config.h, default 1)
* Tilt servo: ID GIMBAL_TILT_ID (config.h, default 2)
*
* Position units: degrees x10 (int16), matching JLink protocol convention.
* e.g. 900 = 90.0°, -450 = -45.0°
*
* Limits:
* Pan: -1800..+1800 (x10 deg) = -180..+180 deg
* Tilt: -900..+900 (x10 deg) = -90..+90 deg
*
* The gimbal_tick() function polls servo feedback at GIMBAL_TLM_HZ (50 Hz).
* Alternates reading pan position on even ticks, tilt on odd ticks — each
* servo polled at 25 Hz to keep bus utilization low.
*/
typedef struct {
/* Command state */
int16_t cmd_pan_x10; /* Commanded pan (deg x10) */
int16_t cmd_tilt_x10; /* Commanded tilt (deg x10) */
uint16_t cmd_speed; /* Servo bus speed (0=max, 1-4095) */
bool torque_enabled; /* True when torques are enabled */
/* Feedback state (updated at ~25 Hz per axis) */
int16_t fb_pan_x10; /* Measured pan (deg x10) */
int16_t fb_tilt_x10; /* Measured tilt (deg x10) */
uint16_t fb_pan_speed; /* Raw speed register, pan servo */
uint16_t fb_tilt_speed; /* Raw speed register, tilt servo */
/* Diagnostics */
uint32_t rx_ok; /* Successful position reads */
uint32_t rx_err; /* Failed position reads */
uint32_t _last_tick_ms; /* Internal: last tick timestamp */
uint8_t _poll_phase; /* Internal: alternates 0=pan 1=tilt */
} gimbal_t;
/*
* gimbal_init(g) — enable torque on both servos, center them.
* servo_bus_init() must be called first.
*/
void gimbal_init(gimbal_t *g);
/*
* gimbal_set_pos(g, pan_x10, tilt_x10, speed) — command a new pan/tilt
* position. pan_x10 and tilt_x10 are degrees×10, clamped to servo limits.
* speed: 0=max servo speed, 1-4095 = scaled.
*/
void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10,
uint16_t speed);
/*
* gimbal_torque(g, enable) — enable or disable torque on both servos.
*/
void gimbal_torque(gimbal_t *g, bool enable);
/*
* gimbal_tick(g, now_ms) — poll servo feedback at GIMBAL_TLM_HZ.
* Call every 1 ms from the main loop; function self-throttles.
*/
void gimbal_tick(gimbal_t *g, uint32_t now_ms);
#endif /* GIMBAL_H */

29
include/jlink.h
View File

@ -29,11 +29,13 @@
* 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)
* 0x0B GIMBAL_POS - int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547)
*
* 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)
* 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
* 0x84 GIMBAL_STATE - jlink_tlm_gimbal_state_t (10 bytes, Issue #547)
*
* Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
@ -59,12 +61,14 @@
#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) */
#define JLINK_CMD_GIMBAL_POS 0x0Bu /* int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) */
/* ---- Telemetry IDs (STM32 to 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) */
#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes) Issue #531 */
#define JLINK_TLM_GIMBAL_STATE 0x84u /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
typedef struct __attribute__((packed)) {
@ -114,6 +118,17 @@ typedef struct __attribute__((packed)) {
uint8_t saved_ok; /* 1 = flash write verified, 0 = write failed */
} jlink_tlm_pid_result_t; /* 13 bytes */
/* ---- Telemetry GIMBAL_STATE payload (10 bytes, packed) Issue #547 ---- */
/* Sent at GIMBAL_TLM_HZ (50 Hz); reports measured pan/tilt and speed. */
typedef struct __attribute__((packed)) {
int16_t pan_x10; /* Measured pan angle, deg x10 */
int16_t tilt_x10; /* Measured tilt angle, deg x10 */
uint16_t pan_speed_raw; /* Present speed register, pan servo */
uint16_t tilt_speed_raw; /* Present speed register, tilt servo */
uint8_t torque_en; /* 1 = torque enabled */
uint8_t rx_err_pct; /* bus error rate 0-100% (rx_err*100/(rx_ok+rx_err)) */
} jlink_tlm_gimbal_state_t; /* 10 bytes */
/* ---- Volatile state (read from main loop) ---- */
typedef struct {
/* Drive command - updated on JLINK_CMD_DRIVE */
@ -140,6 +155,12 @@ typedef struct {
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;
/* Gimbal position command - set by JLINK_CMD_GIMBAL_POS (Issue #547) */
volatile uint8_t gimbal_updated; /* set by parser, cleared by main loop */
volatile int16_t gimbal_pan_x10; /* pan angle deg x10 */
volatile int16_t gimbal_tilt_x10; /* tilt angle deg x10 */
volatile uint16_t gimbal_speed; /* servo speed 0-4095 (0=max) */
} JLinkState;
extern volatile JLinkState jlink_state;
@ -190,4 +211,10 @@ void jlink_send_pid_result(const jlink_tlm_pid_result_t *result);
*/
void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt);
/*
* jlink_send_gimbal_state(state) - transmit JLINK_TLM_GIMBAL_STATE (0x84)
* frame (16 bytes) at GIMBAL_TLM_HZ (50 Hz). Issue #547.
*/
void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state);
#endif /* JLINK_H */

97
include/servo_bus.h Normal file
View File

@ -0,0 +1,97 @@
#ifndef SERVO_BUS_H
#define SERVO_BUS_H
#include <stdint.h>
#include <stdbool.h>
/*
* servo_bus.h — Feetech/ST3215 serial bus servo driver (Issue #547)
*
* Half-duplex single-wire UART protocol at 1 Mbps.
* Hardware: USART3 half-duplex on PB10 (USART3_TX, AF7).
* No separate RX pin — the TX line is bidirectional with HDSEL.
*
* Packet format (host to servo):
* [0xFF][0xFF][ID][LEN][INSTR][PARAMS...][CKSUM]
* CKSUM = ~(ID + LEN + INSTR + PARAMS) & 0xFF
*
* Response (servo to host):
* [0xFF][0xFF][ID][LEN][ERROR][DATA...][CKSUM]
*
* Key ST3215 registers:
* 0x28 Torque Enable (1=on, 0=off)
* 0x2A Goal Position L (0-4095)
* 0x2B Goal Position H
* 0x2E Moving Speed L (0=max, 1-4095)
* 0x2F Moving Speed H
* 0x38 Present Position L (0-4095)
* 0x39 Present Position H
* 0x3A Present Speed L (sign+magnitude: bit15=dir)
* 0x3B Present Speed H
*
* Position encoding: 0-4095 maps to 0-360 degrees.
* Center (180 deg) = 2048 raw.
*/
/* ST3215 register addresses */
#define SB_REG_TORQUE_EN 0x28u
#define SB_REG_GOAL_POS_L 0x2Au
#define SB_REG_MOVING_SPD_L 0x2Eu
#define SB_REG_PRES_POS_L 0x38u
#define SB_REG_PRES_SPD_L 0x3Au
/* ST3215 instructions */
#define SB_INSTR_PING 0x01u
#define SB_INSTR_READ 0x02u
#define SB_INSTR_WRITE 0x03u
/* Position encoding */
#define SB_POS_CENTER 2048u /* 180 deg */
#define SB_POS_MAX 4095u /* 360 deg */
#define SB_SPEED_MAX 4095u /* counts/sec (0 = max speed) */
/* Timeout for servo response (ms) */
#define SB_RX_TIMEOUT_MS 5u
/*
* servo_bus_init() - configure USART3 in half-duplex mode (PB10, AF7) at
* SB_BAUD (1 Mbps, 8N1). Call once at startup before gimbal_init().
*/
void servo_bus_init(void);
/*
* servo_bus_write_pos(id, raw_pos, speed) - write goal position and moving
* speed in a single WRITE DATA packet. raw_pos: 0-4095. speed: 0=max, 1-4095.
* Returns true on successful TX (no response expected on write).
*/
bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed);
/*
* servo_bus_write_torque(id, enable) - enable or disable servo torque.
*/
bool servo_bus_write_torque(uint8_t id, bool enable);
/*
* servo_bus_read_pos(id, raw_pos) - read present position.
* Returns true on success; raw_pos is 0-4095.
*/
bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos);
/*
* servo_bus_read_speed(id, speed) - read present speed.
* speed bit 15 is direction. Returns magnitude in lower 15 bits.
*/
bool servo_bus_read_speed(uint8_t id, uint16_t *speed);
/*
* servo_bus_deg_to_raw(deg) - convert degree (-180..+180) to raw position.
* Center (0 deg) = 2048. Clamps to 0-4095.
*/
uint16_t servo_bus_deg_to_raw(float deg);
/*
* servo_bus_raw_to_deg(raw) - convert raw position (0-4095) to degree (-180..+180).
*/
float servo_bus_raw_to_deg(uint16_t raw);
#endif /* SERVO_BUS_H */

127
src/gimbal.c Normal file
View File

@ -0,0 +1,127 @@
#include "gimbal.h"
#include "servo_bus.h"
#include "config.h"
#include <stddef.h>
/*
* gimbal.c — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547)
*
* Tick rate: called every 1 ms from main loop; self-throttles to GIMBAL_TLM_HZ.
* Feedback polling alternates: pan on even ticks, tilt on odd ticks.
* This gives ~25 Hz per axis, keeping per-read latency under 2 ms total.
*
* Safety limits:
* Pan: -GIMBAL_PAN_LIMIT_DEG .. +GIMBAL_PAN_LIMIT_DEG (±180 deg)
* Tilt: -GIMBAL_TILT_LIMIT_DEG .. +GIMBAL_TILT_LIMIT_DEG (± 90 deg)
*/
#define TICK_PERIOD_MS (1000u / GIMBAL_TLM_HZ) /* 20 ms at 50 Hz */
/* Clamp int16 to [lo, hi] */
static int16_t _clamp16(int16_t v, int16_t lo, int16_t hi)
{
if (v < lo) return lo;
if (v > hi) return hi;
return v;
}
/* ---- gimbal_init() ---- */
void gimbal_init(gimbal_t *g)
{
g->cmd_pan_x10 = 0;
g->cmd_tilt_x10 = 0;
g->cmd_speed = 0; /* 0 = max speed */
g->fb_pan_x10 = 0;
g->fb_tilt_x10 = 0;
g->fb_pan_speed = 0;
g->fb_tilt_speed = 0;
g->rx_ok = 0;
g->rx_err = 0;
g->_last_tick_ms = 0;
g->_poll_phase = 0;
/* Enable torque and center both servos */
servo_bus_write_torque(GIMBAL_PAN_ID, true);
servo_bus_write_torque(GIMBAL_TILT_ID, true);
g->torque_enabled = true;
uint16_t center = servo_bus_deg_to_raw(0.0f);
servo_bus_write_pos(GIMBAL_PAN_ID, center, 0);
servo_bus_write_pos(GIMBAL_TILT_ID, center, 0);
}
/* ---- gimbal_set_pos() ---- */
void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10,
uint16_t speed)
{
/* Clamp to hardware limits */
pan_x10 = _clamp16(pan_x10,
-(int16_t)(GIMBAL_PAN_LIMIT_DEG * 10),
(int16_t)(GIMBAL_PAN_LIMIT_DEG * 10));
tilt_x10 = _clamp16(tilt_x10,
-(int16_t)(GIMBAL_TILT_LIMIT_DEG * 10),
(int16_t)(GIMBAL_TILT_LIMIT_DEG * 10));
if (speed > SB_SPEED_MAX) speed = SB_SPEED_MAX;
g->cmd_pan_x10 = pan_x10;
g->cmd_tilt_x10 = tilt_x10;
g->cmd_speed = speed;
float pan_deg = (float)pan_x10 / 10.0f;
float tilt_deg = (float)tilt_x10 / 10.0f;
servo_bus_write_pos(GIMBAL_PAN_ID,
servo_bus_deg_to_raw(pan_deg), speed);
servo_bus_write_pos(GIMBAL_TILT_ID,
servo_bus_deg_to_raw(tilt_deg), speed);
}
/* ---- gimbal_torque() ---- */
void gimbal_torque(gimbal_t *g, bool enable)
{
servo_bus_write_torque(GIMBAL_PAN_ID, enable);
servo_bus_write_torque(GIMBAL_TILT_ID, enable);
g->torque_enabled = enable;
}
/* ---- gimbal_tick() ---- */
void gimbal_tick(gimbal_t *g, uint32_t now_ms)
{
if ((now_ms - g->_last_tick_ms) < TICK_PERIOD_MS) return;
g->_last_tick_ms = now_ms;
uint16_t raw = 0;
if (g->_poll_phase == 0u) {
/* Poll pan position */
if (servo_bus_read_pos(GIMBAL_PAN_ID, &raw)) {
g->fb_pan_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f);
g->rx_ok++;
} else {
g->rx_err++;
}
/* Also refresh pan speed */
uint16_t spd = 0;
(void)servo_bus_read_speed(GIMBAL_PAN_ID, &spd);
g->fb_pan_speed = spd;
} else {
/* Poll tilt position */
if (servo_bus_read_pos(GIMBAL_TILT_ID, &raw)) {
g->fb_tilt_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f);
g->rx_ok++;
} else {
g->rx_err++;
}
uint16_t spd = 0;
(void)servo_bus_read_speed(GIMBAL_TILT_ID, &spd);
g->fb_tilt_speed = spd;
}
g->_poll_phase ^= 1u; /* toggle 0 / 1 */
}

146
src/jlink.c
View File

@ -38,6 +38,14 @@ static void jlink_tx_locked(uint8_t *buf, uint16_t len)
/* ---- Volatile state ---- */
volatile JLinkState jlink_state;
/* ---- SCHED_SET static receive buffer (Issue #550) ---- */
static JLinkSchedSetBuf s_sched_set_buf;
JLinkSchedSetBuf *jlink_get_sched_set(void)
{
return &s_sched_set_buf;
}
/* ---- CRC16-XModem (poly 0x1021, init 0x0000) ---- */
static uint16_t crc16_xmodem(const uint8_t *data, uint16_t len)
{
@ -67,7 +75,7 @@ void jlink_init(void)
gpio.Alternate = GPIO_AF7_USART1;
HAL_GPIO_Init(GPIOB, &gpio);
/* DMA2 Stream2 Channel4 — USART1_RX circular */
/* DMA2 Stream2 Channel4 -- USART1_RX circular */
__HAL_RCC_DMA2_CLK_ENABLE();
s_dma_rx.Instance = DMA2_Stream2;
s_dma_rx.Init.Channel = DMA_CHANNEL_4;
@ -103,10 +111,11 @@ void jlink_init(void)
HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 7, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
/* Start circular DMA RX — never stops */
/* Start circular DMA RX -- never stops */
HAL_UART_Receive_DMA(&s_uart, s_rx_buf, JLINK_RX_BUF_LEN);
memset((void *)&jlink_state, 0, sizeof(jlink_state));
memset(&s_sched_set_buf, 0, sizeof(s_sched_set_buf));
s_rx_tail = 0;
}
@ -116,7 +125,7 @@ void USART1_IRQHandler(void)
/* Clear IDLE flag by reading SR then DR */
if (__HAL_UART_GET_FLAG(&s_uart, UART_FLAG_IDLE)) {
__HAL_UART_CLEAR_IDLEFLAG(&s_uart);
/* jlink_process() drains the buffer from main loop — no work here */
/* jlink_process() drains the buffer from main loop -- no work here */
}
HAL_UART_IRQHandler(&s_uart);
}
@ -142,7 +151,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
switch (cmd) {
case JLINK_CMD_HEARTBEAT:
/* Heartbeat only — no payload action needed */
/* Heartbeat only -- no payload action needed */
break;
case JLINK_CMD_DRIVE:
@ -150,7 +159,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
int16_t spd, str;
memcpy(&spd, payload, 2);
memcpy(&str, payload + 2, 2);
/* Clamp to ±1000 */
/* Clamp to +/-1000 */
if (spd > 1000) spd = 1000;
if (spd < -1000) spd = -1000;
if (str > 1000) str = 1000;
@ -174,7 +183,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
memcpy(&kp, payload, 4);
memcpy(&ki, payload + 4, 4);
memcpy(&kd, payload + 8, 4);
/* Sanity bounds — same as USB CDC PID handler in main.c */
/* Sanity bounds -- same as USB CDC PID handler in main.c */
if (kp >= 0.0f && kp <= 500.0f) jlink_state.pid_kp = kp;
if (ki >= 0.0f && ki <= 50.0f) jlink_state.pid_ki = ki;
if (kd >= 0.0f && kd <= 50.0f) jlink_state.pid_kd = kd;
@ -208,12 +217,62 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
jlink_state.pid_save_req = 1u;
break;
case JLINK_CMD_GIMBAL_POS:
/* Payload: int16 pan_x10, int16 tilt_x10, uint16 speed (6 bytes) Issue #547 */
if (plen == 6u) {
int16_t pan, tilt;
uint16_t spd;
memcpy(&pan, payload, 2);
memcpy(&tilt, payload + 2, 2);
memcpy(&spd, payload + 4, 2);
jlink_state.gimbal_pan_x10 = pan;
jlink_state.gimbal_tilt_x10 = tilt;
jlink_state.gimbal_speed = spd;
jlink_state.gimbal_updated = 1u;
}
break;
case JLINK_CMD_SCHED_GET:
/* Payload-less; main loop calls jlink_send_sched_telemetry() (Issue #550) */
jlink_state.sched_get_req = 1u;
break;
case JLINK_CMD_SCHED_SET:
/* Payload: uint8 num_bands + num_bands * sizeof(pid_sched_entry_t) bytes */
if (plen >= 1u) {
uint8_t nb = payload[0];
if (nb == 0u) nb = 1u;
if (nb > PID_SCHED_MAX_BANDS) nb = PID_SCHED_MAX_BANDS;
uint8_t expected = 1u + nb * (uint8_t)sizeof(pid_sched_entry_t);
if (plen >= expected) {
s_sched_set_buf.num_bands = nb;
memcpy(s_sched_set_buf.bands, payload + 1,
nb * sizeof(pid_sched_entry_t));
s_sched_set_buf.ready = 1u;
}
}
break;
case JLINK_CMD_SCHED_SAVE:
/* Payload: float kp, float ki, float kd (12 bytes) for single-PID record */
if (plen == 12u) {
float kp, ki, kd;
memcpy(&kp, payload, 4);
memcpy(&ki, payload + 4, 4);
memcpy(&kd, payload + 8, 4);
if (kp >= 0.0f && kp <= 500.0f) jlink_state.sched_save_kp = kp;
if (ki >= 0.0f && ki <= 50.0f) jlink_state.sched_save_ki = ki;
if (kd >= 0.0f && kd <= 50.0f) jlink_state.sched_save_kd = kd;
jlink_state.sched_save_req = 1u;
}
break;
default:
break;
}
}
/* ---- jlink_process() — call from main loop every tick ---- */
/* ---- jlink_process() -- call from main loop every tick ---- */
/*
* Parser state machine.
* Frame: [STX][LEN][CMD][PAYLOAD 0..LEN-1][CRC_hi][CRC_lo][ETX]
@ -222,7 +281,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
* Maximum payload = 253 - 1 = 252 bytes (LEN field is 1 byte, max 0xFF=255,
* but we cap at 64 for safety).
*/
#define JLINK_MAX_PAYLOAD 252u /* enlarged for AUDIO chunks (126 × int16) */
#define JLINK_MAX_PAYLOAD 252u /* enlarged for AUDIO chunks (126 x int16) */
typedef enum {
PS_WAIT_STX = 0,
@ -242,7 +301,7 @@ void jlink_process(void)
static uint8_t s_crc_hi = 0;
/* Compute how many bytes the DMA has written since last drain */
uint32_t head = JLINK_RX_BUF_LEN - __HAL_DMA_GET_COUNTER(&s_dma_rx);
uint32_t head = JLINK_RX_BUF_LEN - __HAL_DMA_GET_COUNTER(&s_dma_rx);
uint32_t bytes = (head - s_rx_tail) & (JLINK_RX_BUF_LEN - 1u);
for (uint32_t i = 0; i < bytes; i++) {
@ -256,7 +315,7 @@ void jlink_process(void)
case PS_WAIT_LEN:
if (b == 0u || b > JLINK_MAX_PAYLOAD + 1u) {
/* Invalid length — resync */
/* Invalid length -- resync */
s_state = PS_WAIT_STX;
} else {
s_len = b;
@ -276,12 +335,12 @@ void jlink_process(void)
break;
case PS_WAIT_CRC_LO: {
uint16_t rx_crc = ((uint16_t)s_crc_hi << 8) | b;
uint16_t rx_crc = ((uint16_t)s_crc_hi << 8) | b;
uint16_t calc_crc = crc16_xmodem(s_frame, s_len);
if (rx_crc == calc_crc)
s_state = PS_WAIT_ETX;
else
s_state = PS_WAIT_STX; /* CRC mismatch — drop */
s_state = PS_WAIT_STX; /* CRC mismatch -- drop */
break;
}
@ -304,7 +363,7 @@ void jlink_send_telemetry(const jlink_tlm_status_t *status)
* Frame: [STX][LEN][0x80][20 bytes STATUS][CRC_hi][CRC_lo][ETX]
* LEN = 1 (CMD) + 20 (payload) = 21
* Total frame length = 1+1+1+20+2+1 = 26 bytes
* At 921600 baud (10 bits/byte): 26×10/921600 ≈ 0.28ms — safe to block.
* At 921600 baud (10 bits/byte): 26x10/921600 ~0.28ms -- safe to block.
*/
static uint8_t frame[26];
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_status_t); /* 20 */
@ -329,7 +388,7 @@ void jlink_send_power_telemetry(const jlink_tlm_power_t *power)
/*
* Frame: [STX][LEN][0x81][11 bytes POWER][CRC_hi][CRC_lo][ETX]
* LEN = 1 (CMD) + 11 (payload) = 12; total = 17 bytes
* At 921600 baud: 17×10/921600 ≈ 0.18 ms — safe to block.
* At 921600 baud: 17x10/921600 ~0.18 ms -- safe to block.
*/
static uint8_t frame[17];
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_power_t); /* 11 */
@ -373,13 +432,13 @@ void jlink_send_pid_result(const jlink_tlm_pid_result_t *result)
jlink_tx_locked(frame_pid, sizeof(frame_pid));
}
/* ---- jlink_send_battery_telemetry() — Issue #533 ---- */
/* ---- jlink_send_battery_telemetry() -- Issue #533 ---- */
void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt)
{
/*
* Frame: [STX][LEN][0x82][10 bytes BATTERY][CRC_hi][CRC_lo][ETX]
* LEN = 1 (CMD) + 10 (payload) = 11; total = 16 bytes
* At 921600 baud: 16×10/921600 ≈ 0.17 ms — safe to block.
* At 921600 baud: 16x10/921600 ~0.17 ms -- safe to block.
*/
static uint8_t frame[16];
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_battery_t); /* 10 */
@ -397,3 +456,58 @@ void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt)
jlink_tx_locked(frame, sizeof(frame));
}
/* ---- jlink_send_gimbal_state() -- Issue #547 ---- */
void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state)
{
/*
* Frame: [STX][LEN][0x84][10 bytes GIMBAL_STATE][CRC_hi][CRC_lo][ETX]
* LEN = 1 (CMD) + 10 (payload) = 11; total = 16 bytes
* At 921600 baud: 16x10/921600 ~0.17 ms -- safe to block.
*/
static uint8_t frame[16];
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_gimbal_state_t); /* 10 */
const uint8_t len = 1u + plen; /* 11 */
frame[0] = JLINK_STX;
frame[1] = len;
frame[2] = JLINK_TLM_GIMBAL_STATE;
memcpy(&frame[3], state, plen);
uint16_t crc = crc16_xmodem(&frame[2], len);
frame[3 + plen] = (uint8_t)(crc >> 8);
frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
frame[3 + plen + 2] = JLINK_ETX;
jlink_tx_locked(frame, sizeof(frame));
}
/* ---- jlink_send_sched_telemetry() -- Issue #550 ---- */
void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm)
{
/*
* Frame: [STX][LEN][0x85][1+N*16 bytes SCHED][CRC_hi][CRC_lo][ETX]
* Actual payload = 1 (num_bands) + tlm->num_bands * 16 bytes.
* Max payload = 1 + 6*16 = 97; max frame = 103 bytes.
* At 921600 baud: 103x10/921600 ~1.1 ms -- safe to block.
*/
uint8_t nb = tlm->num_bands;
if (nb > PID_SCHED_MAX_BANDS) nb = PID_SCHED_MAX_BANDS;
uint8_t plen = 1u + nb * (uint8_t)sizeof(pid_sched_entry_t);
uint8_t len = 1u + plen; /* CMD + payload */
/* frame: STX + LEN + CMD + payload + CRC_hi + CRC_lo + ETX */
uint8_t frame[103];
frame[0] = JLINK_STX;
frame[1] = len;
frame[2] = JLINK_TLM_SCHED;
frame[3] = nb;
memcpy(&frame[4], tlm->bands, nb * sizeof(pid_sched_entry_t));
uint16_t crc = crc16_xmodem(&frame[2], len);
frame[3 + plen] = (uint8_t)(crc >> 8);
frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
frame[3 + plen + 2] = JLINK_ETX;
jlink_tx_locked(frame, (uint16_t)(3u + plen + 3u));
}

34
src/main.c
View File

@ -31,6 +31,8 @@
#include "coulomb_counter.h"
#include "watchdog.h"
#include "pid_flash.h"
#include "servo_bus.h"
#include "gimbal.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
@ -208,6 +210,13 @@ int main(void) {
/* Init servo pan-tilt driver for camera head (TIM4 PWM on PB6/PB7, Issue #206) */
servo_init();
/* Init ST3215 serial bus servo driver (USART3 half-duplex PB10, Issue #547) */
servo_bus_init();
/* Init pan/tilt gimbal (ST3215 servos, centers both axes on boot) */
gimbal_t gimbal;
gimbal_init(&gimbal);
/* Init HC-SR04 ultrasonic distance sensor (TIM1 input capture on PA1, Issue #243) */
ultrasonic_init();
@ -270,6 +279,9 @@ int main(void) {
/* Servo pan-tilt animation tick — updates smooth sweeps */
servo_tick(now);
/* Gimbal ST3215 feedback polling tick (self-throttles to GIMBAL_TLM_HZ) */
gimbal_tick(&gimbal, now);
/* Accumulate coulombs for battery state-of-charge estimation (Issue #325) */
battery_accumulate_coulombs();
@ -347,6 +359,15 @@ int main(void) {
jlink_state.sleep_req = 0u;
power_mgmt_request_sleep();
}
/* GIMBAL_POS: forward new pan/tilt command to gimbal (Issue #547) */
if (jlink_state.gimbal_updated) {
jlink_state.gimbal_updated = 0u;
gimbal_set_pos(&gimbal,
(int16_t)jlink_state.gimbal_pan_x10,
(int16_t)jlink_state.gimbal_tilt_x10,
jlink_state.gimbal_speed);
}
/* PID_SAVE: persist current gains to flash, reply with result (Issue #531).
* Only allowed while disarmed -- flash sector erase stalls CPU for ~1 s. */
if (jlink_state.pid_save_req && bal.state != BALANCE_ARMED) {
@ -558,6 +579,19 @@ int main(void) {
tlm.fw_minor = FW_MINOR;
tlm.fw_patch = FW_PATCH;
jlink_send_telemetry(&tlm);
/* Send gimbal state at same 50 Hz cadence (Issue #547) */
jlink_tlm_gimbal_state_t gtlm;
gtlm.pan_x10 = gimbal.fb_pan_x10;
gtlm.tilt_x10 = gimbal.fb_tilt_x10;
gtlm.pan_speed_raw = gimbal.fb_pan_speed;
gtlm.tilt_speed_raw = gimbal.fb_tilt_speed;
gtlm.torque_en = gimbal.torque_enabled ? 1u : 0u;
uint32_t rx_total = gimbal.rx_ok + gimbal.rx_err;
gtlm.rx_err_pct = (rx_total > 0u)
? (uint8_t)(gimbal.rx_err * 100u / rx_total)
: 0u;
jlink_send_gimbal_state(&gtlm);
}
/* JLINK_TLM_POWER telemetry at PM_TLM_HZ (1 Hz) */

189
src/servo_bus.c Normal file
View File

@ -0,0 +1,189 @@
#include "servo_bus.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>
/*
* servo_bus.c — Feetech/ST3215 serial bus servo driver (Issue #547)
*
* Half-duplex on USART3 PB10 (USART3_TX, AF7) at 1 Mbps.
* HDSEL bit (CR3.3) routes TX and RX to the TX pin.
* TX direction: TX active, RX gated.
* RX direction: TX idle, RX active — enabled after TC interrupt.
*
* All operations are blocking with short timeouts; the servo bus
* is polled at 50 Hz from the main loop (not from an ISR).
*/
static UART_HandleTypeDef s_uart;
/* ---- Init ---- */
void servo_bus_init(void)
{
/* GPIO: PB10 open-drain AF7 (USART3_TX) with external pull-up on bus */
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_InitTypeDef gpio = {0};
gpio.Pin = SERVO_BUS_PIN;
gpio.Mode = GPIO_MODE_AF_OD; /* open-drain for half-duplex bus */
gpio.Pull = GPIO_PULLUP;
gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
gpio.Alternate = GPIO_AF7_USART3;
HAL_GPIO_Init(SERVO_BUS_PORT, &gpio);
/* USART3 at SERVO_BUS_BAUD (1 Mbps), 8N1 */
__HAL_RCC_USART3_CLK_ENABLE();
s_uart.Instance = USART3;
s_uart.Init.BaudRate = SERVO_BUS_BAUD;
s_uart.Init.WordLength = UART_WORDLENGTH_8B;
s_uart.Init.StopBits = UART_STOPBITS_1;
s_uart.Init.Parity = UART_PARITY_NONE;
s_uart.Init.Mode = UART_MODE_TX_RX;
s_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
s_uart.Init.OverSampling = UART_OVERSAMPLING_8; /* required for 1 Mbps @ APB1 54 MHz */
HAL_UART_Init(&s_uart);
/* Enable half-duplex: HDSEL bit in CR3 routes TX/RX to TX pin */
SET_BIT(s_uart.Instance->CR3, USART_CR3_HDSEL);
}
/* ---- Checksum ---- */
static uint8_t _cksum(uint8_t id, uint8_t len, uint8_t instr,
const uint8_t *params, uint8_t nparams)
{
uint16_t sum = (uint16_t)id + len + instr;
for (uint8_t i = 0; i < nparams; i++) sum += params[i];
return (uint8_t)(~sum & 0xFFu);
}
/* ---- Transmit a raw packet ---- */
static bool _tx(uint8_t id, uint8_t instr, const uint8_t *params, uint8_t nparams)
{
uint8_t len = nparams + 2u; /* instr + params + cksum */
uint8_t buf[32];
uint8_t n = 0;
buf[n++] = 0xFFu;
buf[n++] = 0xFFu;
buf[n++] = id;
buf[n++] = len;
buf[n++] = instr;
for (uint8_t i = 0; i < nparams; i++) buf[n++] = params[i];
buf[n++] = _cksum(id, len, instr, params, nparams);
return HAL_UART_Transmit(&s_uart, buf, n, 10u) == HAL_OK;
}
/* ---- Receive response packet ---- */
/*
* Read one byte with timeout. In half-duplex the TX line is still driving
* after the last stop bit; we must wait ~1 bit-time before the servo begins
* its response. HAL_UART_Receive handles the timing via the timeout.
*/
static bool _rx(uint8_t id, uint8_t *data, uint8_t ndata)
{
uint8_t hdr[4]; /* 0xFF 0xFF ID LEN */
uint8_t tail[2]; /* ERROR CKSUM (or just CKSUM if ndata==0) */
/* Read header */
if (HAL_UART_Receive(&s_uart, hdr, 4u, SB_RX_TIMEOUT_MS) != HAL_OK)
return false;
/* Validate header */
if (hdr[0] != 0xFFu || hdr[1] != 0xFFu || hdr[2] != id)
return false;
uint8_t pkt_len = hdr[3]; /* ERROR + DATA... + CKSUM */
if (pkt_len < 2u || pkt_len > 20u) /* sanity */
return false;
/* Read payload: ERROR + DATA bytes + CKSUM */
uint8_t payload[20];
uint8_t payload_len = pkt_len; /* includes error byte and cksum */
if (HAL_UART_Receive(&s_uart, payload, payload_len, SB_RX_TIMEOUT_MS) != HAL_OK)
return false;
/* Verify checksum: ~(ID + LEN + ERROR + DATA) */
uint16_t sum = (uint16_t)id + pkt_len;
for (uint8_t i = 0; i < (uint8_t)(pkt_len - 1u); i++) sum += payload[i];
uint8_t expected_cksum = (uint8_t)(~sum & 0xFFu);
if (payload[pkt_len - 1u] != expected_cksum)
return false;
/* payload[0] = ERROR; payload[1..] = data */
if (payload[0] != 0u) return false; /* servo reported error */
uint8_t n_data_bytes = pkt_len - 2u; /* minus ERROR and CKSUM */
if (n_data_bytes < ndata) return false;
for (uint8_t i = 0; i < ndata; i++) data[i] = payload[1u + i];
return true;
}
/* ---- Public API ---- */
bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed)
{
if (raw_pos > SB_POS_MAX) raw_pos = SB_POS_MAX;
if (speed > SB_SPEED_MAX) speed = SB_SPEED_MAX;
/* Write 4 bytes starting at SB_REG_GOAL_POS_L:
* [addr][POS_L][POS_H][SPD_L][SPD_H] */
uint8_t params[5] = {
SB_REG_GOAL_POS_L,
(uint8_t)(raw_pos & 0xFFu),
(uint8_t)(raw_pos >> 8),
(uint8_t)(speed & 0xFFu),
(uint8_t)(speed >> 8),
};
return _tx(id, SB_INSTR_WRITE, params, 5u);
/* WRITE does not generate a status packet by default */
}
bool servo_bus_write_torque(uint8_t id, bool enable)
{
uint8_t params[2] = { SB_REG_TORQUE_EN, enable ? 1u : 0u };
return _tx(id, SB_INSTR_WRITE, params, 2u);
}
bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos)
{
/* READ instruction: [addr][count] */
uint8_t params[2] = { SB_REG_PRES_POS_L, 2u };
if (!_tx(id, SB_INSTR_READ, params, 2u)) return false;
uint8_t data[2];
if (!_rx(id, data, 2u)) return false;
*raw_pos = (uint16_t)data[0] | ((uint16_t)data[1] << 8);
return true;
}
bool servo_bus_read_speed(uint8_t id, uint16_t *speed)
{
uint8_t params[2] = { SB_REG_PRES_SPD_L, 2u };
if (!_tx(id, SB_INSTR_READ, params, 2u)) return false;
uint8_t data[2];
if (!_rx(id, data, 2u)) return false;
*speed = (uint16_t)data[0] | ((uint16_t)data[1] << 8);
return true;
}
uint16_t servo_bus_deg_to_raw(float deg)
{
/* Center = 0 deg = 2048 raw; 1 deg = 4096/360 raw */
int32_t raw = (int32_t)(SB_POS_CENTER + (int32_t)(deg * (4096.0f / 360.0f)));
if (raw < 0) raw = 0;
if (raw > (int32_t)SB_POS_MAX) raw = (int32_t)SB_POS_MAX;
return (uint16_t)raw;
}
float servo_bus_raw_to_deg(uint16_t raw)
{
return ((float)raw - (float)SB_POS_CENTER) * (360.0f / 4096.0f);
}