diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 819da7aa28..dfaf1d9766 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -69,11 +69,11 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; static constexpr unsigned max_mags = 3; static constexpr float mag_sphere_radius = 0.2f; -static constexpr unsigned int calibration_sides = 3; ///< The total number of sides +static unsigned int calibration_sides = 6; ///< The total number of sides 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.75f; ///< 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 = 1.3f; ///< The maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, so an offset larger than ~1.3 Ga means the mag will saturate in some directions. int32_t device_ids[max_mags]; bool internal[max_mags]; @@ -447,40 +447,32 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; - // Collect: Right-side up, Left Side, Nose down - worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false; - worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false; - worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false; - worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; - worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; - worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true; - - calibration_log_info(mavlink_log_pub, - "[cal] %s side done, rotate to a different side", - detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN)); - usleep(100000); - calibration_log_info(mavlink_log_pub, - "[cal] %s side done, rotate to a different side", - detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN)); - usleep(100000); - calibration_log_info(mavlink_log_pub, - "[cal] %s side done, rotate to a different side", - detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN)); - usleep(100000); - calibration_log_info(mavlink_log_pub, - "[cal] %s side done, rotate to a different side", - detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN)); - usleep(100000); - calibration_log_info(mavlink_log_pub, - "[cal] %s side done, rotate to a different side", - detect_orientation_str(DETECT_ORIENTATION_RIGHT)); - usleep(100000); - calibration_log_info(mavlink_log_pub, - "[cal] %s side done, rotate to a different side", - detect_orientation_str(DETECT_ORIENTATION_RIGHT)); - usleep(100000); + // Collect: As defined by configuration + // start with a full mask, all six bits set + uint32_t cal_mask = (1 << 6) - 1; + param_get(param_find("CAL_MAG_SIDES"), &cal_mask); - for (size_t cur_mag=0; cur_mag 0) { + // mark as missing + worker_data.side_data_collected[i] = false; + calibration_sides++; + } else { + // mark as completed from the beginning + worker_data.side_data_collected[i] = true; + + calibration_log_info(mavlink_log_pub, + "[cal] %s side done, rotate to a different side", + detect_orientation_str(static_cast(i))); + usleep(100000); + } + } + + for (size_t cur_mag = 0; cur_mag MAG_MAX_OFFSET_LEN || fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN || fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) { - calibration_log_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external"); - calibration_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag], + calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external"); + calibration_log_info(mavlink_log_pub, "Offsets: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)sphere_x[cur_mag], (double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag); result = calibrate_return_ok; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index cce4669a27..5454f34586 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -687,6 +687,25 @@ PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0); */ PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0); +/** + * Bitfield selecting mag sides for calibration + * + * DETECT_ORIENTATION_TAIL_DOWN = 1 + * DETECT_ORIENTATION_NOSE_DOWN = 2 + * DETECT_ORIENTATION_LEFT = 4 + * DETECT_ORIENTATION_RIGHT = 8 + * DETECT_ORIENTATION_UPSIDE_DOWN = 16 + * DETECT_ORIENTATION_RIGHTSIDE_UP = 32 + * + * @min 34 + * @max 63 + * @value 34 Two side calibration + * @value 38 Three side calibration + * @value 63 Six side calibration + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG_SIDES, 63); + /** * Primary baro ID * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ddb25ece7f..a555884db9 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -672,6 +672,7 @@ Sensors::Sensors() : (void)param_find("CAL_MAG0_ROT"); (void)param_find("CAL_MAG1_ROT"); (void)param_find("CAL_MAG2_ROT"); + (void)param_find("CAL_MAG_SIDES"); (void)param_find("SYS_PARAM_VER"); (void)param_find("SYS_AUTOSTART"); (void)param_find("SYS_AUTOCONFIG");