GCC Code Coverage Report

Directory: ./
Coverage: low: ≥ 0% medium: ≥ 75.0% high: ≥ 90.0%
Coverage Exec / Excl / Total
Lines: 100.0% 71 / 0 / 71
Functions: 100.0% 1 / 0 / 1
Branches: 75.0% 6 / 0 / 8
app/rev2/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 3 void sensorCalibrationSWCON(){
54 3 debug_log_msg("Beginning calibration sequence. On the emulator, this may take longer than expected.", LOG_LVL_INFO);
55 #ifdef DEBUG
56 1 uint32_t start_time = HAL_GetTick();
57 #endif
58 3 uint16_t samples = preset_data.config_settings.sensor_calibration_samples;
59 3 SENSOR_STATUS sensor_status = SENSOR_OK;
60 3 (void)sensor_status;
61
62 /* Make sure this is consistent at calib, then flip after if we need to */
63 3 sensor_set_mount_orientation( MOUNT_ORIENTATION_IMU_NORMAL );
64
65 3 preset_data.imu_offset.accel_x = 0.00;
66 3 preset_data.imu_offset.accel_y = 0.00;
67 3 preset_data.imu_offset.accel_z = 0.00;
68 3 preset_data.imu_offset.gyro_x = 0.00;
69 3 preset_data.imu_offset.gyro_y = 0.00;
70 3 preset_data.imu_offset.gyro_z = 0.00;
71
72 3 imu_offset.accel_x = 0.00;
73 3 imu_offset.accel_y = 0.00;
74 3 imu_offset.accel_z = 0.00;
75 3 imu_offset.gyro_x = 0.00;
76 3 imu_offset.gyro_y = 0.00;
77 3 imu_offset.gyro_z = 0.00;
78
79 3 baro_preset.baro_pres = 0.00;
80 3 baro_preset.baro_temp = 0.00;
81
82 3 float calc_acc_x = 0.00;
83 3 float calc_acc_y = 0.00;
84 3 float calc_acc_z = 0.00;
85 3 float calc_gyro_x = 0.00;
86 3 float calc_gyro_y = 0.00;
87 3 float calc_gyro_z = 0.00;
88
89 3 float calc_baro_pres = 0.00;
90 3 float calc_baro_temp = 0.00;
91
92
2/2
✓ Branch 0 taken 220 times.
✓ Branch 1 taken 3 times.
 223 for (int i = 0; i < samples; i++){
93 220 sensor_status = sensor_dump( &sensor_data );
94
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20 times.
 220 debug_assert( sensor_status == SENSOR_OK, ERROR_SENSOR_CMD_ERROR );
95 220 calc_acc_x = calc_acc_x + sensor_data.imu_converted.accel_x;
96 220 calc_acc_y = calc_acc_y + sensor_data.imu_converted.accel_y;
97 220 calc_acc_z = calc_acc_z + sensor_data.imu_converted.accel_z;
98 220 calc_gyro_x = calc_gyro_x + sensor_data.imu_converted.gyro_x;
99 220 calc_gyro_y = calc_gyro_y + sensor_data.imu_converted.gyro_y;
100 220 calc_gyro_z = calc_gyro_z + sensor_data.imu_converted.gyro_z;
101 220 calc_baro_pres = calc_baro_pres + sensor_data.baro_pressure;
102 220 calc_baro_temp = calc_baro_temp + sensor_data.baro_temp;
103 }
104
105 3 calc_acc_x = calc_acc_x / ( samples );
106 3 calc_acc_y = calc_acc_y / ( samples );
107 3 calc_acc_z = calc_acc_z / ( samples );
108
109 3 calc_gyro_x = calc_gyro_x / ( samples );
110 3 calc_gyro_y = calc_gyro_y / ( samples );
111 3 calc_gyro_z = calc_gyro_z / ( samples );
112
113
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 time.
 3 debug_assert( calc_acc_x != 0.0f, ERROR_SENSOR_CMD_ERROR );
114
115 3 calc_baro_pres = calc_baro_pres / ( samples );
116 3 calc_baro_temp = calc_baro_temp / ( samples );
117
118 3 preset_data.imu_offset.accel_x = calc_acc_x;
119 3 preset_data.imu_offset.accel_y = calc_acc_y;
120 3 preset_data.imu_offset.accel_z = calc_acc_z;
121
122 3 preset_data.imu_offset.gyro_x = calc_gyro_x;
123 3 preset_data.imu_offset.gyro_y = calc_gyro_y;
124 3 preset_data.imu_offset.gyro_z = calc_gyro_z;
125
126 3 preset_data.baro_preset.baro_pres = calc_baro_pres;
127 3 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 2 times.
 3 if ( calc_acc_x < 0.0f )
133 {
134 1 sensor_set_mount_orientation( MOUNT_ORIENTATION_IMU_INVERTED );
135 }
136
137 /* Initialize sensor */
138 3 sensor_init( &preset_data );
139
140 #ifdef DEBUG
141 1 char sensor_dbg_msg[128];
142 1 uint32_t tdelta = HAL_GetTick() - start_time;
143 1 debug_ignore_emulator_warnings_start();
144 1 size_t msg_len = snprintf(sensor_dbg_msg, 128, "Calibration Finished. Ttotal: %ums, per-sample: %fms.",
145 1 tdelta, (float)tdelta / samples );
146 1 debug_ignore_emulator_warnings_stop();
147 1 debug_log(sensor_dbg_msg, msg_len, LOG_LVL_INFO);
148 1 MOUNT_ORIENTATION orientation = sensor_get_mount_orientation();
149 1 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 1 debug_log(sensor_dbg_msg, msg_len, LOG_LVL_INFO);
152 #endif
153
154 3 }
155
156
157 /*******************************************************************************
158 * END OF FILE *
159 *******************************************************************************/
160