From f396224d41f4a937653d16d22d8f5500f52de033 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 8 Jul 2017 14:02:23 -0400 Subject: [PATCH] mag calibration add notifications to rotate --- .../commander/calibration_routines.cpp | 3 ++ src/modules/commander/mag_calibration.cpp | 35 ++++++++++--------- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 5afe076cf2..1b7795d21e 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -759,6 +759,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, /* inform user about already handled side */ if (side_data_collected[orient]) { orientation_failures++; + set_tune(TONE_NOTIFY_NEGATIVE_TUNE); calibration_log_info(mavlink_log_pub, "[cal] %s side already completed", detect_orientation_str(orient)); usleep(20000); continue; @@ -784,8 +785,10 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, // Note that this side is complete side_data_collected[orient] = true; + // output neutral tune set_tune(TONE_NOTIFY_NEUTRAL_TUNE); + // temporary priority boost for the white blinking led to come trough rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST, 3, 1); usleep(200000); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 1923b75f93..062c56c1ad 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -55,11 +55,12 @@ #include #include #include -#include #include +#include #include #include #include +#include static const char *sensor_name = "mag"; static constexpr unsigned max_mags = 4; @@ -116,17 +117,17 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) // reset the learned EKF mag in-flight bias offsets which have been learned for the previous // sensor calibration and will be invalidated by a new sensor calibration (void)sprintf(str, "EKF2_MAGBIAS_X"); - result = param_set(param_find(str), &mscale_null.x_offset); + result = param_set_no_notification(param_find(str), &mscale_null.x_offset); if (result != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "EKF2_MAGBIAS_Y"); - result = param_set(param_find(str), &mscale_null.y_offset); + result = param_set_no_notification(param_find(str), &mscale_null.y_offset); if (result != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "EKF2_MAGBIAS_Z"); - result = param_set(param_find(str), &mscale_null.z_offset); + result = param_set_no_notification(param_find(str), &mscale_null.z_offset); if (result != PX4_OK) { PX4_ERR("unable to reset %s", str); } @@ -150,42 +151,42 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) #else (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); - result = param_set(param_find(str), &mscale_null.x_offset); + result = param_set_no_notification(param_find(str), &mscale_null.x_offset); if (result != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); - result = param_set(param_find(str), &mscale_null.y_offset); + result = param_set_no_notification(param_find(str), &mscale_null.y_offset); if (result != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); - result = param_set(param_find(str), &mscale_null.z_offset); + result = param_set_no_notification(param_find(str), &mscale_null.z_offset); if (result != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); - result = param_set(param_find(str), &mscale_null.x_scale); + result = param_set_no_notification(param_find(str), &mscale_null.x_scale); if (result != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); - result = param_set(param_find(str), &mscale_null.y_scale); + result = param_set_no_notification(param_find(str), &mscale_null.y_scale); if (result != PX4_OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); - result = param_set(param_find(str), &mscale_null.z_scale); + result = param_set_no_notification(param_find(str), &mscale_null.z_scale); if (result != PX4_OK) { PX4_ERR("unable to reset %s", str); @@ -193,6 +194,8 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) #endif + param_notify_changes(); + #ifdef __PX4_NUTTX // Attempt to open mag (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); @@ -340,6 +343,9 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta mag_worker_data_t *worker_data = (mag_worker_data_t *)(data); + // notify user to start rotating + set_tune(TONE_SINGLE_BEEP_TUNE); + calibration_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle around the detected orientation"); calibration_log_info(worker_data->mavlink_log_pub, "[cal] Continue rotation for %s %u s", detect_orientation_str(orientation), worker_data->calibration_interval_perside_seconds); @@ -530,8 +536,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) calibration_sides = 0; - for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) / - sizeof(worker_data.side_data_collected[0])); i++) { + for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) / sizeof(worker_data.side_data_collected[0])); i++) { if ((cal_mask & (1 << i)) > 0) { // mark as missing @@ -869,12 +874,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) } else { calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga", - cur_mag, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); + cur_mag, (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); #ifndef __PX4_QURT calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f", - cur_mag, - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); + cur_mag, (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); #endif usleep(200000); }