fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 17:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: accel cal rotate offsets and scales from body frame back into sensor frame before saving (#25626)

Browse Source 
- fixes https://github.com/PX4/PX4-Autopilot/issues/25606
This commit is contained in:
Jacob Dahl
2025-09-25 14:12:15 -04:00
committed by GitHub
parent a6f8b00a6a
commit 35f882cd3c
1 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+10 -3
View File
@@ -366,17 +366,24 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
const Matrix3f accel_T = mat_A.I() * CONSTANTS_ONE_G; const Matrix3f accel_T = mat_A.I() * CONSTANTS_ONE_G;
// update calibration // update calibration
worker_data.calibration[i].set_offset(offset); const Dcmf &R = worker_data.calibration[i].rotation();
worker_data.calibration[i].set_scale(accel_T.diag());
const Vector3f sensor_frame_offsets{R.transpose() *offset};
const Matrix3f sensor_frame_scale{R.transpose() *accel_T * R};
worker_data.calibration[i].set_offset(sensor_frame_offsets);
worker_data.calibration[i].set_scale(sensor_frame_scale.diag());
#if defined(DEBUD_BUILD) #if defined(DEBUD_BUILD)
PX4_INFO("accel %d: offset", i); PX4_INFO("accel %d: offset", i);
offset.print(); sensor_frame_offsets.print();
PX4_INFO("accel %d: mat_A", i); PX4_INFO("accel %d: mat_A", i);
mat_A.print(); mat_A.print();
PX4_INFO("accel %d: accel_T", i); PX4_INFO("accel %d: accel_T", i);
accel_T.print(); accel_T.print();
PX4_INFO("accel %d: scale matrix", i);
sensor_frame_scale.print();
#endif // DEBUD_BUILD #endif // DEBUD_BUILD
worker_data.calibration[i].PrintStatus(); worker_data.calibration[i].PrintStatus();