Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : IMU temperature calibration handling
17 : */
18 :
19 : #define AP_INLINE_VECTOR_OPS
20 :
21 : #include "AP_InertialSensor_tempcal.h"
22 : #include "AP_InertialSensor_config.h"
23 :
24 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
25 : #include <GCS_MAVLink/GCS.h>
26 : #include <AP_Logger/AP_Logger.h>
27 : #include <AP_Common/ExpandingString.h>
28 : #include <AP_Notify/AP_Notify.h>
29 : #include "AP_InertialSensor.h"
30 :
31 : // this scale factor ensures params are easy to work with in GUI parameter editors
32 : #define SCALE_FACTOR 1.0e6
33 : #define INV_SCALE_FACTOR 1.0e-6
34 : #define TEMP_RANGE_MIN 10
35 :
36 : // timeout calibration after 10 minutes, if no temperature rise
37 : #define CAL_TIMEOUT_MS (600U*1000U)
38 :
39 : /*
40 : we use a fixed reference temperature of 35C. This has the advantage
41 : that we don't need to know the final temperature when doing an
42 : online calibration which allows us to have a calibration timeout
43 : */
44 : #define TEMP_REFERENCE 35.0
45 :
46 : extern const AP_HAL::HAL& hal;
47 :
48 : // temperature calibration parameters, per IMU
49 : const AP_Param::GroupInfo AP_InertialSensor_TCal::var_info[] = {
50 :
51 : // @Param: ENABLE
52 : // @DisplayName: Enable temperature calibration
53 : // @Description: Enable the use of temperature calibration parameters for this IMU. For automatic learning set to 2 and also set the INS_TCALn_TMAX to the target temperature, then reboot
54 : // @Values: 0:Disabled,1:Enabled,2:LearnCalibration
55 : // @User: Advanced
56 : // @RebootRequired: True
57 : AP_GROUPINFO_FLAGS("ENABLE", 1, AP_InertialSensor_TCal, enable, float(Enable::Disabled), AP_PARAM_FLAG_ENABLE),
58 :
59 : // @Param: TMIN
60 : // @DisplayName: Temperature calibration min
61 : // @Description: The minimum temperature that the calibration is valid for
62 : // @Range: -70 80
63 : // @Units: degC
64 : // @User: Advanced
65 : // @Calibration: 1
66 : AP_GROUPINFO("TMIN", 2, AP_InertialSensor_TCal, temp_min, 0),
67 :
68 : // @Param: TMAX
69 : // @DisplayName: Temperature calibration max
70 : // @Description: The maximum temperature that the calibration is valid for. This must be at least 10 degrees above TMIN for calibration
71 : // @Range: -70 80
72 : // @Units: degC
73 : // @User: Advanced
74 : // @Calibration: 1
75 : AP_GROUPINFO("TMAX", 3, AP_InertialSensor_TCal, temp_max, 70),
76 :
77 : // @Param: ACC1_X
78 : // @DisplayName: Accelerometer 1st order temperature coefficient X axis
79 : // @Description: This is the 1st order temperature coefficient from a temperature calibration
80 : // @User: Advanced
81 : // @Calibration: 1
82 :
83 : // @Param: ACC1_Y
84 : // @DisplayName: Accelerometer 1st order temperature coefficient Y axis
85 : // @Description: This is the 1st order temperature coefficient from a temperature calibration
86 : // @User: Advanced
87 : // @Calibration: 1
88 :
89 : // @Param: ACC1_Z
90 : // @DisplayName: Accelerometer 1st order temperature coefficient Z axis
91 : // @Description: This is the 1st order temperature coefficient from a temperature calibration
92 : // @User: Advanced
93 : // @Calibration: 1
94 :
95 : AP_GROUPINFO("ACC1", 4, AP_InertialSensor_TCal, accel_coeff[0], 0),
96 :
97 : // @Param: ACC2_X
98 : // @DisplayName: Accelerometer 2nd order temperature coefficient X axis
99 : // @Description: This is the 2nd order temperature coefficient from a temperature calibration
100 : // @User: Advanced
101 : // @Calibration: 1
102 :
103 : // @Param: ACC2_Y
104 : // @DisplayName: Accelerometer 2nd order temperature coefficient Y axis
105 : // @Description: This is the 2nd order temperature coefficient from a temperature calibration
106 : // @User: Advanced
107 : // @Calibration: 1
108 :
109 : // @Param: ACC2_Z
110 : // @DisplayName: Accelerometer 2nd order temperature coefficient Z axis
111 : // @Description: This is the 2nd order temperature coefficient from a temperature calibration
112 : // @User: Advanced
113 : // @Calibration: 1
114 :
115 : AP_GROUPINFO("ACC2", 5, AP_InertialSensor_TCal, accel_coeff[1], 0),
116 :
117 : // @Param: ACC3_X
118 : // @DisplayName: Accelerometer 3rd order temperature coefficient X axis
119 : // @Description: This is the 3rd order temperature coefficient from a temperature calibration
120 : // @User: Advanced
121 : // @Calibration: 1
122 :
123 : // @Param: ACC3_Y
124 : // @DisplayName: Accelerometer 3rd order temperature coefficient Y axis
125 : // @Description: This is the 3rd order temperature coefficient from a temperature calibration
126 : // @User: Advanced
127 : // @Calibration: 1
128 :
129 : // @Param: ACC3_Z
130 : // @DisplayName: Accelerometer 3rd order temperature coefficient Z axis
131 : // @Description: This is the 3rd order temperature coefficient from a temperature calibration
132 : // @User: Advanced
133 : // @Calibration: 1
134 :
135 : AP_GROUPINFO("ACC3", 6, AP_InertialSensor_TCal, accel_coeff[2], 0),
136 :
137 : // @Param: GYR1_X
138 : // @DisplayName: Gyroscope 1st order temperature coefficient X axis
139 : // @Description: This is the 1st order temperature coefficient from a temperature calibration
140 : // @User: Advanced
141 : // @Calibration: 1
142 :
143 : // @Param: GYR1_Y
144 : // @DisplayName: Gyroscope 1st order temperature coefficient Y axis
145 : // @Description: This is the 1st order temperature coefficient from a temperature calibration
146 : // @User: Advanced
147 : // @Calibration: 1
148 :
149 : // @Param: GYR1_Z
150 : // @DisplayName: Gyroscope 1st order temperature coefficient Z axis
151 : // @Description: This is the 1st order temperature coefficient from a temperature calibration
152 : // @User: Advanced
153 : // @Calibration: 1
154 :
155 : AP_GROUPINFO("GYR1", 7, AP_InertialSensor_TCal, gyro_coeff[0], 0),
156 :
157 : // @Param: GYR2_X
158 : // @DisplayName: Gyroscope 2nd order temperature coefficient X axis
159 : // @Description: This is the 2nd order temperature coefficient from a temperature calibration
160 : // @User: Advanced
161 : // @Calibration: 1
162 :
163 : // @Param: GYR2_Y
164 : // @DisplayName: Gyroscope 2nd order temperature coefficient Y axis
165 : // @Description: This is the 2nd order temperature coefficient from a temperature calibration
166 : // @User: Advanced
167 : // @Calibration: 1
168 :
169 : // @Param: GYR2_Z
170 : // @DisplayName: Gyroscope 2nd order temperature coefficient Z axis
171 : // @Description: This is the 2nd order temperature coefficient from a temperature calibration
172 : // @User: Advanced
173 : // @Calibration: 1
174 :
175 : AP_GROUPINFO("GYR2", 8, AP_InertialSensor_TCal, gyro_coeff[1], 0),
176 :
177 : // @Param: GYR3_X
178 : // @DisplayName: Gyroscope 3rd order temperature coefficient X axis
179 : // @Description: This is the 3rd order temperature coefficient from a temperature calibration
180 : // @User: Advanced
181 : // @Calibration: 1
182 :
183 : // @Param: GYR3_Y
184 : // @DisplayName: Gyroscope 3rd order temperature coefficient Y axis
185 : // @Description: This is the 3rd order temperature coefficient from a temperature calibration
186 : // @User: Advanced
187 : // @Calibration: 1
188 :
189 : // @Param: GYR3_Z
190 : // @DisplayName: Gyroscope 3rd order temperature coefficient Z axis
191 : // @Description: This is the 3rd order temperature coefficient from a temperature calibration
192 : // @User: Advanced
193 : // @Calibration: 1
194 :
195 : AP_GROUPINFO("GYR3", 9, AP_InertialSensor_TCal, gyro_coeff[2], 0),
196 :
197 : AP_GROUPEND
198 : };
199 :
200 :
201 : /*
202 : evaluate a 3rd order polynomial (without the constant term) given a set of coefficients
203 : */
204 2057024661 : Vector3f AP_InertialSensor_TCal::polynomial_eval(float tdiff, const AP_Vector3f coeff[3]) const
205 : {
206 : // evaluate order 3 polynomial
207 2057024661 : const Vector3f *c = (Vector3f *)&coeff[0];
208 2057024661 : return (c[0] + (c[1] + c[2]*tdiff)*tdiff)*tdiff*INV_SCALE_FACTOR;
209 : }
210 :
211 : /*
212 : correct a single sensor for the current temperature
213 : */
214 543805142 : void AP_InertialSensor_TCal::correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const
215 : {
216 543805142 : if (enable != Enable::Enabled) {
217 542868725 : return;
218 : }
219 936360 : temperature = constrain_float(temperature, temp_min, temp_max);
220 936363 : cal_temp = constrain_float(cal_temp, temp_min, temp_max);
221 :
222 :
223 : // get the polynomial correction for the difference between the
224 : // current temperature and the mid temperature
225 936363 : v -= polynomial_eval(temperature - TEMP_REFERENCE, coeff);
226 :
227 : // we need to add the correction for the temperature
228 : // difference between the TREF, which is the reference used for
229 : // the calibration process, and the cal_temp, which is the
230 : // temperature that the offsets and scale factors was setup for
231 936363 : v += polynomial_eval(cal_temp - TEMP_REFERENCE, coeff);
232 : }
233 :
234 275561165 : void AP_InertialSensor_TCal::correct_accel(float temperature, float cal_temp, Vector3f &accel) const
235 : {
236 275561165 : correct_sensor(temperature, cal_temp, accel_coeff, accel);
237 275561151 : }
238 :
239 268244226 : void AP_InertialSensor_TCal::correct_gyro(float temperature, float cal_temp, Vector3f &gyro) const
240 : {
241 268244226 : correct_sensor(temperature, cal_temp, gyro_coeff, gyro);
242 268244219 : }
243 :
244 : /*
245 : for SITL we don't apply the temperature limits and use mid-point as
246 : reference. This makes the SITL data independent of TEMP_REFERENCE
247 : and prevents an abrupt change at the endpoints
248 : */
249 728655862 : void AP_InertialSensor_TCal::sitl_apply_accel(float temperature, Vector3f &accel) const
250 : {
251 728655862 : const float tmid = 0.5*(temp_max+temp_min);
252 728655862 : accel += polynomial_eval(temperature - tmid, accel_coeff);
253 728655862 : }
254 :
255 1326496073 : void AP_InertialSensor_TCal::sitl_apply_gyro(float temperature, Vector3f &gyro) const
256 : {
257 1326496073 : const float tmid = 0.5*(temp_max+temp_min);
258 1326496073 : gyro += polynomial_eval(temperature - tmid, gyro_coeff);
259 1326496073 : }
260 :
261 2 : AP_InertialSensor_TCal::Learn::Learn(AP_InertialSensor_TCal &_tcal, float _start_temp) :
262 2 : start_temp(_start_temp),
263 6 : tcal(_tcal)
264 : {
265 2 : reset(_start_temp);
266 2 : }
267 :
268 : /*
269 : update polyfit with new sample
270 : */
271 748345 : void AP_InertialSensor_TCal::Learn::add_sample(const Vector3f &sample, float temperature, struct LearnState &st)
272 : {
273 748345 : temperature = st.temp_filter.apply(temperature);
274 :
275 748345 : st.sum += sample;
276 748345 : st.sum_count++;
277 :
278 748345 : uint32_t now = AP_HAL::millis();
279 :
280 748345 : if (st.sum_count < 100 ||
281 717853 : temperature - st.last_temp < 0.5) {
282 : // check for timeout
283 748039 : if (st.last_sample_ms != 0 &&
284 743067 : temperature - start_temp >= TEMP_RANGE_MIN &&
285 670652 : now - st.last_sample_ms > CAL_TIMEOUT_MS) {
286 : // we have timed out, finish up now
287 0 : finish_calibration(st.last_temp);
288 : }
289 : // wait for more data
290 748039 : return;
291 : }
292 :
293 306 : st.sum /= st.sum_count;
294 :
295 306 : const uint8_t si = &st - &state[0];
296 :
297 306 : const float T = (temperature + st.last_temp) * 0.5;
298 :
299 306 : if (si == 0) {
300 : // we use the first accel sample as the zero baseline
301 154 : if (accel_start.is_zero()) {
302 2 : accel_start = st.sum;
303 2 : start_temp = T;
304 : }
305 154 : st.sum -= accel_start;
306 : }
307 :
308 306 : const float tdiff = T - TEMP_REFERENCE;
309 : #if HAL_LOGGING_ENABLED
310 : // @LoggerMessage: TCLR
311 : // @Description: Temperature Calibration Information
312 : // @Field: TimeUS: Time since system startup
313 : // @Field: I: temperature calibration instance number
314 : // @Field: SType: sensor type (0==accel, 1==gyro)
315 : // @Field: Temp: current temperature
316 : // @Field: X: x-axis sample sum
317 : // @Field: Y: y-axis sample sum
318 : // @Field: Z: z-axis sample sum
319 : // @Field: NSamp: sample count
320 612 : AP::logger().Write("TCLR", "TimeUS,I,SType,Temp,X,Y,Z,NSamp",
321 : "s#------",
322 : "F000000-",
323 : "QBBffffI",
324 : AP_HAL::micros64(),
325 306 : instance(),
326 : si,
327 : T,
328 306 : st.sum.x, st.sum.y, st.sum.z,
329 : st.sum_count);
330 : #endif
331 :
332 :
333 306 : st.pfit.update(tdiff, st.sum);
334 :
335 306 : st.sum.zero();
336 306 : st.sum_count = 0;
337 306 : st.last_temp = temperature;
338 306 : st.last_sample_ms = now;
339 :
340 306 : if (temperature - start_temp >= TEMP_RANGE_MIN) {
341 226 : if (temperature >= start_tmax) {
342 : // we've reached the target temperature
343 2 : finish_calibration(temperature);
344 224 : } else if (now - last_save_ms > 15000) {
345 : // save partial calibration, so if user stops the cal part
346 : // way then they still have a useful calibration
347 22 : last_save_ms = now;
348 22 : save_calibration(st.last_temp);
349 : }
350 : }
351 : }
352 :
353 : /*
354 : update accel temperature compensation learning
355 : */
356 381279 : void AP_InertialSensor_TCal::update_accel_learning(const Vector3f &accel, float temperature)
357 : {
358 381279 : if (enable != Enable::LearnCalibration) {
359 66 : return;
360 : }
361 381213 : if (learn == nullptr && hal.scheduler->is_system_initialized()) {
362 2 : learn = NEW_NOTHROW Learn(*this, temperature);
363 2 : if (learn) {
364 2 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: started calibration t=%.1fC tmax=%.1fC",
365 : instance()+1,
366 : temperature, learn->start_tmax);
367 2 : AP_Notify::events.initiated_temp_cal = 1;
368 : }
369 : }
370 381213 : if (learn != nullptr) {
371 378330 : AP_Notify::flags.temp_cal_running = true;
372 378330 : learn->add_sample(accel, temperature, learn->state[0]);
373 : }
374 : }
375 :
376 : /*
377 : update gyro temperature compensation learning
378 : */
379 372903 : void AP_InertialSensor_TCal::update_gyro_learning(const Vector3f &gyro, float temperature)
380 : {
381 372903 : if (enable != Enable::LearnCalibration) {
382 68 : return;
383 : }
384 372835 : if (learn != nullptr) {
385 370015 : learn->add_sample(gyro, temperature, learn->state[1]);
386 : }
387 : }
388 :
389 : /*
390 : reset calibration
391 : */
392 2 : void AP_InertialSensor_TCal::Learn::reset(float temperature)
393 : {
394 2 : memset((void*)&state[0], 0, sizeof(state));
395 2 : start_tmax = tcal.temp_max;
396 2 : accel_start.zero();
397 6 : for (uint8_t i=0; i<ARRAY_SIZE(state); i++) {
398 4 : state[i].temp_filter.set_cutoff_frequency(1000, 0.5);
399 4 : state[i].temp_filter.reset(temperature);
400 4 : state[i].last_temp = temperature;
401 : }
402 2 : }
403 :
404 : /*
405 : finish and save calibration
406 : */
407 2 : void AP_InertialSensor_TCal::Learn::finish_calibration(float temperature)
408 : {
409 2 : if (!save_calibration(temperature)) {
410 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: failed fit", instance()+1);
411 0 : AP_Notify::events.temp_cal_failed = 1;
412 0 : tcal.enable.set_and_save_ifchanged(int8_t(AP_InertialSensor_TCal::Enable::Disabled));
413 0 : return;
414 : }
415 2 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: completed calibration tmin=%.1f tmax=%.1f",
416 : instance()+1,
417 : tcal.temp_min.get(), tcal.temp_max.get());
418 2 : tcal.enable.set_and_save_ifchanged(int8_t(AP_InertialSensor_TCal::Enable::Enabled));
419 : }
420 :
421 : /*
422 : save calibration state
423 : */
424 24 : bool AP_InertialSensor_TCal::Learn::save_calibration(float temperature)
425 : {
426 24 : Vector3f coefficients[3];
427 :
428 24 : Vector3f p[4];
429 24 : if (!state[0].pfit.get_polynomial(p)) {
430 0 : return false;
431 : }
432 96 : for (uint8_t k=0; k<3; k++) {
433 72 : coefficients[k] = p[2-k] * SCALE_FACTOR;
434 : }
435 :
436 96 : for (uint8_t k=0; k<3; k++) {
437 72 : tcal.accel_coeff[k].set_and_save_ifchanged(coefficients[k]);
438 : }
439 :
440 24 : if (!state[1].pfit.get_polynomial(p)) {
441 0 : return false;
442 : }
443 96 : for (uint8_t k=0; k<3; k++) {
444 72 : coefficients[k] = p[2-k] * SCALE_FACTOR;
445 : }
446 :
447 96 : for (uint8_t k=0; k<3; k++) {
448 72 : tcal.gyro_coeff[k].set_and_save_ifchanged(coefficients[k]);
449 : }
450 24 : tcal.temp_min.set_and_save_ifchanged(start_temp);
451 24 : tcal.temp_max.set_and_save_ifchanged(temperature);
452 24 : return true;
453 : }
454 :
455 310 : uint8_t AP_InertialSensor_TCal::instance(void) const
456 : {
457 310 : return AP::ins().tcal_instance(*this);
458 : }
459 :
460 : /*
461 : get a string representation of parameters for this calibration set
462 : */
463 0 : void AP_InertialSensor_TCal::get_persistent_params(ExpandingString &str) const
464 : {
465 0 : if (enable != AP_InertialSensor_TCal::Enable::Enabled) {
466 0 : return;
467 : }
468 0 : const uint8_t imu = instance()+1;
469 : #if INS_AUX_INSTANCES
470 : if (imu > 3) {
471 : str.printf("INS%u_TCAL_ENABLE=1\n", imu);
472 : str.printf("INS%u_TCAL_TMIN=%.2f\n", imu, temp_min.get());
473 : str.printf("INS%u_TCAL_TMAX=%.2f\n", imu, temp_max.get());
474 : for (uint8_t k=0; k<3; k++) {
475 : const Vector3f &acc = accel_coeff[k].get();
476 : const Vector3f &gyr = gyro_coeff[k].get();
477 : str.printf("INS%u_TCAL_ACC%u_X=%f\n", imu, k+1, acc.x);
478 : str.printf("INS%u_TCAL_ACC%u_Y=%f\n", imu, k+1, acc.y);
479 : str.printf("INS%u_TCAL_ACC%u_Z=%f\n", imu, k+1, acc.z);
480 : str.printf("INS%u_TCAL_GYR%u_X=%f\n", imu, k+1, gyr.x);
481 : str.printf("INS%u_TCAL_GYR%u_Y=%f\n", imu, k+1, gyr.y);
482 : str.printf("INS%u_TCAL_GYR%u_Z=%f\n", imu, k+1, gyr.z);
483 : }
484 : return;
485 : }
486 : #endif
487 0 : str.printf("INS_TCAL%u_ENABLE=1\n", imu);
488 0 : str.printf("INS_TCAL%u_TMIN=%.2f\n", imu, temp_min.get());
489 0 : str.printf("INS_TCAL%u_TMAX=%.2f\n", imu, temp_max.get());
490 0 : for (uint8_t k=0; k<3; k++) {
491 0 : const Vector3f &acc = accel_coeff[k].get();
492 0 : const Vector3f &gyr = gyro_coeff[k].get();
493 0 : str.printf("INS_TCAL%u_ACC%u_X=%f\n", imu, k+1, acc.x);
494 0 : str.printf("INS_TCAL%u_ACC%u_Y=%f\n", imu, k+1, acc.y);
495 0 : str.printf("INS_TCAL%u_ACC%u_Z=%f\n", imu, k+1, acc.z);
496 0 : str.printf("INS_TCAL%u_GYR%u_X=%f\n", imu, k+1, gyr.x);
497 0 : str.printf("INS_TCAL%u_GYR%u_Y=%f\n", imu, k+1, gyr.y);
498 0 : str.printf("INS_TCAL%u_GYR%u_Z=%f\n", imu, k+1, gyr.z);
499 : }
500 : }
501 :
502 : /*
503 : get a string representation of parameters that should be made
504 : persistent across changes of firmware type
505 : */
506 0 : void AP_InertialSensor::get_persistent_params(ExpandingString &str) const
507 : {
508 0 : bool save_options = false;
509 0 : if (uint32_t(tcal_options.get()) & uint32_t(TCalOptions::PERSIST_ACCEL_CAL)) {
510 0 : save_options = true;
511 0 : for (uint8_t i=0; i<(INS_MAX_INSTANCES-INS_AUX_INSTANCES); i++) {
512 0 : const uint8_t imu = i+1;
513 0 : const Vector3f &aoff = _accel_offset(i).get();
514 0 : const Vector3f &ascl = _accel_scale(i).get();
515 0 : char id[2] = "";
516 0 : if (i > 0) {
517 0 : id[0] = '1'+i;
518 : }
519 0 : str.printf("INS_ACC%s_ID=%u\n", id, unsigned(_accel_id(i).get()));
520 0 : str.printf("INS_ACC%sOFFS_X=%f\n", id, aoff.x);
521 0 : str.printf("INS_ACC%sOFFS_Y=%f\n", id, aoff.y);
522 0 : str.printf("INS_ACC%sOFFS_Z=%f\n", id, aoff.z);
523 0 : str.printf("INS_ACC%sSCAL_X=%f\n", id, ascl.x);
524 0 : str.printf("INS_ACC%sSCAL_Y=%f\n", id, ascl.y);
525 0 : str.printf("INS_ACC%sSCAL_Z=%f\n", id, ascl.z);
526 0 : str.printf("INS_ACC%u_CALTEMP=%.2f\n", imu, caltemp_accel(i).get());
527 : }
528 : #if INS_AUX_INSTANCES
529 : for (uint8_t i=0; i<INS_AUX_INSTANCES; i++) {
530 : const uint8_t imu = i+(INS_MAX_INSTANCES-INS_AUX_INSTANCES)+1;
531 : const Vector3f &aoff = params[i]._accel_offset.get();
532 : const Vector3f &ascl = params[i]._accel_scale.get();
533 : str.printf("INS%u_ACC_ID=%u\n", imu, unsigned(params[i]._accel_id.get()));
534 : str.printf("INS%u_ACCOFFS_X=%f\n", imu, aoff.x);
535 : str.printf("INS%u_ACCOFFS_Y=%f\n", imu, aoff.y);
536 : str.printf("INS%u_ACCOFFS_Z=%f\n", imu, aoff.z);
537 : str.printf("INS%u_ACCSCAL_X=%f\n", imu, ascl.x);
538 : str.printf("INS%u_ACCSCAL_Y=%f\n", imu, ascl.y);
539 : str.printf("INS%u_ACCSCAL_Z=%f\n", imu, ascl.z);
540 : str.printf("INS%u_ACC_CALTEMP=%.2f\n", imu, params[i].caltemp_accel.get());
541 : }
542 : #endif
543 : }
544 0 : if (uint32_t(tcal_options.get()) & uint32_t(TCalOptions::PERSIST_TEMP_CAL)) {
545 0 : for (auto &tc : tcal_old_param) {
546 0 : tc.get_persistent_params(str);
547 : }
548 : #if INS_AUX_INSTANCES
549 : for (uint8_t i=0; i<INS_AUX_INSTANCES; i++) {
550 : params[i].tcal.get_persistent_params(str);
551 : }
552 : #endif
553 0 : save_options = true;
554 : }
555 0 : if (save_options) {
556 : /*
557 : we also have to save the TCAL_OPTIONS parameter so that
558 : future flashing of the bootloader doesn't cause an erase
559 : */
560 0 : str.printf("INS_TCAL_OPTIONS=%u\n", unsigned(tcal_options.get()));
561 : }
562 0 : }
563 :
564 :
565 : #endif // HAL_INS_TEMPERATURE_CAL_ENABLE
|
