dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: RRelax mag cal requirements and report offsets on failure

This commit is contained in:
Lorenz Meier 2016-02-25 01:49:59 +01:00
parent c5b0650c23
commit 4f666cebe7
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -73,7 +73,7 @@ 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.9f; ///< 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.
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;
@ -520,14 +520,16 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
&sphere_radius[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit 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);
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Excessive offset mag, #%u", cur_mag);
mavlink_and_console_log_critical(mavlink_fd, "[cal] Offsets: %8.4f,%8.4f,%8.4f", sphere_x[cur_mag],
sphere_y[cur_mag], sphere_z[cur_mag]);
result = calibrate_return_error;
}
}