From 36643dd652ec9b0b5faf8471edfd134fbd11a637 Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Sat, 14 Mar 2026 10:37:39 -0400 Subject: [PATCH] feat: Pan/tilt gimbal servo driver for ST3215 bus servos (Issue #547) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - servo_bus.c/h: half-duplex USART3 driver for Feetech ST3215 servos at 1 Mbps; blocking TX/RX with CRC checksum; read/write position, torque enable, speed; deg<->raw conversion (center=2048, 4096 counts/360°) - gimbal.c/h: gimbal_t controller; 50 Hz feedback poll alternating pan/tilt at 25 Hz each; clamps to ±GIMBAL_PAN/TILT_LIMIT_DEG soft limits - jlink.c: dispatch JLINK_CMD_GIMBAL_POS (0x0B, 6-byte payload int16+int16+ uint16); jlink_send_gimbal_state() for JLINK_TLM_GIMBAL_STATE (0x84) - main.c: servo_bus_init() + gimbal_init() on boot; gimbal_tick() in main loop; gimbal_updated flag handler; GIMBAL_STATE telemetry at 50 Hz - config.h: SERVO_BUS_UART/PORT/PIN/BAUD, GIMBAL_PAN/TILT_ID, GIMBAL_TLM_HZ, GIMBAL_PAN/TILT_LIMIT_DEG - jlink.h: CMD_GIMBAL_POS, TLM_GIMBAL_STATE, jlink_tlm_gimbal_state_t (10 B), gimbal_updated/pan_x10/tilt_x10/speed volatile fields in JLinkState Co-Authored-By: Claude Sonnet 4.6 --- include/config.h | 13 +++ include/gimbal.h | 72 +++++++++++++++++ include/jlink.h | 29 ++++++- include/servo_bus.h | 97 +++++++++++++++++++++++ src/gimbal.c | 127 +++++++++++++++++++++++++++++ src/jlink.c | 146 ++++++++++++++++++++++++++++++---- src/main.c | 34 ++++++++ src/servo_bus.c | 189 ++++++++++++++++++++++++++++++++++++++++++++ 8 files changed, 690 insertions(+), 17 deletions(-) create mode 100644 include/gimbal.h create mode 100644 include/servo_bus.h create mode 100644 src/gimbal.c create mode 100644 src/servo_bus.c diff --git a/include/config.h b/include/config.h index fa1e4e3..158b59f 100644 --- a/include/config.h +++ b/include/config.h @@ -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 diff --git a/include/gimbal.h b/include/gimbal.h new file mode 100644 index 0000000..65ed07e --- /dev/null +++ b/include/gimbal.h @@ -0,0 +1,72 @@ +#ifndef GIMBAL_H +#define GIMBAL_H + +#include +#include + +/* + * 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 */ diff --git a/include/jlink.h b/include/jlink.h index 6d0938f..c008754 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -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 */ diff --git a/include/servo_bus.h b/include/servo_bus.h new file mode 100644 index 0000000..5200c56 --- /dev/null +++ b/include/servo_bus.h @@ -0,0 +1,97 @@ +#ifndef SERVO_BUS_H +#define SERVO_BUS_H + +#include +#include + +/* + * 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 */ diff --git a/src/gimbal.c b/src/gimbal.c new file mode 100644 index 0000000..be3ddb9 --- /dev/null +++ b/src/gimbal.c @@ -0,0 +1,127 @@ +#include "gimbal.h" +#include "servo_bus.h" +#include "config.h" +#include + +/* + * 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 */ +} diff --git a/src/jlink.c b/src/jlink.c index 45f5334..afb87aa 100644 --- a/src/jlink.c +++ b/src/jlink.c @@ -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)); +} diff --git a/src/main.c b/src/main.c index e9de09d..4198b80 100644 --- a/src/main.c +++ b/src/main.c @@ -31,6 +31,8 @@ #include "coulomb_counter.h" #include "watchdog.h" #include "pid_flash.h" +#include "servo_bus.h" +#include "gimbal.h" #include #include #include @@ -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(>lm); } /* JLINK_TLM_POWER telemetry at PM_TLM_HZ (1 Hz) */ diff --git a/src/servo_bus.c b/src/servo_bus.c new file mode 100644 index 0000000..ab1f650 --- /dev/null +++ b/src/servo_bus.c @@ -0,0 +1,189 @@ +#include "servo_bus.h" +#include "config.h" +#include "stm32f7xx_hal.h" +#include + +/* + * 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); +}