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: Add range check to mag cal

Browse Source
This commit is contained in:
Lorenz Meier 2015-12-27 01:13:05 +01:00
parent 4d41f1d6ce
commit 3bb733018b
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/commander/mag_calibration.cpp
View File

@ -73,6 +73,8 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of
static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer
static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take
static constexpr float MAG_MAX_OFFSET_LEN = 0.6f; ///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions.
int32_t device_ids[max_mags];
int device_prio_max = 0;
int32_t device_id_primary = 0;
@ -507,6 +509,13 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
result = calibrate_return_error;
}
if (sqrtf(sphere_x[cur_mag] * sphere_x[cur_mag] +
sphere_y[cur_mag] * sphere_y[cur_mag] + sphere_z[cur_mag] * sphere_z[cur_mag])
> MAG_MAX_OFFSET_LEN) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Excessive offset for mag #%u", cur_mag);
result = calibrate_return_error;
}
}
}
}