From 4f666cebe7ab2dd2c70d379b02dc450c391bf7f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Feb 2016 01:49:59 +0100 Subject: [PATCH] Commander: RRelax mag cal requirements and report offsets on failure --- src/modules/commander/mag_calibration.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 64873ceb29..57bb2f7f54 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -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; } }