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>
98 lines
2.8 KiB
C
98 lines
2.8 KiB
C
#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 */
|