GCC Code Coverage Report

Directory: ./
Coverage: low: ≥ 0% medium: ≥ 75.0% high: ≥ 90.0%
Coverage Exec / Excl / Total
Lines: 100.0% 236 / 0 / 236
Functions: 100.0% 21 / 0 / 21
Branches: 70.0% 35 / 0 / 50
mod/sensor/sensor.c
Line Branch Exec Source
1 /**
2 ******************************************************************************
3 * @file : sensor.c
4 * @brief : Contains functions to interface between SDEC terminal commands and SDR sensor APIs
5 ******************************************************************************
6 * @copyright
7 *
8 * Copyright (c) 2025 Sun Devil Rocketry.
9 * All rights reserved.
10 *
11 * This software is licensed under terms that can be found in the LICENSE
12 * file in the root directory of this software component.
13 * If no LICENSE file comes with this software, it is covered under the
14 * BSD-3-Clause.
15 *
16 * https://opensource.org/license/bsd-3-clause
17 *
18 ******************************************************************************
19 */
20
21
22 /*------------------------------------------------------------------------------
23 Standard Includes
24 ------------------------------------------------------------------------------*/
25 #include <string.h>
26 #include <stdbool.h>
27 #include <math.h>
28
29
30 /*------------------------------------------------------------------------------
31 MCU Pins
32 ------------------------------------------------------------------------------*/
33 #include "sdr_pin_defines_A0002.h"
34
35
36 /*------------------------------------------------------------------------------
37 Project Includes
38 ------------------------------------------------------------------------------*/
39 #include "main.h"
40 #include "imu.h"
41 #include "baro.h"
42 #include "timer.h"
43 #include "usb.h"
44 #include "sensor.h"
45 #include "math_sdr.h"
46 #include "mahony.h"
47 #include "error_sdr.h"
48 #include "debug_sdr.h"
49
50 /*------------------------------------------------------------------------------
51 Private Macros
52 ------------------------------------------------------------------------------*/
53
54 /*
55 * Initial Mahony gains for firmware integration.
56 *
57 * Proportional correction is enabled conservatively. Integral correction
58 * remains disabled until the gains are tuned using stationary and flight data.
59 *
60 *
61 * Proportional gain (KP) corrects gyro drift using the accelerometer. It is
62 * deliberately low to reduce overcorrection during flight.
63 *
64 * Integral gain (KI) learns persistent gyro bias over time. It remains
65 * disabled because an untuned integral term can accumulate incorrect
66 * corrections during vibration, launch acceleration, or invalid accelerometer
67 * data.
68 */
69 #define SENSOR_MAHONY_KP 1.0f
70 #define SENSOR_MAHONY_KI 0.0f
71
72 /* Barometer EMA Alphas */
73 #define BARO_PRESS_ALPHA (0.7f)
74 #define BARO_TEMP_ALPHA (0.7f)
75
76
77 /*------------------------------------------------------------------------------
78 Global Variables
79 ------------------------------------------------------------------------------*/
80 extern GPS_DATA gps_data;
81 extern IMU_OFFSET imu_offset;
82
83 /* Timing (sensors) */
84 uint64_t imu_velo_tick = 0;
85
86 /* IMU */
87 float velo_x_prev = 0.0f;
88 float velo_y_prev = 0.0f;
89 float velo_z_prev = 0.0f;
90
91 /* State estimation */
92 QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f };
93
94
95 /*------------------------------------------------------------------------------
96 Static Variables
97 ------------------------------------------------------------------------------*/
98 static MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_NORMAL;
99
100 /*
101 * Persistent attitude filter state. This instance retains the quaternion and
102 * integral correction between consecutive IMU updates.
103 */
104 static MAHONY_FILTER mahony_filter;
105
106 /* Timestamp of the previous Mahony update in microseconds. */
107 static uint64_t mahony_tick = 0;
108
109 static float ema_press_prev = 0.0f;
110 static float ema_temp_prev = 0.0f;
111
112
113 /*------------------------------------------------------------------------------
114 Internal function prototypes
115 ------------------------------------------------------------------------------*/
116
117 static SENSOR_STATUS sensor_get_it_ready
118 (
119 uint32_t timeout
120 );
121
122 static void sensor_conv_mag
123 (
124 IMU_CONVERTED* imu_converted,
125 IMU_RAW* imu_raw
126 );
127
128 static QUAT quat_grav_attitude
129 (
130 float ax,
131 float ay,
132 float az,
133 QUAT attitude
134 );
135
136 static float quat_to_yaw
137 (
138 QUAT q
139 );
140
141 static void sensor_baro_ema
142 (
143 SENSOR_DATA* sen_data_ptr
144 );
145
146
147 /*------------------------------------------------------------------------------
148 API Functions
149 ------------------------------------------------------------------------------*/
150
151 /**
152 * @brief Executes a sensor subcommand and transmits the requested readings.
153 * @param subcommand Sensor subcommand code.
154 * @return Sensor operation status.
155 */
156 2 SENSOR_STATUS sensor_cmd_execute
157 (
158 uint8_t subcommand
159 )
160 {
161
162 /*------------------------------------------------------------------------------
163 Local Variables
164 ------------------------------------------------------------------------------*/
165 2 SENSOR_STATUS sensor_status; /* Status indicating if
166 subcommand function
167 returned properly */
168 2 USB_STATUS usb_status; /* USB return codes */
169 2 SENSOR_DATA sensor_data; /* Struct with all sensor
170 data */
171 2 uint8_t sensor_data_bytes[ SENSOR_DATA_SIZE ]; /* Byte array with sensor
172 readouts */
173 2 uint8_t num_sensor_bytes = SENSOR_DATA_SIZE; /* Size of data in bytes */
174
175 /*------------------------------------------------------------------------------
176 Initializations
177 ------------------------------------------------------------------------------*/
178 2 usb_status = USB_OK;
179 2 sensor_status = SENSOR_OK;
180
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 memset( &sensor_data_bytes[0], 0, sizeof( sensor_data_bytes ) );
181 2 memset( &sensor_data , 0, sizeof( sensor_data ) );
182
183
184 /*------------------------------------------------------------------------------
185 Implementation
186 ------------------------------------------------------------------------------*/
187
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 switch ( subcommand )
188 {
189 /*--------------------------------------------------------------------------
190 SENSOR DUMP
191 --------------------------------------------------------------------------*/
192 2 case SENSOR_DUMP_CODE:
193 {
194 /* Tell the PC how many bytes to expect */
195 2 usb_status = usb_transmit( &num_sensor_bytes,
196 sizeof( num_sensor_bytes ),
197 HAL_DEFAULT_TIMEOUT );
198
199
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if ( usb_status != USB_OK )
200 {
201 return SENSOR_USB_FAIL;
202 }
203
204 /* Get the sensor readings */
205 2 sensor_status = sensor_dump( &sensor_data );
206
207 /* Convert to byte array */
208
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 memcpy( &(sensor_data_bytes[0]), &sensor_data, sizeof( sensor_data ) );
209
210 /* Transmit sensor readings to PC */
211
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if ( sensor_status == SENSOR_OK )
212 {
213 2 usb_transmit( &sensor_data_bytes[0],
214 sizeof( sensor_data_bytes ),
215 HAL_SENSOR_TIMEOUT );
216 2 return ( sensor_status );
217 }
218 else
219 {
220 /* Sensor readings not recieved */
221 return( SENSOR_FAIL );
222 }
223 } /* SENSOR_DUMP_CODE */
224
225 /*--------------------------------------------------------------------------
226 UNRECOGNIZED SUBCOMMAND
227 --------------------------------------------------------------------------*/
228 default:
229 {
230 return ( SENSOR_UNRECOGNIZED_OP );
231 }
232 }
233
234 } /* sensor_cmd_execute */
235
236
237
238 /**
239 * @brief Reads the available sensors and fills the sensor data structure.
240 * @param sensor_data_ptr Pointer to the sensor data structure to fill.
241 * @return Sensor operation status.
242 */
243 1182 SENSOR_STATUS sensor_dump
244 (
245 SENSOR_DATA* sensor_data_ptr /* Pointer to the sensor data struct should
246 be written */
247 )
248 {
249 /*------------------------------------------------------------------------------
250 Local Variables
251 ------------------------------------------------------------------------------*/
252 1182 SENSOR_STATUS parallel_status;
253 1182 SENSOR_STATUS body_state_status;
254 1182 IMU_STATUS imu_status;
255 1182 BARO_STATUS baro_status;
256 1182 IMU_RAW imu_raw;
257
258 /*------------------------------------------------------------------------------
259 Initializations
260 ------------------------------------------------------------------------------*/
261 1182 parallel_status = SENSOR_OK;
262 1182 body_state_status = SENSOR_OK;
263 1182 imu_status = IMU_OK;
264 1182 baro_status = BARO_OK;
265
266 /* Poll Sensors */
267
268 /*Call sensor API functions*/
269
270 /* check that IMU & BARO are ready to be read */
271 1182 parallel_status = sensor_get_it_ready( HAL_DEFAULT_TIMEOUT );
272
273
1/2
✓ Branch 0 taken 1181 times.
✗ Branch 1 not taken.
 1181 if( parallel_status != SENSOR_OK )
274 {
275 return parallel_status;
276 }
277
278 /* Disabling interrupts to avoid race conditions */
279 1181 sensor_mutex_reserve();
280
281 /* CRITICAL SECTION BEGIN */
282
283 1181 memset( &(imu_raw), 0, sizeof( IMU_RAW ) );
284
285 /* GPS sensor */
286 1181 sensor_data_ptr->gps_altitude_ft = gps_data.altitude_ft;
287 1181 sensor_data_ptr->gps_speed_kmh = gps_data.speed_km;
288 1181 sensor_data_ptr->gps_utc_time = gps_data.utc_time;
289 1181 sensor_data_ptr->gps_dec_longitude = gps_data.dec_longitude;
290 1181 sensor_data_ptr->gps_dec_latitude = gps_data.dec_latitude;
291 1181 sensor_data_ptr->gps_ns = gps_data.ns;
292 1181 sensor_data_ptr->gps_ew = gps_data.ew;
293 1181 sensor_data_ptr->gps_gll_status = gps_data.gll_status;
294 1181 sensor_data_ptr->gps_rmc_status = gps_data.rmc_status;
295
296 /* IMU Read */
297 1181 imu_status = get_imu_it( &imu_raw );
298
299 /* Baro Read */
300 1181 baro_status = get_baro_it( &(sensor_data_ptr->baro_pressure), &(sensor_data_ptr->baro_temp) );
301
302 /*Compute State Estimations*/
303
304 /* Calculated and retrieve converted IMU data */
305 1181 sensor_conv_imu( &(sensor_data_ptr->imu_converted), &imu_raw );
306
307 /* Calculated to get body state */
308 1181 body_state_status = sensor_body_state( &(sensor_data_ptr->imu_converted), &(sensor_data_ptr->state_estimate) );
309
310 /* Calculated velocity and position */
311 1181 sensor_imu_velo( &(sensor_data_ptr->imu_converted), &(sensor_data_ptr->state_estimate) );
312
313 /* Calculated baro exponential moving average */
314 1181 sensor_baro_ema( sensor_data_ptr );
315
316 /* Calculated altitude from barometer */
317 1181 sensor_baro_alt( sensor_data_ptr );
318
319 /* CRITICAL SECTION END */
320
321 /* Re-enabling interrupts after potentially dangerous reads/writes occur */
322 1181 sensor_mutex_release();
323
324 /*------------------------------------------------------------------------------
325 Set command status from sensor API returns
326 ------------------------------------------------------------------------------*/
327
328 /* Start next measurement and return status */
329 1181 parallel_status |= sensor_start_IT( sensor_data_ptr );
330
331
1/2
✓ Branch 0 taken 1181 times.
✗ Branch 1 not taken.
 1181 if( imu_status != IMU_OK )
332 {
333 return SENSOR_IMU_FAIL;
334 }
335
1/2
✓ Branch 0 taken 1181 times.
✗ Branch 1 not taken.
 1181 else if ( body_state_status != SENSOR_OK )
336 {
337 return body_state_status;
338 }
339
1/2
✓ Branch 0 taken 1181 times.
✗ Branch 1 not taken.
 1181 else if ( baro_status != BARO_OK)
340 {
341 return SENSOR_BARO_ERROR;
342 }
343
1/2
✓ Branch 0 taken 1181 times.
✗ Branch 1 not taken.
 1181 else if ( parallel_status != SENSOR_OK )
344 {
345 return SENSOR_IT_TIMEOUT;
346 }
347 else
348 {
349 1181 return SENSOR_OK;
350 }
351 } /* sensor_dump */
352
353
354 /**
355 * @brief Initializes sensor timing and resets velocity state.
356 * @param preset_data Pointer to the preset calibration data.
357 */
358 13 SENSOR_STATUS sensor_init
359 (
360 PRESET_DATA* preset_data
361 )
362 {
363 13 float ax = preset_data->imu_offset.accel_x;
364 13 float ay = preset_data->imu_offset.accel_y;
365 13 float az = preset_data->imu_offset.accel_z;
366
367 13 QUAT initial_attitude = quat_grav_attitude
368 (
369 ax,
370 ay,
371 az,
372 13 IDENTITY_QUAT
373 );
374
375 13 imu_velo_tick = get_us_tick();
376 13 mahony_tick = imu_velo_tick;
377
378 13 sensor_reset_velo();
379
380 13 MAHONY_STATUS mahony_status = mahony_init
381 (
382 &mahony_filter,
383 initial_attitude,
384 SENSOR_MAHONY_KP,
385 SENSOR_MAHONY_KI
386 );
387
388
2/2
✓ Branch 0 taken 1 time.
✓ Branch 1 taken 12 times.
 13 if ( mahony_status != MAHONY_OK )
389 {
390 1 return( SENSOR_FAIL );
391 }
392
393 return( SENSOR_OK );
394
395 } /* sensor_init */
396
397
398
399 /**
400 * @brief Converts raw IMU readings into calibrated, remapped sensor data.
401 * @param imu_converted Converted IMU data to fill.
402 * @param imu_raw Raw IMU readouts.
403 */
404 1182 void sensor_conv_imu
405 (
406 IMU_CONVERTED* imu_converted,
407 IMU_RAW* imu_raw
408 )
409 {
410 /* Convert raw accel values */
411 1182 imu_converted->accel_x = sensor_acc_conv(imu_raw->accel_x);
412 1182 imu_converted->accel_y = sensor_acc_conv(imu_raw->accel_y);
413 1182 imu_converted->accel_z = sensor_acc_conv(imu_raw->accel_z);
414
415 1182 sensor_axis_remap( &(imu_converted->accel_x), &(imu_converted->accel_y), &(imu_converted->accel_z) );
416
417 /* Do not use offset compensation for accel to preserve gravity */
418 /*
419 imu_converted.accel_x -= imu_offset.accel_x;
420 imu_converted.accel_y -= imu_offset.accel_y;
421 imu_converted.accel_z -= imu_offset.accel_z;
422 */
423
424 /* Convert raw gyroscope values to deg/s and remap axes */
425 1182 imu_converted->gyro_x = sensor_gyro_conv(imu_raw->gyro_x);
426 1182 imu_converted->gyro_y = sensor_gyro_conv(imu_raw->gyro_y);
427 1182 imu_converted->gyro_z = sensor_gyro_conv(imu_raw->gyro_z);
428
429 /* Remove gyro bias BEFORE applying axis conversion */
430 1182 imu_converted->gyro_x -= imu_offset.gyro_x;
431 1182 imu_converted->gyro_y -= imu_offset.gyro_y;
432 1182 imu_converted->gyro_z -= imu_offset.gyro_z;
433
434 1182 sensor_axis_remap( &(imu_converted->gyro_x), &(imu_converted->gyro_y), &(imu_converted->gyro_z) );
435
436 1182 sensor_conv_mag(imu_converted, imu_raw);
437 1182 }
438
439
440 /**
441 * @brief Get the mount orientation
442 *
443 * @return The configured mount orientation
444 */
445 2 MOUNT_ORIENTATION sensor_get_mount_orientation
446 (
447 void
448 )
449 {
450 2 return mount_orientation;
451
452 } /* get_mount_orientation */
453
454
455 /**
456 * @brief Set the mount orientation
457 *
458 * @param orientation The new mount orientation
459 */
460 10 void sensor_set_mount_orientation
461 (
462 MOUNT_ORIENTATION orientation
463 )
464 {
465 10 mount_orientation = orientation;
466
467 10 } /* set_mount_orientation */
468
469
470 /**
471 * @brief Integrates gyro data to update the estimated body attitude and rate.
472 * @param imu_converted Converted IMU data.
473 * @param state_estimate State estimate to update.
474 */
475 1182 SENSOR_STATUS sensor_body_state
476 (
477 const IMU_CONVERTED* imu_converted,
478 STATE_ESTIMATION* state_estimate
479 )
480 {
481 1182 uint64_t current_tick;
482 1182 uint64_t imu_tdelta;
483
484 1182 float delta_time_s;
485
486 1182 bool use_accel;
487
488 1182 VECTOR_3F gyro_body_rad_s;
489 1182 VECTOR_3F accel_body_m_s2;
490
491 /* Calculate elapsed time between attitude updates using the microsecond timer. */
492 1182 current_tick = get_us_tick();
493 1182 imu_tdelta = current_tick - mahony_tick;
494
495 1182 delta_time_s = (float)imu_tdelta / (float)MICROSEC_PER_SEC;
496
497
2/2
✓ Branch 0 taken 1179 times.
✓ Branch 1 taken 3 times.
 1182 if ( mahony_tick == 0
498
1/2
✓ Branch 0 taken 1179 times.
✗ Branch 1 not taken.
 1179 || delta_time_s <= 0.0f
499
2/2
✓ Branch 0 taken 1 time.
✓ Branch 1 taken 1178 times.
 1179 || delta_time_s > 1.0f )
500 {
501 4 delta_time_s = 0.01f;
502 }
503
504 1182 mahony_tick = current_tick;
505
506 /*
507 * Converted gyro data is in degrees per second. Mahony requires radians per
508 * second.
509 */
510 1182 gyro_body_rad_s.x = deg_to_rad(imu_converted->gyro_x);
511 1182 gyro_body_rad_s.y = deg_to_rad(imu_converted->gyro_y);
512 1182 gyro_body_rad_s.z = deg_to_rad(imu_converted->gyro_z);
513
514 /*
515 * Converted accelerometer data is already in meters per second squared.
516 */
517 1182 accel_body_m_s2.x = imu_converted->accel_x;
518 1182 accel_body_m_s2.y = imu_converted->accel_y;
519 1182 accel_body_m_s2.z = imu_converted->accel_z;
520
521 /*
522 * Permit accelerometer correction only before powered flight. The Mahony
523 * filter still performs its own magnitude and finite-value checks.
524 */
525 1182 use_accel = get_fc_state() <= FC_STATE_LAUNCH_DETECT;
526
527 1182 MAHONY_STATUS mahony_status = mahony_update_imu
528 (
529 &mahony_filter,
530 gyro_body_rad_s,
531 accel_body_m_s2,
532 delta_time_s,
533 use_accel
534 );
535
536
1/2
✓ Branch 0 taken 1182 times.
✗ Branch 1 not taken.
 1182 if ( mahony_status != MAHONY_OK )
537 {
538 return SENSOR_IMU_FAIL;
539 }
540
541 /*
542 * Store the filter's body-to-world quaternion as the system attitude estimate.
543 */
544 1182 state_estimate->attitude = mahony_filter.attitude;
545
546 /*
547 * Preserve the existing public roll-rate units of degrees per second.
548 */
549 1182 state_estimate->roll_rate = imu_converted->gyro_x;
550
551 1182 return SENSOR_OK;
552
553 } /* sensor_body_state */
554
555
556
557 /**
558 * @brief Remaps sensor axes according to the flight-computer mounting orientation.
559 * @param x X-axis value to remap.
560 * @param y Y-axis value to remap.
561 * @param z Z-axis value to remap.
562 */
563 2365 void sensor_axis_remap
564 (
565 float* x,
566 float* y,
567 float* z
568 )
569 {
570 2365 *x *= mount_orientation;
571 2365 (void)y;
572 2365 *z *= mount_orientation;
573 2365 }
574
575
576
577 /**
578 * @brief Converts a raw accelerometer reading to meters per second squared.
579 * @param readout Raw accelerometer readout.
580 * @return Acceleration in meters per second squared.
581 */
582 3547 float sensor_acc_conv
583 (
584 int16_t readout
585 )
586 {
587 /* Scale readout value from integer limits to +/- the accelerometer measurement range */
588 3547 const float accel_step = (2.0f * ACCEL_G_RANGE * GRAVITY ) / UINT16_MAX;
589
590 3547 return accel_step * readout;
591
592 } /* sensor_acc_conv */
593
594
595 /**
596 * @brief Converts a raw gyro reading to degrees per second.
597 * @param readout Raw gyro readout.
598 * @return Angular rate in degrees per second.
599 */
600 3547 float sensor_gyro_conv
601 (
602 int16_t readout
603 )
604 {
605 3547 const float gyro_sens = UINT16_MAX / ( 2.0f * GYRO_RANGE );
606
607 3547 return readout / gyro_sens;
608
609 } /* sensor_gyro_conv */
610
611
612 /**
613 * @brief Calculates velocity from converted accelerometer measurements.
614 * @param imu_converted Converted IMU data.
615 * @param state_estimate State estimate to update.
616 */
617 1182 void sensor_imu_velo
618 (
619 const IMU_CONVERTED* imu_converted,
620 STATE_ESTIMATION* state_estimate
621 )
622 {
623 1182 float velo_x, velo_y, velo_z, velocity;
624
625 /*
626 * The world frame uses North-East-Down (NED) coordinates,
627 * so gravity points in the world-frame +Z direction.
628 */
629 1182 const QUAT gravity_world =
630 {
631 .w = 0.0f,
632 .x = 0.0f,
633 .y = 0.0f,
634 .z = -GRAVITY
635 };
636
637 /*
638 * The attitude quaternion is body-to-world, so rotate world gravity
639 * into the body frame before subtracting it from the accelerometer.
640 */
641 1182 QUAT gravity_body = quat_rotate_world_to_body
642 (
643 state_estimate->attitude,
644 gravity_world
645 );
646
647 1182 QUAT linear_accel_body =
648 {
649 .w = 0.0f,
650 1182 .x = imu_converted->accel_x - gravity_body.x,
651 1182 .y = imu_converted->accel_y - gravity_body.y,
652 1182 .z = imu_converted->accel_z - gravity_body.z
653 };
654
655 /*
656 * Rotate gravity-compensated acceleration into the world frame so the
657 * integrated velocity components remain in a fixed coordinate frame.
658 */
659 1182 QUAT linear_accel_world = quat_rotate_body_to_world
660 (
661 state_estimate->attitude,
662 linear_accel_body
663 );
664
665 1182 float accel_world_x = linear_accel_world.x;
666 1182 float accel_world_y = linear_accel_world.y;
667 1182 float accel_world_z = linear_accel_world.z;
668
669 1182 float ts_delta;
670
671 1182 uint64_t current_tick = get_us_tick();
672 1182 uint64_t imu_tdelta = current_tick - imu_velo_tick;
673 1182 ts_delta = (float) imu_tdelta / (float) MICROSEC_PER_SEC;
674
675 // Calculate 3 velocity vectors using motion equations
676 1182 velo_x = velo_x_prev + accel_world_x * ts_delta;
677 1182 velo_y = velo_y_prev + accel_world_y * ts_delta;
678 1182 velo_z = velo_z_prev + accel_world_z * ts_delta;
679
680 // Calculate the velocity scalar
681 1182 velocity = sqrtf
682 (
683 1182 velo_x * velo_x +
684 1182 velo_y * velo_y +
685 1182 velo_z * velo_z
686 );
687
688 /* Update state estimations*/
689 1182 state_estimate->velo_x = velo_x;
690 1182 state_estimate->velo_y = velo_y;
691 1182 state_estimate->velo_z = velo_z;
692
693 1182 state_estimate->velocity = velocity;
694
695 // Save current velocity for next computation
696 1182 velo_x_prev = velo_x;
697 1182 velo_y_prev = velo_y;
698 1182 velo_z_prev = velo_z;
699
700 1182 imu_velo_tick = current_tick;
701
702 1182 }
703
704
705 /**
706 * @brief Calculates altitude from the current barometric pressure and temperature.
707 * @param sen_data Sensor data structure to update.
708 */
709 1292 void sensor_baro_alt(SENSOR_DATA* sen_data)
710 {
711 1292 float pressure = sen_data->baro_pressure;
712 1292 float temp = sen_data->baro_temp;
713 // conv pressure to pascal for equation
714 // pressure *= 6894.76;
715
716 // calc altitude
717 1292 float PRESSURE_SEA_LEVEL = 101325;
718 1292 float EXP = 0.190294958;
719 1292 float TEMP_LAPSE_RATE = 0.0065;
720
721 1292 float alt = (pow(PRESSURE_SEA_LEVEL / pressure, EXP) - 1) * (temp + 273.15) / TEMP_LAPSE_RATE;
722
723 1292 sen_data->baro_alt = alt;
724
725 1292 }
726
727
728 /**
729 * @brief Resets accumulated velocity values to prevent drift.
730 */
731 14 void sensor_reset_velo
732 (
733 void
734 )
735 {
736 14 velo_x_prev = 0;
737 14 velo_y_prev = 0;
738 14 velo_z_prev = 0;
739
740 14 } /* sensor_reset_velo */
741
742
743 #if defined( A0002_REV2 )
744
745 /**
746 * @brief Starts interrupt-driven measurements for the enabled sensors.
747 * @param sensor_data_ptr Sensor data structure associated with the readings.
748 * @return Sensor operation status.
749 */
750 1189 SENSOR_STATUS sensor_start_IT
751 (
752 SENSOR_DATA* sensor_data_ptr
753 )
754 {
755
2/2
✓ Branch 1 taken 1188 times.
✓ Branch 2 taken 1 time.
 1189 if( start_imu_read_IT() != IMU_OK )
756 {
757 return SENSOR_IMU_FAIL;
758 }
759
2/2
✓ Branch 1 taken 1 time.
✓ Branch 2 taken 1187 times.
 1188 if( start_baro_read_IT() != BARO_OK )
760 {
761 1 return SENSOR_BARO_ERROR;
762 }
763 return SENSOR_OK;
764
765 } /* sensor_start_IT */
766
767
768
769 /**
770 * @brief Disables sensor-related interrupts while sensor data is accessed.
771 */
772 1181 void sensor_mutex_reserve
773 (
774 void
775 )
776 {
777 1181 HAL_NVIC_DisableIRQ( GPS_UART_IRQn );
778 /* HAL_NVIC_DisableIRQ( [lora placeholder] ); */
779
780 1181 } /* sensor_mutex_reserve */
781
782
783
784 /**
785 * @brief Re-enables sensor-related interrupts after sensor data access.
786 */
787 1181 void sensor_mutex_release
788 (
789 void
790 )
791 {
792 1181 HAL_NVIC_EnableIRQ( GPS_UART_IRQn );
793 /* HAL_NVIC_EnableIRQ( [lora placeholder] ); */
794
795 1181 } /* sensor_mutex_release */
796
797 #endif
798
799
800 /*------------------------------------------------------------------------------
801 Internal procedures
802 ------------------------------------------------------------------------------*/
803
804
805
806 /*******************************************************************************
807 * *
808 * PROCEDURE: *
809 * quat_grav_attitude *
810 * *
811 * DESCRIPTION: *
812 * Computes quaternion attitude from static accelerometer data *
813 * Experiences gimbal lock at pitch = +/- 90 degrees *
814 * *
815 *******************************************************************************/
816 13 static QUAT quat_grav_attitude
817 (
818 float ax,
819 float ay,
820 float az,
821 QUAT attitude
822 )
823 {
824 /* Compute pitch/roll from accelerometer */
825 13 float grav_pitch = atan2f(ax, sqrtf(ay * ay + az * az));
826 13 float grav_roll = atan2f(ay, az);
827
828 13 float yaw = quat_to_yaw(attitude);
829
830 13 return eul_to_quat(yaw, grav_pitch, grav_roll);
831
832 }
833
834 /* Comment deferred until mod#132
835 Formula in https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles */
836 13 static float quat_to_yaw
837 (
838 QUAT q
839 )
840 {
841 13 float y = 2.0f * (q.w * q.z + q.x * q.y);
842 13 float x = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);
843
844 13 return atan2f(y, x);
845 }
846
847 #ifdef A0002_REV2
848
849 /**
850 * @brief Waits until the interrupt-driven sensors report ready status.
851 * @param timeout Maximum wait time in milliseconds.
852 * @return Sensor operation status.
853 */
854 1182 static SENSOR_STATUS sensor_get_it_ready
855 (
856 uint32_t timeout
857 )
858 {
859 /* set up timeout */
860 1182 uint32_t starting_time = HAL_GetTick();
861 1182 uint32_t curr_time = HAL_GetTick();
862
1/2
✓ Branch 1 taken 1049867179 times.
✗ Branch 2 not taken.
 1049867179 while( curr_time <= starting_time + timeout )
863 {
864 /* Ensure both the IMU and barometer are ready to be read */
865
2/2
✓ Branch 1 taken 521741381 times.
✓ Branch 2 taken 528125798 times.
 1049867179 if ( imu_get_imu_data_ready()
866
2/2
✓ Branch 1 taken 1181 times.
✓ Branch 2 taken 521740200 times.
 521741381 && imu_get_mag_data_ready()
867
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1181 times.
 1181 && baro_get_baro_data_ready() )
868 {
869 return SENSOR_OK;
870 }
871
872 /* update timeout poll */
873 1049865998 curr_time = HAL_GetTick();
874 }
875
876 return SENSOR_IT_TIMEOUT;
877
878 }
879
880
881
882 /**
883 * @brief Converts raw magnetometer readings into magnetic field data.
884 * @param imu_converted Converted IMU data to update.
885 * @param imu_raw Raw magnetometer readouts.
886 *
887 * @attention
888 *
889 * This function is heavily derived from the official Bosch BMM150
890 * driver, which is protected by the BSD-3-Clause license. This function
891 * is exempt from any licensing that may be applied to a current/future
892 * Sun Devil Rocketry project. Per the terms of the BSD-3-Clause license,
893 * the following notice is retained from the source project and applies
894 * to the procedure below.
895 *
896 * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
897 *
898 * BSD-3-Clause
899 *
900 * Redistribution and use in source and binary forms, with or without
901 * modification, are permitted provided that the following conditions are
902 * met:
903 *
904 * 1. Redistributions of source code must retain the above copyright
905 * notice, this list of conditions and the following disclaimer.
906 *
907 * 2. Redistributions in binary form must reproduce the above copyright
908 * notice, this list of conditions and the following disclaimer in the
909 * documentation and/or other materials provided with the distribution.
910 *
911 * 3. Neither the name of the copyright holder nor the names of its
912 * contributors may be used to endorse or promote products derived from
913 * this software without specific prior written permission.
914 *
915 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
916 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
917 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
918 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
919 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
920 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
921 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
922 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
923 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
924 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
925 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
926 * POSSIBILITY OF SUCH DAMAGE.
927 */
928 1182 static void sensor_conv_mag
929 (
930 IMU_CONVERTED* imu_converted,
931 IMU_RAW* imu_raw
932 )
933 {
934 /*------------------------------------------------------------------------------
935 Local Variables
936 ------------------------------------------------------------------------------*/
937 1182 MAG_TRIM mag_trim;
938 1182 float mag_x;
939 1182 float mag_y;
940 1182 float mag_z;
941
942 /*------------------------------------------------------------------------------
943 Initializations
944 ------------------------------------------------------------------------------*/
945 1182 mag_trim = imu_get_mag_trim();
946
947 /*------------------------------------------------------------------------------
948 Apply Bosch compensation using factory trim values
949 ------------------------------------------------------------------------------*/
950
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1179 times.
 1182 float rhall = (imu_raw->mag_hall == 0) ? mag_trim.dig_xyz1 : imu_raw->mag_hall;
951
952 /* ---- X compensation ---- */
953 1182 float process_comp_x0 = ((float)mag_trim.dig_xyz1) * 16384.0f / rhall;
954 1182 float process_comp_x1 = process_comp_x0 - 16384.0f;
955 1182 float process_comp_x2 = ((float)mag_trim.dig_xy2) * (process_comp_x1 * process_comp_x1 / 268435456.0f);
956 1182 float process_comp_x3 = process_comp_x2 + process_comp_x1 * ((float)mag_trim.dig_xy1) / 16384.0f;
957 1182 float process_comp_x4 = ((float)mag_trim.dig_x2) + 160.0f;
958 1182 float process_comp_x5 = ((float)imu_raw->mag_x) * (((process_comp_x3 + 256.0f) * process_comp_x4) / 8192.0f);
959 1182 mag_x = (process_comp_x5 / 16.0f) / 10.0f; // µT
960
961 /* ---- Y compensation ---- */
962 1182 float process_comp_y0 = ((float)mag_trim.dig_xyz1) * 16384.0f / rhall;
963 1182 float process_comp_y1 = process_comp_y0 - 16384.0f;
964 1182 float process_comp_y2 = ((float)mag_trim.dig_xy2) * (process_comp_y1 * process_comp_y1 / 268435456.0f);
965 1182 float process_comp_y3 = process_comp_y2 + process_comp_y1 * ((float)mag_trim.dig_xy1) / 16384.0f;
966 1182 float process_comp_y4 = ((float)mag_trim.dig_y2) + 160.0f;
967 1182 float process_comp_y5 = ((float)imu_raw->mag_y) * (((process_comp_y3 + 256.0f) * process_comp_y4) / 8192.0f);
968 1182 mag_y = (process_comp_y5 / 16.0f) / 10.0f; // µT
969
970 /* ---- Z compensation ---- */
971 1182 float process_comp_z0 = ((float)imu_raw->mag_z) - ((float)mag_trim.dig_z4) * 128.0f;
972 1182 float process_comp_z1 = ((float)mag_trim.dig_z3) * (rhall - ((float)mag_trim.dig_xyz1)) / 4.0f;
973
2/2
✓ Branch 0 taken 1179 times.
✓ Branch 1 taken 3 times.
 1182 float process_comp_z2 = (mag_trim.dig_z2 == 0) ? 0.0f :
974 1179 (((process_comp_z0 - process_comp_z1) * ((float)mag_trim.dig_z1)) /
975 1179 (((float)mag_trim.dig_z2) + ((float)mag_trim.dig_z1) * (rhall - (float)mag_trim.dig_xyz1) / 32768.0f));
976 1182 mag_z = process_comp_z2 / 4.0f / 10.0f; // µT
977
978 /*------------------------------------------------------------------------------
979 Store converted field data
980 ------------------------------------------------------------------------------*/
981 1182 imu_converted->mag_x = mag_x;
982 1182 imu_converted->mag_y = mag_y;
983 1182 imu_converted->mag_z = mag_z;
984 1182 } /* sensor_conv_mag */
985 #endif
986
987
988 /**
989 * @brief Updates the barometer exponential moving average (EMA) with the latest sensor readings.
990 * @param sen_data_ptr Pointer to the sensor data structure containing barometer readings.
991 */
992 1181 static void sensor_baro_ema
993 (
994 SENSOR_DATA* sen_data_ptr
995 )
996 {
997
3/4
✓ Branch 0 taken 1177 times.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1177 times.
 1181 if (ema_press_prev == 0.0f || ema_temp_prev == 0.0f)
998 {
999 4 ema_press_prev = sen_data_ptr->baro_pressure;
1000 4 ema_temp_prev = sen_data_ptr->baro_temp;
1001 4 return;
1002
1003 }
1004
1005 1177 ema_press_prev = ( BARO_PRESS_ALPHA * sen_data_ptr->baro_pressure) + ( (1-BARO_PRESS_ALPHA) * ema_press_prev);
1006 1177 ema_temp_prev = ( BARO_TEMP_ALPHA * sen_data_ptr->baro_temp) + ( (1-BARO_TEMP_ALPHA) * ema_temp_prev);
1007
1008 } /* sensor_baro_ema*/
1009
1010 /**
1011 * END OF FILE
1012 */
1013