seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity
saltylab-firmware/src/servo.c
sl-firmware 532edb835b feat(firmware): Pan-tilt servo driver for camera head (Issue #206) 
Implements TIM4 PWM driver for 2-servo camera mount with:
- 50 Hz PWM frequency (standard servo control)
- CH1 (PB6) pan servo, CH2 (PB7) tilt servo
- 0-180° angle range → 500-2500 µs pulse width mapping
- Non-blocking servo_set_angle() for immediate positioning
- servo_sweep() for smooth pan-tilt animation (linear interpolation)
- Independent sweep control per servo (pan and tilt move simultaneously)
- 15 comprehensive unit tests covering all scenarios

Integration:
- servo_init() called at startup after power_mgmt_init()
- servo_tick(now_ms) called every 1ms in main loop
- Ready for camera/gimbal control automation

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-02 11:44:56 -05:00

243 lines
7.7 KiB
C
Raw Blame History

#include "servo.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>
/* ================================================================
* Servo PWM Protocol
* ================================================================
* TIM4 at 50 Hz (20 ms period)
* APB1 clock: 54 MHz
* Prescaler: 53 (54 MHz / 54 = 1 MHz)
* ARR: 19999 (1 MHz / 20000 = 50 Hz)
* CCR: 500-2500 (0.5-2.5 ms out of 20 ms)
*
* Servo pulse mapping:
* 500 µs → 0° (full left/down)
* 1500 µs → 90° (center)
* 2500 µs → 180° (full right/up)
*/
#define SERVO_PWM_FREQ 50u /* 50 Hz */
#define SERVO_PERIOD_MS 20u /* 20 ms = 1/50 Hz */
#define SERVO_CLOCK_HZ 1000000u /* 1 MHz timer clock */
#define SERVO_PRESCALER 53u /* APB1 54 MHz / 54 = 1 MHz */
#define SERVO_ARR 19999u /* 1 MHz / 20000 = 50 Hz */
typedef struct {
uint16_t current_angle_deg[SERVO_COUNT];
uint16_t target_angle_deg[SERVO_COUNT];
uint16_t pulse_us[SERVO_COUNT];
/* Sweep state */
uint32_t sweep_start_ms[SERVO_COUNT];
uint32_t sweep_duration_ms[SERVO_COUNT];
uint16_t sweep_start_deg[SERVO_COUNT];
uint16_t sweep_end_deg[SERVO_COUNT];
bool is_sweeping[SERVO_COUNT];
} ServoState;
static ServoState s_servo = {0};
static TIM_HandleTypeDef s_tim_handle = {0};
/* ================================================================
* Helper functions
* ================================================================
*/
static uint16_t angle_to_pulse_us(uint16_t degrees)
{
/* Linear interpolation: 0° → 500µs, 180° → 2500µs */
if (degrees > 180) degrees = 180;
uint32_t pulse = SERVO_MIN_US + (uint32_t)degrees * (SERVO_MAX_US - SERVO_MIN_US) / 180;
return (uint16_t)pulse;
}
static uint16_t pulse_us_to_angle(uint16_t pulse_us)
{
/* Inverse mapping: 500µs → 0°, 2500µs → 180° */
if (pulse_us < SERVO_MIN_US) pulse_us = SERVO_MIN_US;
if (pulse_us > SERVO_MAX_US) pulse_us = SERVO_MAX_US;
uint32_t angle = (uint32_t)(pulse_us - SERVO_MIN_US) * 180 / (SERVO_MAX_US - SERVO_MIN_US);
return (uint16_t)angle;
}
static void update_pwm(ServoChannel channel)
{
/* Convert pulse width (500-2500 µs) to CCR value */
/* At 1 MHz timer clock: 1 µs = 1 count */
uint32_t ccr_value = s_servo.pulse_us[channel];
if (channel == SERVO_PAN) {
__HAL_TIM_SET_COMPARE(&s_tim_handle, SERVO_PAN_CHANNEL, ccr_value);
} else {
__HAL_TIM_SET_COMPARE(&s_tim_handle, SERVO_TILT_CHANNEL, ccr_value);
}
}
/* ================================================================
* Public API
* ================================================================
*/
void servo_init(void)
{
/* Initialize state */
memset(&s_servo, 0, sizeof(s_servo));
/* Center both servos at 90° */
for (int i = 0; i < SERVO_COUNT; i++) {
s_servo.current_angle_deg[i] = 90;
s_servo.target_angle_deg[i] = 90;
s_servo.pulse_us[i] = SERVO_CENTER_US;
}
/* Configure GPIO PB6 (CH1) and PB7 (CH2) as TIM4 PWM */
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_InitTypeDef gpio_init = {0};
gpio_init.Mode = GPIO_MODE_AF_PP;
gpio_init.Pull = GPIO_NOPULL;
gpio_init.Speed = GPIO_SPEED_FREQ_HIGH;
gpio_init.Alternate = SERVO_AF;
/* Configure PB6 (pan) */
gpio_init.Pin = SERVO_PAN_PIN;
HAL_GPIO_Init(SERVO_PAN_PORT, &gpio_init);
/* Configure PB7 (tilt) */
gpio_init.Pin = SERVO_TILT_PIN;
HAL_GPIO_Init(SERVO_TILT_PORT, &gpio_init);
/* Configure TIM4: 50 Hz PWM */
__HAL_RCC_TIM4_CLK_ENABLE();
s_tim_handle.Instance = SERVO_TIM;
s_tim_handle.Init.Prescaler = SERVO_PRESCALER;
s_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
s_tim_handle.Init.Period = SERVO_ARR;
s_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
s_tim_handle.Init.RepetitionCounter = 0;
HAL_TIM_PWM_Init(&s_tim_handle);
/* Configure TIM4_CH1 (pan) for PWM */
TIM_OC_InitTypeDef oc_init = {0};
oc_init.OCMode = TIM_OCMODE_PWM1;
oc_init.Pulse = SERVO_CENTER_US;
oc_init.OCPolarity = TIM_OCPOLARITY_HIGH;
oc_init.OCFastMode = TIM_OCFAST_DISABLE;
HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, SERVO_PAN_CHANNEL);
HAL_TIM_PWM_Start(&s_tim_handle, SERVO_PAN_CHANNEL);
/* Configure TIM4_CH2 (tilt) for PWM */
oc_init.Pulse = SERVO_CENTER_US;
HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, SERVO_TILT_CHANNEL);
HAL_TIM_PWM_Start(&s_tim_handle, SERVO_TILT_CHANNEL);
}
void servo_set_angle(ServoChannel channel, uint16_t degrees)
{
if (channel >= SERVO_COUNT) return;
if (degrees > 180) degrees = 180;
s_servo.current_angle_deg[channel] = degrees;
s_servo.target_angle_deg[channel] = degrees;
s_servo.pulse_us[channel] = angle_to_pulse_us(degrees);
/* Stop any sweep in progress */
s_servo.is_sweeping[channel] = false;
/* Update PWM immediately */
update_pwm(channel);
}
uint16_t servo_get_angle(ServoChannel channel)
{
if (channel >= SERVO_COUNT) return 0;
return s_servo.current_angle_deg[channel];
}
void servo_set_pulse_us(ServoChannel channel, uint16_t pulse_us)
{
if (channel >= SERVO_COUNT) return;
if (pulse_us < SERVO_MIN_US) pulse_us = SERVO_MIN_US;
if (pulse_us > SERVO_MAX_US) pulse_us = SERVO_MAX_US;
s_servo.pulse_us[channel] = pulse_us;
s_servo.current_angle_deg[channel] = pulse_us_to_angle(pulse_us);
s_servo.target_angle_deg[channel] = s_servo.current_angle_deg[channel];
/* Stop any sweep in progress */
s_servo.is_sweeping[channel] = false;
/* Update PWM immediately */
update_pwm(channel);
}
void servo_sweep(ServoChannel channel, uint16_t start_deg, uint16_t end_deg, uint32_t duration_ms)
{
if (channel >= SERVO_COUNT) return;
if (duration_ms == 0) return;
if (start_deg > 180) start_deg = 180;
if (end_deg > 180) end_deg = 180;
s_servo.sweep_start_deg[channel] = start_deg;
s_servo.sweep_end_deg[channel] = end_deg;
s_servo.sweep_duration_ms[channel] = duration_ms;
s_servo.sweep_start_ms[channel] = 0; /* Will be set on first tick */
s_servo.is_sweeping[channel] = true;
}
void servo_tick(uint32_t now_ms)
{
for (int ch = 0; ch < SERVO_COUNT; ch++) {
if (!s_servo.is_sweeping[ch]) continue;
/* Initialize start time on first call */
if (s_servo.sweep_start_ms[ch] == 0) {
s_servo.sweep_start_ms[ch] = now_ms;
}
uint32_t elapsed = now_ms - s_servo.sweep_start_ms[ch];
uint32_t duration = s_servo.sweep_duration_ms[ch];
if (elapsed >= duration) {
/* Sweep complete */
s_servo.is_sweeping[ch] = false;
s_servo.current_angle_deg[ch] = s_servo.sweep_end_deg[ch];
s_servo.pulse_us[ch] = angle_to_pulse_us(s_servo.sweep_end_deg[ch]);
} else {
/* Linear interpolation */
int16_t start = (int16_t)s_servo.sweep_start_deg[ch];
int16_t end = (int16_t)s_servo.sweep_end_deg[ch];
int32_t delta = end - start;
/* angle = start + (delta * elapsed / duration) */
int32_t angle_i32 = start + (delta * (int32_t)elapsed / (int32_t)duration);
s_servo.current_angle_deg[ch] = (uint16_t)angle_i32;
s_servo.pulse_us[ch] = angle_to_pulse_us(s_servo.current_angle_deg[ch]);
}
/* Update PWM */
update_pwm((ServoChannel)ch);
}
}
bool servo_is_sweeping(void)
{
for (int i = 0; i < SERVO_COUNT; i++) {
if (s_servo.is_sweeping[i]) return true;
}
return false;
}
void servo_stop_sweep(ServoChannel channel)
{
if (channel >= SERVO_COUNT) return;
s_servo.is_sweeping[channel] = false;
}
Reference in New Issue View Git Blame Copy Permalink