fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 10:12:15 -08:00 committed by GitHub
parent a6f8b00a6a
commit 35f882cd3c
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/commander/accelerometer_calibration.cpp
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;
// update calibration
worker_data.calibration[i].set_offset(offset);
worker_data.calibration[i].set_scale(accel_T.diag());
const Dcmf &R = worker_data.calibration[i].rotation();
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)
PX4_INFO("accel %d: offset", i);
offset.print();
sensor_frame_offsets.print();
PX4_INFO("accel %d: mat_A", i);
mat_A.print();
PX4_INFO("accel %d: accel_T", i);
accel_T.print();
PX4_INFO("accel %d: scale matrix", i);
sensor_frame_scale.print();
#endif // DEBUD_BUILD
worker_data.calibration[i].PrintStatus();