sl-firmware
36643dd652
- 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 <noreply@anthropic.com>
128 lines
3.6 KiB
C
128 lines
3.6 KiB
C
#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 */
|
|
}
|