fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 16:57:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: mag calibration auto rotation simplify applying calibration and rotation

Browse Source
This commit is contained in:
Daniel Agar
2020-08-18 11:18:49 -04:00
parent 1a6eb7859c
commit 07d727a4a8
1 changed files with 19 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/mag_calibration.cpp
+19 -21
View File
@@ -670,21 +670,32 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
// only proceed if there's a valid internal
if (internal_index >= 0) {
const Dcmf board_rotation = calibration::GetBoardRotation();
// apply new calibrations to all raw sensor data before comparison
for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
if (worker_data.calibration[cur_mag].device_id() != 0) {
float scale_data[9] {
diag[cur_mag](0), offdiag[cur_mag](0), offdiag[cur_mag](1),
offdiag[cur_mag](0), diag[cur_mag](1), offdiag[cur_mag](2),
offdiag[cur_mag](1), offdiag[cur_mag](2), diag[cur_mag](2)
};
const Matrix3f scale{scale_data};
const Vector3f &offset{sphere[cur_mag]};
for (unsigned i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
const Vector3f sample{worker_data.x[cur_mag][i], worker_data.y[cur_mag][i], worker_data.z[cur_mag][i]};
float scale_data[9] {
diag[cur_mag](0), offdiag[cur_mag](0), offdiag[cur_mag](1),
offdiag[cur_mag](0), diag[cur_mag](1), offdiag[cur_mag](2),
offdiag[cur_mag](1), offdiag[cur_mag](2), diag[cur_mag](2)
};
// apply calibration
const Vector3f m{Matrix3f{scale_data} *(sample - sphere[cur_mag])};
Vector3f m{scale *(sample - offset)};
if (!worker_data.calibration[cur_mag].external()) {
// rotate internal mag data to board
m = board_rotation * m;
}
// store back in worker_data
worker_data.x[cur_mag][i] = m(0);
@@ -694,19 +705,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
}
}
// rotate internal mag data to board
const Dcmf board_rotation = calibration::GetBoardRotation();
for (unsigned i = 0; i < worker_data.calibration_counter_total[internal_index]; i++) {
const Vector3f m = board_rotation * Vector3f{worker_data.x[internal_index][i],
worker_data.y[internal_index][i], worker_data.z[internal_index][i]};
worker_data.x[internal_index][i] = m(0);
worker_data.y[internal_index][i] = m(1);
worker_data.z[internal_index][i] = m(2);
}
// external mags try all rotations and compute mean square error (MSE) compared with first internal mag
for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
if (worker_data.calibration[cur_mag].external() && (worker_data.calibration[cur_mag].device_id() != 0)) {
@@ -735,7 +734,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
float diff_sum = 0.f;
for (int i = 0; i < last_sample_index; i++) {
float x = worker_data.x[cur_mag][i];
float y = worker_data.y[cur_mag][i];
float z = worker_data.z[cur_mag][i];