mag calibration add notifications to rotate

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);

src/modules/commander/mag_calibration.cpp

@@ -55,11 +55,12 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
-#include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_mag.h>
+#include <drivers/drv_tone_alarm.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
+#include <uORB/topics/sensor_combined.h>
 
 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) /
+	for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) / sizeof(worker_data.side_data_collected[0])); i++) {
-				  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,
+								     cur_mag, (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
-								     (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,
+								     cur_mag, (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
-								     (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
 #endif
 						usleep(200000);
 					}