Skip to content

Commit 814fa8d

Browse files
lew1101lew1101
committed
servo driver refactor
1 parent c4a24d4 commit 814fa8d

2 files changed

Lines changed: 88 additions & 76 deletions

File tree

‎embedded-software/firmware/ulysses-flight-controller/Core/Inc/motor_drivers/servo_driver.h‎

Lines changed: 18 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -9,15 +9,17 @@ typedef struct {
99

1010
/* Calibration parameters */
1111
uint16_t us_min;
12-
uint16_t us_mid;
1312
uint16_t us_max;
14-
15-
float deg_range;
16-
float mid_pt;
13+
float deg_range_max;
14+
15+
float deg_min;
16+
float deg_max;
17+
float deg_bias;
1718

1819
volatile uint32_t compare_val;
1920

2021
uint16_t us_last;
22+
bool reversed;
2123
bool enabled;
2224
} servo_t;
2325

@@ -33,32 +35,33 @@ typedef struct {
3335
*/
3436

3537
/* Servo 1 (Y-axis / pitch): TIM1 CH2 / PE11 */
36-
#define SERVO1_US_MIN 900
37-
#define SERVO1_US_MID 1125
38-
#define SERVO1_US_MAX 1450
38+
#define SERVO1_US_MIN 1000
39+
#define SERVO1_US_MAX 2000
40+
#define SERVO1_DEG_BIAS 3.0f // Calibrated to center gimbal at 0° when both servos are at 1500µs
41+
#define SERVO1_DEG_RANGE_MAX 120.0f
42+
#define SERVO1_REVERSED false
3943

4044
/* Servo 2 (X-axis / roll): TIM3 CH3 / PB0 */
41-
#define SERVO2_US_MIN 1575
42-
#define SERVO2_US_MID 1860
43-
#define SERVO2_US_MAX 2200
45+
#define SERVO2_US_MIN 1000
46+
#define SERVO2_US_MAX 2000
47+
#define SERVO2_DEG_BIAS -2.0f // Calibrated to center gimbal at 0° when both servos are at 1500µs
48+
#define SERVO2_DEG_RANGE_MAX 120.0f
49+
#define SERVO2_REVERSED false
4450

4551
/* ── Gimbal angular limits ───────────────────────────────────────────── */
46-
#define SERVO_GIMBAL_RANGE_DEG 60.0f /**< Full range: ±30 ° each side */
47-
#define SERVO_GIMBAL_HALF_RANGE_DEG 30.0f /**< Clamp limit applied to both axes */
52+
#define SERVO_GIMBAL_DEG_MIN -30.0f
53+
#define SERVO_GIMBAL_DEG_MAX 30.0f
4854

4955
typedef struct {
5056
servo_t servo1; /**< Y-axis (pitch) servo */
5157
servo_t servo2; /**< X-axis (roll) servo */
5258
} servo_pair_t;
5359

5460
void servo_init(servo_t *servo, const pwm_output_t *pwm);
55-
void servo_init_with_deg_range(servo_t *servo, const pwm_output_t *pwm, float deg_range, float mid_pt);
5661
void servo_enable(servo_t *servo, bool enable);
57-
void servo_set_deg_range(servo_t *servo, float deg_range, float mid_pt);
5862

5963
/* Task-level: store desired angle (called from controls task). */
6064
void set_servo_degree(servo_t *servo, float degree);
61-
void set_servo_pair_degrees(float s1, float s2);
6265

6366
/**
6467
* @brief Command gimbal angles by physical axis.

‎embedded-software/firmware/ulysses-flight-controller/Core/Src/motor_drivers/servo_driver.c‎

Lines changed: 70 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -3,14 +3,42 @@
33

44
#include <stddef.h>
55

6+
// Helper functions
67
static uint16_t degree_to_us(const servo_t *servo, float degree);
7-
static void servo_init_with_cal(servo_t *servo, const pwm_output_t *pwm,
8-
uint16_t us_min, uint16_t us_mid, uint16_t us_max);
8+
static void servo_calibrate(servo_t *servo);
9+
10+
// -------- Global state --------
911

10-
/* Written by task (set_servo_pair_degrees / set_gimbal_degrees); read by ISR (apply_servo_pair_degrees). */
11-
static servo_pair_t servos;
1212
static volatile bool g_servo_pair_ready = false;
1313

14+
static servo_pair_t servos = {
15+
servo1 = {
16+
.us_min = SERVO1_US_MIN,
17+
.us_max = SERVO1_US_MAX,
18+
19+
.deg_range_max = SERVO1_DEG_RANGE_MAX,
20+
.deg_min = SERVO_GIMBAL_DEG_MIN,
21+
.deg_max = SERVO_GIMBAL_DEG_MAX,
22+
.deg_bias = SERVO1_DEG_BIAS,
23+
24+
.reversed = SERVO1_REVERSED,
25+
.enabled = false
26+
},
27+
.servo2 = {
28+
.us_min = SERVO2_US_MIN,
29+
.us_max = SERVO2_US_MAX,
30+
31+
.deg_range_max = SERVO2_DEG_RANGE_MAX,
32+
.deg_min = SERVO_GIMBAL_DEG_MIN,
33+
.deg_max = SERVO_GIMBAL_DEG_MAX,
34+
.deg_bias = SERVO2_DEG_BIAS,
35+
36+
.reversed = SERVO2_REVERSED,
37+
.enabled = false
38+
}
39+
};
40+
41+
1442
/* ---- Task-level API ---------------------------------------------------- */
1543

1644
void set_servo_degree(servo_t *servo, float degree) {
@@ -24,23 +52,15 @@ void set_servo_degree(servo_t *servo, float degree) {
2452
servo->compare_val = pwm_clamp_ticks(&servo->pwm, ticks);
2553
}
2654

27-
void set_servo_pair_degrees(float degree1, float degree2) {
28-
if (!g_servo_pair_ready) {
29-
return;
30-
}
31-
set_servo_degree(&servos.servo1, degree1);
32-
set_servo_degree(&servos.servo2, degree2);
33-
}
34-
3555
void set_gimbal_degrees(float x_deg, float y_deg) {
3656
if (!g_servo_pair_ready) {
3757
return;
3858
}
3959
/* servo1 = Y-axis (pitch); servo2 = X-axis (roll).
4060
* Negate both: positive command tilts toward positive axis per right-hand convention,
4161
* but servo mechanical direction is inverted relative to body frame. */
42-
set_servo_degree(&servos.servo1, -y_deg);
43-
set_servo_degree(&servos.servo2, -x_deg);
62+
set_servo_degree(&servos.servo1, y_deg);
63+
set_servo_degree(&servos.servo2, x_deg);
4464
}
4565

4666
/* ---- ISR-level API ----------------------------------------------------- */
@@ -55,25 +75,18 @@ void apply_servo_pair_degrees(void) {
5575

5676
/* ---- Init / enable ----------------------------------------------------- */
5777

58-
static void servo_init_with_cal(servo_t *servo, const pwm_output_t *pwm,
59-
uint16_t us_min, uint16_t us_mid, uint16_t us_max) {
60-
if (servo == NULL || pwm == NULL || pwm->htim == NULL) {
61-
return;
62-
}
78+
static void servo_calibrate(servo_t *servo)
79+
{
80+
if (servo == NULL) return;
6381

64-
servo->pwm = *pwm;
65-
servo->us_min = us_min;
66-
servo->us_mid = us_mid;
67-
servo->us_max = us_max;
68-
servo->deg_range = SERVO_GIMBAL_RANGE_DEG;
69-
servo->mid_pt = 0.0f; /* 0° command = center PWM (straight down) */
70-
servo->enabled = false;
71-
servo->us_last = us_mid;
82+
float home = degree_to_us(servo, deg_bias);
83+
84+
servo->us_last = home; /* Start at mid position. */
7285

7386
/* Start PWM output, set to mid position, then stop until enabled. */
7487
(void)HAL_TIM_PWM_Start(pwm->htim, pwm->channel);
7588
{
76-
uint32_t ticks = pwm_us_to_ticks(&servo->pwm, us_mid);
89+
uint32_t ticks = pwm_us_to_ticks(&servo->pwm, home);
7790
ticks = pwm_clamp_ticks(&servo->pwm, ticks);
7891
servo->compare_val = ticks;
7992
pwm_set_compare(&servo->pwm, ticks);
@@ -82,12 +95,24 @@ static void servo_init_with_cal(servo_t *servo, const pwm_output_t *pwm,
8295
}
8396

8497
void servo_init(servo_t *servo, const pwm_output_t *pwm) {
85-
servo_init_with_cal(servo, pwm, SERVO1_US_MIN, SERVO1_US_MID, SERVO1_US_MAX);
86-
}
98+
if (servo == NULL || pwm == NULL) {
99+
return;
100+
}
101+
102+
servo->pwm = *pwm;
103+
104+
servo->us_min = SERVO1_US_MIN;
105+
servo->us_max = SERVO1_US_MAX;
106+
servo->deg_range_max = SERVO1_DEG_RANGE_MAX;
107+
108+
servo->deg_min = SERVO_GIMBAL_DEG_MIN;
109+
servo->deg_max = SERVO_GIMBAL_DEG_MAX;
110+
servo->deg_bias = SERVO1_DEG_BIAS;
111+
servo->reversed = SERVO1_REVERSED;
87112

88-
void servo_init_with_deg_range(servo_t *servo, const pwm_output_t *pwm, float deg_range, float mid_pt) {
89-
servo_init(servo, pwm);
90-
servo_set_deg_range(servo, deg_range, mid_pt);
113+
servo->enabled = false;
114+
115+
servo_calibrate(servo);
91116
}
92117

93118
void servo_enable(servo_t *servo, bool enable) {
@@ -98,33 +123,27 @@ void servo_enable(servo_t *servo, bool enable) {
98123
servo->enabled = enable;
99124

100125
if (enable) {
101-
servo->us_last = clamp_u16(servo->us_last, servo->us_min, servo->us_max);
102126
uint32_t ticks = pwm_us_to_ticks(&servo->pwm, servo->us_last);
103127
ticks = pwm_clamp_ticks(&servo->pwm, ticks);
104128
pwm_set_compare(&servo->pwm, ticks);
105129
(void)HAL_TIM_PWM_Start(servo->pwm.htim, servo->pwm.channel);
106130
} else {
107-
uint32_t ticks = pwm_us_to_ticks(&servo->pwm, servo->us_mid);
131+
float home = degree_to_us(servo, deg_bias);
132+
133+
uint32_t ticks = pwm_us_to_ticks(&servo->pwm, home);
108134
ticks = pwm_clamp_ticks(&servo->pwm, ticks);
109135
pwm_set_compare(&servo->pwm, ticks);
110136
(void)HAL_TIM_PWM_Stop(servo->pwm.htim, servo->pwm.channel);
111137
}
112138
}
113139

114-
void servo_set_deg_range(servo_t *servo, float deg_range, float mid_pt) {
115-
if (servo == NULL) {
116-
return;
117-
}
118-
servo->deg_range = deg_range;
119-
servo->mid_pt = mid_pt;
120-
}
121-
122140
void servo_pair_init(const pwm_output_t *pwm1, const pwm_output_t *pwm2) {
123141
if (pwm1 == NULL || pwm2 == NULL) {
124142
return;
125143
}
126-
servo_init_with_cal(&servos.servo1, pwm1, SERVO1_US_MIN, SERVO1_US_MID, SERVO1_US_MAX);
127-
servo_init_with_cal(&servos.servo2, pwm2, SERVO2_US_MIN, SERVO2_US_MID, SERVO2_US_MAX);
144+
145+
servo_calibrate(&servos.servo1);
146+
servo_calibrate(&servos.servo2);
128147
g_servo_pair_ready = true;
129148
}
130149

@@ -139,21 +158,11 @@ void servo_pair_enable(bool enable) {
139158
/* ---- Static helpers ---------------------------------------------------- */
140159

141160
static uint16_t degree_to_us(const servo_t *servo, float degree) {
142-
float half_range = servo->deg_range * 0.5f;
143-
float min_deg = servo->mid_pt - half_range;
144-
float max_deg = servo->mid_pt + half_range;
145-
float d = clamp_float(degree, min_deg, max_deg);
146-
147-
float us_f;
148-
if (d < servo->mid_pt) {
149-
/* Negative side: [min_deg .. mid_pt] → [us_min .. us_mid] */
150-
float t = (half_range > 0.0f) ? (d - min_deg) / half_range : 0.0f;
151-
us_f = (float)servo->us_min + t * (float)(servo->us_mid - servo->us_min);
152-
} else {
153-
/* Positive side: [mid_pt .. max_deg] → [us_mid .. us_max] */
154-
float t = (half_range > 0.0f) ? (d - servo->mid_pt) / half_range : 0.0f;
155-
us_f = (float)servo->us_mid + t * (float)(servo->us_max - servo->us_mid);
156-
}
161+
float scale = (servo->us_max - servo->us_min) / servo->deg_range_max;
162+
163+
float d = (servo->reversed ? -degree : degree) + servo->deg_bias;
164+
float d_clamped = clamp_float(d, servo->deg_min, servo->deg_max);
157165

158-
return clamp_u16((uint16_t)(us_f + 0.5f), servo->us_min, servo->us_max);
166+
uint16_t us = servo->us_min + d_clamped * scale;
167+
return clamp_u16(us, servo->us_min, servo->us_max);
159168
}

0 commit comments

Comments
 (0)