GCC Code Coverage Report

Directory: /home/runner/work/Flight-Computer-Firmware/Flight-Computer-Firmware/app/rev2/
Coverage: low: ≥ 0% medium: ≥ 75.0% high: ≥ 90.0%
Coverage Exec / Excl / Total
Lines: 100.0% 60 / 0 / 60
Functions: 100.0% 1 / 0 / 1
Branches: 100.0% 4 / 0 / 4
sensor_calibrate.c
Line Branch Exec Source
1 /*******************************************************************************
2 * *
3 * FILE: *
4 * imu_calib.c *
5 * *
6 * DESCRIPTION: *
7 * Calibrate inertial measuring unit. *
8 * *
9 * CRITICALITY: *
10 * FQ - Flight Qualified *
11 * *
12 * COPYRIGHT: *
13 * Copyright (c) 2025 Sun Devil Rocketry. *
14 * All rights reserved. *
15 * *
16 * This software is licensed under terms that can be found in the LICENSE *
17 * file in the root directory of this software component. *
18 * If no LICENSE file comes with this software, it is covered under the *
19 * BSD-3-Clause. *
20 * *
21 * https://opensource.org/license/bsd-3-clause *
22 * *
23 *******************************************************************************/
24
25 /*------------------------------------------------------------------------------
26 Standard Includes
27 ------------------------------------------------------------------------------*/
28 #include "led.h"
29 #include "main.h"
30 #include "usb.h"
31 #include "imu.h"
32 #include "sensor.h"
33 #include "debug_sdr.h"
34 #include "error_sdr.h"
35
36 /*------------------------------------------------------------------------------
37 Instantiations
38 ------------------------------------------------------------------------------*/
39 extern PRESET_DATA preset_data;
40 extern SENSOR_DATA sensor_data;
41 extern IMU_OFFSET imu_offset;
42 extern BARO_PRESET baro_preset;
43
44 /*******************************************************************************
45 * *
46 * PROCEDURE: *
47 * sensorCalibrationSWCON *
48 * *
49 * DESCRIPTION: *
50 * Calibrate initial state of the sensor for n samples *
51 * *
52 *******************************************************************************/
53 2 void sensorCalibrationSWCON(){
54 2 debug_log_msg("Beginning calibration sequence. On the emulator, this may take longer than expected.", LOG_LVL_INFO);
55 #ifdef DEBUG
56 uint32_t start_time = HAL_GetTick();
57 #endif
58 2 uint16_t samples = preset_data.config_settings.sensor_calibration_samples;
59 2 SENSOR_STATUS sensor_status = SENSOR_OK;
60 2 (void)sensor_status;
61
62 /* Make sure this is consistent at calib, then flip after if we need to */
63 2 sensor_set_mount_orientation( MOUNT_ORIENTATION_IMU_NORMAL );
64
65 2 preset_data.imu_offset.accel_x = 0.00;
66 2 preset_data.imu_offset.accel_y = 0.00;
67 2 preset_data.imu_offset.accel_z = 0.00;
68 2 preset_data.imu_offset.gyro_x = 0.00;
69 2 preset_data.imu_offset.gyro_y = 0.00;
70 2 preset_data.imu_offset.gyro_z = 0.00;
71
72 2 imu_offset.accel_x = 0.00;
73 2 imu_offset.accel_y = 0.00;
74 2 imu_offset.accel_z = 0.00;
75 2 imu_offset.gyro_x = 0.00;
76 2 imu_offset.gyro_y = 0.00;
77 2 imu_offset.gyro_z = 0.00;
78
79 2 baro_preset.baro_pres = 0.00;
80 2 baro_preset.baro_temp = 0.00;
81
82 2 float calc_acc_x = 0.00;
83 2 float calc_acc_y = 0.00;
84 2 float calc_acc_z = 0.00;
85 2 float calc_gyro_x = 0.00;
86 2 float calc_gyro_y = 0.00;
87 2 float calc_gyro_z = 0.00;
88
89 2 float calc_baro_pres = 0.00;
90 2 float calc_baro_temp = 0.00;
91
92
2/2
✓ Branch 0 taken 200 times.
✓ Branch 1 taken 2 times.
 202 for (int i = 0; i < samples; i++){
93 200 sensor_status = sensor_dump( &sensor_data );
94 200 debug_assert( sensor_status == SENSOR_OK, ERROR_SENSOR_CMD_ERROR );
95 200 calc_acc_x = calc_acc_x + sensor_data.imu_converted.accel_x;
96 200 calc_acc_y = calc_acc_y + sensor_data.imu_converted.accel_y;
97 200 calc_acc_z = calc_acc_z + sensor_data.imu_converted.accel_z;
98 200 calc_gyro_x = calc_gyro_x + sensor_data.imu_converted.gyro_x;
99 200 calc_gyro_y = calc_gyro_y + sensor_data.imu_converted.gyro_y;
100 200 calc_gyro_z = calc_gyro_z + sensor_data.imu_converted.gyro_z;
101 200 calc_baro_pres = calc_baro_pres + sensor_data.baro_pressure;
102 200 calc_baro_temp = calc_baro_temp + sensor_data.baro_temp;
103 }
104
105 2 calc_acc_x = calc_acc_x / ( samples );
106 2 calc_acc_y = calc_acc_y / ( samples );
107 2 calc_acc_z = calc_acc_z / ( samples );
108
109 2 calc_gyro_x = calc_gyro_x / ( samples );
110 2 calc_gyro_y = calc_gyro_y / ( samples );
111 2 calc_gyro_z = calc_gyro_z / ( samples );
112
113 2 debug_assert( calc_acc_x != 0.0f, ERROR_SENSOR_CMD_ERROR );
114
115 2 calc_baro_pres = calc_baro_pres / ( samples );
116 2 calc_baro_temp = calc_baro_temp / ( samples );
117
118 2 preset_data.imu_offset.accel_x = calc_acc_x;
119 2 preset_data.imu_offset.accel_y = calc_acc_y;
120 2 preset_data.imu_offset.accel_z = calc_acc_z;
121
122 2 preset_data.imu_offset.gyro_x = calc_gyro_x;
123 2 preset_data.imu_offset.gyro_y = calc_gyro_y;
124 2 preset_data.imu_offset.gyro_z = calc_gyro_z;
125
126 2 preset_data.baro_preset.baro_pres = calc_baro_pres;
127 2 preset_data.baro_preset.baro_temp = calc_baro_temp;
128
129 /* Sensor dump readings already factor in the orientation.
130 If we read a negative value for gravity, flip the orientation.
131 This ensures that the x axis is always pointing up */
132
2/2
✓ Branch 0 taken 1 time.
✓ Branch 1 taken 1 time.
 2 if ( calc_acc_x < 0.0f )
133 {
134 1 sensor_set_mount_orientation( MOUNT_ORIENTATION_IMU_INVERTED );
135 }
136
137 /* Initialize sensor */
138 2 sensor_init( &preset_data );
139
140 #ifdef DEBUG
141 char sensor_dbg_msg[128];
142 uint32_t tdelta = HAL_GetTick() - start_time;
143 debug_ignore_emulator_warnings_start();
144 size_t msg_len = snprintf(sensor_dbg_msg, 128, "Calibration Finished. Ttotal: %ums, per-sample: %fms.",
145 tdelta, (float)tdelta / samples );
146 debug_ignore_emulator_warnings_stop();
147 debug_log(sensor_dbg_msg, msg_len, LOG_LVL_INFO);
148 MOUNT_ORIENTATION orientation = sensor_get_mount_orientation();
149 msg_len = snprintf(sensor_dbg_msg, 128, "IMU Offsets: %.04f %.04f %.04f %.04f %.04f %.04f. Baro Offsets: %.04f %.04f. Orientation: %d",
150 calc_acc_x, calc_acc_y, calc_acc_z, calc_gyro_x, calc_gyro_y, calc_gyro_z, calc_baro_pres, calc_baro_temp, orientation );
151 debug_log(sensor_dbg_msg, msg_len, LOG_LVL_INFO);
152 #endif
153
154 2 }
155
156
157 /*******************************************************************************
158 * END OF FILE *
159 *******************************************************************************/
160