GCC Code Coverage Report

Directory: ../../
Coverage: low: ≥ 0% medium: ≥ 75.0% high: ≥ 90.0%
Coverage Exec / Excl / Total
Lines: 56.1% 32 / 0 / 57
Functions: 57.1% 4 / 0 / 7
Branches: 33.3% 6 / 0 / 18
driver/servo/servo.c
Line Branch Exec Source
1 /*******************************************************************************
2 *
3 * FILE:
4 * servo.c
5 *
6 * DESCRIPTION:
7 * Contains API functions to get access to the servo motors driver
8 *
9 * COPYRIGHT:
10 * Copyright (c) 2025 Sun Devil Rocketry.
11 * All rights reserved.
12 *
13 * This software is licensed under terms that can be found in the LICENSE
14 * file in the root directory of this software component.
15 * If no LICENSE file comes with this software, it is covered under the
16 * BSD-3-Clause.
17 *
18 * https://opensource.org/license/bsd-3-clause
19 *
20 *******************************************************************************/
21
22 /*------------------------------------------------------------------------------
23 Standard Includes
24 ------------------------------------------------------------------------------*/
25
26
27 /*------------------------------------------------------------------------------
28 Project Includes
29 ------------------------------------------------------------------------------*/
30 #include "main.h"
31 #include "sdr_pin_defines_A0002.h"
32 #include "servo.h"
33 #include "led.h"
34 #include "init.h"
35 #include "usb.h"
36
37 /*------------------------------------------------------------------------------
38 Literal Constants
39 ------------------------------------------------------------------------------*/
40 #define SER_PER 0.55555
41
42 /*------------------------------------------------------------------------------
43 Global Variables
44 ------------------------------------------------------------------------------*/
45 #ifdef CANARD
46 extern SERVO_PRESET servo_preset;
47 #endif
48
49 #ifdef APPA
50 extern PRESET_DATA preset_data;
51 #endif
52
53 /*------------------------------------------------------------------------------
54 Procedures
55 ------------------------------------------------------------------------------*/
56
57 /*******************************************************************************
58 * *
59 * PROCEDURE: *
60 * servo_init *
61 * *
62 * DESCRIPTION: *
63 * Initialize/Reset servo *
64 * *
65 *******************************************************************************/
66 2 SERVO_STATUS servo_init
67 (
68 void
69 )
70 {
71 /* GPIO Initialization */
72 2 HAL_GPIO_WritePin(MOTOR1_EN_PORT, MOTOR1_EN, GPIO_PIN_SET);
73 2 HAL_GPIO_WritePin(MOTOR2_EN_PORT, MOTOR2_EN, GPIO_PIN_SET);
74 2 HAL_GPIO_WritePin(MOTOR3_EN_PORT, MOTOR3_EN, GPIO_PIN_SET);
75 2 HAL_GPIO_WritePin(MOTOR4_EN_PORT, MOTOR4_EN, GPIO_PIN_SET);
76
77 /* Timer intialization */
78 2 HAL_StatusTypeDef hal_status1, hal_status2, hal_status3, hal_status4;
79
80 2 hal_status1 = HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);
81 2 hal_status2 = HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
82 2 hal_status3 = HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
83 2 hal_status4 = HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
84
85 2 if ( hal_status1 == HAL_OK &&
86 hal_status2 == HAL_OK &&
87
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 hal_status3 == HAL_OK &&
88 hal_status4 == HAL_OK )
89 {
90 return SERVO_OK;
91 }
92 else
93 {
94 ✗ return SERVO_FAIL;
95 }
96
97 } /* servo_init */
98
99
100 /*******************************************************************************
101 * *
102 * PROCEDURE: *
103 * motor_drive *
104 * *
105 * DESCRIPTION: *
106 * Drives the specified servo motor to the desired angle (0-180). *
107 * *
108 *******************************************************************************/
109 8 void motor_drive
110 (
111 SERVO_ID servo,
112 uint8_t angle
113 )
114 {
115 8 uint8_t pulse = angle_to_pulse(motor_snap_to_bound(angle, 180, 0));
116
117
4/5
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 8 switch (servo)
118 {
119 2 case SERVO_1:
120 2 htim3.Instance->CCR4 = pulse;
121 2 break;
122 2 case SERVO_2:
123 2 htim3.Instance->CCR3 = pulse;
124 2 break;
125 2 case SERVO_3:
126 2 htim3.Instance->CCR1 = pulse;
127 2 break;
128 2 case SERVO_4:
129 2 htim2.Instance->CCR1 = pulse;
130 2 break;
131 default:
132 /* Optionally: handle error */
133 break;
134 }
135
136 8 } /* motor_drive */
137
138
139 /*******************************************************************************
140 * *
141 * PROCEDURE: *
142 * servo_reset *
143 * *
144 * DESCRIPTION: *
145 * Resets all servos to their preset positions *
146 * *
147 *******************************************************************************/
148 ✗ void servo_reset
149 (
150 void
151 )
152 {
153 #ifdef CANARD
154 motor_drive(SERVO_1, servo_preset.rp_servo1);
155 motor_drive(SERVO_2, servo_preset.rp_servo2);
156 motor_drive(SERVO_3, servo_preset.rp_servo3);
157 motor_drive(SERVO_4, servo_preset.rp_servo4);
158 #endif
159
160 #ifdef APPA
161 motor_drive(SERVO_1, preset_data.servo_preset.rp_servo1);
162 motor_drive(SERVO_2, preset_data.servo_preset.rp_servo2);
163 motor_drive(SERVO_3, preset_data.servo_preset.rp_servo3);
164 motor_drive(SERVO_4, preset_data.servo_preset.rp_servo4);
165 #endif
166
167 ✗ } /* servo_reset */
168
169
170 /*******************************************************************************
171 * *
172 * PROCEDURE: *
173 * motors_drive *
174 * *
175 * DESCRIPTION: *
176 * A complete function that drives all servos in this board *
177 * Uses motor_drive for each servo. *
178 * *
179 *******************************************************************************/
180 ✗ void motors_drive
181 (
182 uint8_t angle
183 )
184 {
185 ✗ motor_drive(SERVO_1, angle);
186 ✗ motor_drive(SERVO_2, angle);
187 ✗ motor_drive(SERVO_3, angle);
188 ✗ motor_drive(SERVO_4, angle);
189
190 ✗ } /* motors_drive */
191
192
193 /*******************************************************************************
194 * *
195 * PROCEDURE: *
196 * servo_cmd_execute *
197 * *
198 * DESCRIPTION: *
199 * Executes a servo command given from the terminal. *
200 * *
201 *******************************************************************************/
202 ✗ SERVO_STATUS servo_cmd_execute
203 (
204 uint8_t subcommand
205 )
206 {
207 ✗ USB_STATUS usb_status;
208
209 ✗ switch ( subcommand )
210 {
211 ✗ case SERVO_SWEEP:
212 {
213 ✗ uint8_t degree;
214 ✗ usb_status = usb_receive(&degree, sizeof(uint8_t), 1000);
215 ✗ motors_drive(degree);
216 ✗ if (usb_status == USB_OK)
217 {
218 return SERVO_OK;
219 }
220 else
221 {
222 ✗ led_set_color( LED_YELLOW );
223 ✗ HAL_Delay( 5000 );
224 ✗ return SERVO_FAIL;
225 }
226 }
227 ✗ case SERVO_RESET:
228 {
229 ✗ servo_reset();
230 ✗ return SERVO_OK;
231 }
232 default:
233 {
234 return SERVO_FAIL;
235 }
236 }
237
238 } /* servo_cmd_execute */
239
240
241 /*******************************************************************************
242 * *
243 * PROCEDURE: *
244 * angle_to_pulse *
245 * *
246 * DESCRIPTION: *
247 * Convert turn angle degree into PWM pulse *
248 * *
249 *******************************************************************************/
250 8 uint8_t angle_to_pulse
251 (
252 uint8_t angle
253 )
254 {
255 8 return 25 + (angle*SER_PER);
256
257 } /* angle_to_pulse */
258
259
260 /*******************************************************************************
261 * *
262 * PROCEDURE: *
263 * motor_snap_to_bound *
264 * *
265 * DESCRIPTION: *
266 * Given an angle, upper bound, and lower bound, snap the angle to upper *
267 * or lower if the angle is outside of the bounds. *
268 * *
269 *******************************************************************************/
270 8 uint8_t motor_snap_to_bound
271 (
272 uint8_t angle,
273 uint8_t upper,
274 uint8_t lower
275 )
276 {
277
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
 8 if (angle >= lower && angle <= upper)
278 {
279 return angle;
280 }
281 ✗ else if ( angle > upper && angle <= ( upper + ( ( 255 - upper ) / 2 ) ) )
282 {
283 return upper;
284 }
285 else
286 {
287 ✗ return lower;
288 }
289
290 } /* motor_snap_to_bound */
291