GCC Code Coverage Report

Directory: ../../
Coverage: low: ≥ 0% medium: ≥ 75.0% high: ≥ 90.0%
Coverage Exec / Excl / Total
Lines: 0.0% 0 / 0 / 71
Functions: 0.0% 0 / 0 / 1
Branches: 0.0% 0 / 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 ✗ void sensorCalibrationSWCON(){
54 ✗ 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 ✗ uint16_t samples = preset_data.config_settings.sensor_calibration_samples;
59 ✗ SENSOR_STATUS sensor_status = SENSOR_OK;
60 ✗ (void)sensor_status;
61
62 /* Make sure this is consistent at calib, then flip after if we need to */
63 ✗ sensor_set_mount_orientation( MOUNT_ORIENTATION_IMU_NORMAL );
64
65 ✗ preset_data.imu_offset.accel_x = 0.00;
66 ✗ preset_data.imu_offset.accel_y = 0.00;
67 ✗ preset_data.imu_offset.accel_z = 0.00;
68 ✗ preset_data.imu_offset.gyro_x = 0.00;
69 ✗ preset_data.imu_offset.gyro_y = 0.00;
70 ✗ preset_data.imu_offset.gyro_z = 0.00;
71
72 ✗ imu_offset.accel_x = 0.00;
73 ✗ imu_offset.accel_y = 0.00;
74 ✗ imu_offset.accel_z = 0.00;
75 ✗ imu_offset.gyro_x = 0.00;
76 ✗ imu_offset.gyro_y = 0.00;
77 ✗ imu_offset.gyro_z = 0.00;
78
79 ✗ baro_preset.baro_pres = 0.00;
80 ✗ baro_preset.baro_temp = 0.00;
81
82 ✗ float calc_acc_x = 0.00;
83 ✗ float calc_acc_y = 0.00;
84 ✗ float calc_acc_z = 0.00;
85 ✗ float calc_gyro_x = 0.00;
86 ✗ float calc_gyro_y = 0.00;
87 ✗ float calc_gyro_z = 0.00;
88
89 ✗ float calc_baro_pres = 0.00;
90 ✗ float calc_baro_temp = 0.00;
91
92 ✗ for (int i = 0; i < samples; i++){
93 ✗ sensor_status = sensor_dump( &sensor_data );
94 ✗ debug_assert( sensor_status == SENSOR_OK, ERROR_SENSOR_CMD_ERROR );
95 ✗ calc_acc_x = calc_acc_x + sensor_data.imu_converted.accel_x;
96 ✗ calc_acc_y = calc_acc_y + sensor_data.imu_converted.accel_y;
97 ✗ calc_acc_z = calc_acc_z + sensor_data.imu_converted.accel_z;
98 ✗ calc_gyro_x = calc_gyro_x + sensor_data.imu_converted.gyro_x;
99 ✗ calc_gyro_y = calc_gyro_y + sensor_data.imu_converted.gyro_y;
100 ✗ calc_gyro_z = calc_gyro_z + sensor_data.imu_converted.gyro_z;
101 ✗ calc_baro_pres = calc_baro_pres + sensor_data.baro_pressure;
102 ✗ calc_baro_temp = calc_baro_temp + sensor_data.baro_temp;
103 }
104
105 ✗ calc_acc_x = calc_acc_x / ( samples );
106 ✗ calc_acc_y = calc_acc_y / ( samples );
107 ✗ calc_acc_z = calc_acc_z / ( samples );
108
109 ✗ calc_gyro_x = calc_gyro_x / ( samples );
110 ✗ calc_gyro_y = calc_gyro_y / ( samples );
111 ✗ calc_gyro_z = calc_gyro_z / ( samples );
112
113 ✗ debug_assert( calc_acc_x != 0.0f, ERROR_SENSOR_CMD_ERROR );
114
115 ✗ calc_baro_pres = calc_baro_pres / ( samples );
116 ✗ calc_baro_temp = calc_baro_temp / ( samples );
117
118 ✗ preset_data.imu_offset.accel_x = calc_acc_x;
119 ✗ preset_data.imu_offset.accel_y = calc_acc_y;
120 ✗ preset_data.imu_offset.accel_z = calc_acc_z;
121
122 ✗ preset_data.imu_offset.gyro_x = calc_gyro_x;
123 ✗ preset_data.imu_offset.gyro_y = calc_gyro_y;
124 ✗ preset_data.imu_offset.gyro_z = calc_gyro_z;
125
126 ✗ preset_data.baro_preset.baro_pres = calc_baro_pres;
127 ✗ 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 ✗ if ( calc_acc_x < 0.0f )
133 {
134 ✗ sensor_set_mount_orientation( MOUNT_ORIENTATION_IMU_INVERTED );
135 }
136
137 /* Initialize sensor */
138 ✗ 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 ✗ }
155
156
157 /*******************************************************************************
158 * END OF FILE *
159 *******************************************************************************/
160