A lot more on calibration and RC checks. Needs more testing, but no known issues

3 files changed, 28 insertions, 6 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index c2b2e9258..82df7c37d 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -136,6 +136,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 int do_accel_calibration(int mavlink_fd) {
 	/* announce change */
 	mavlink_log_info(mavlink_fd, "accel calibration started");
+	mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
 
 	/* measure and calculate offsets & scales */
 	float accel_offs[3];
@@ -211,17 +212,28 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 	}
 
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+
+	unsigned done_count = 0;
+
 	while (true) {
 		bool done = true;
-		char str[80];
+		char str[60];
 		int str_ptr;
 		str_ptr = sprintf(str, "keep vehicle still:");
+		unsigned old_done_count = done_count;
+		done_count = 0;
 		for (int i = 0; i < 6; i++) {
 			if (!data_collected[i]) {
 				str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
 				done = false;
+			} else {
+				done_count++;
 			}
 		}
+
+		if (old_done_count != done_count)
+			mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
+
 		if (done)
 			break;
 		mavlink_log_info(mavlink_fd, str);
@@ -236,8 +248,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 			continue;
 		}
 
-		sprintf(str, "meas started: %s", orientation_strs[orient]);
-		mavlink_log_info(mavlink_fd, str);
+		// sprintf(str, 
+		mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]);
 		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
 		str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient],
 			(double)accel_ref[orient][0],
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 3654839fb..e3d314881 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -84,6 +84,7 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <systemlib/cpuload.h>
+#include <systemlib/rc_check.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -617,6 +618,8 @@ int commander_thread_main(int argc, char *argv[])
 
 	bool updated = false;
 
+	bool rc_calibration_ok = (OK == rc_calibration_check());
+
 	/* Subscribe to safety topic */
 	int safety_sub = orb_subscribe(ORB_ID(safety));
 	memset(&safety, 0, sizeof(safety));
@@ -727,6 +730,9 @@ int commander_thread_main(int argc, char *argv[])
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
 				status_changed = true;
+
+				/* Re-check RC calibration */
+				rc_calibration_ok = (OK == rc_calibration_check());
 			}
 		}
 
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 9263c6a15..42f0190c7 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -100,6 +100,8 @@ int do_mag_calibration(int mavlink_fd)
 
 	close(fd);
 
+	mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
+
 	/* calibrate offsets */
 
 	// uint64_t calibration_start = hrt_absolute_time();
@@ -135,9 +137,8 @@ int do_mag_calibration(int mavlink_fd)
 
 			axis_index++;
 
-			char buf[50];
-			sprintf(buf, "please rotate around %c", axislabels[axis_index]);
-			mavlink_log_info(mavlink_fd, buf);
+			mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]);
+			mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter);
 			tune_neutral();
 
 			axis_deadline += calibration_interval / 3;
@@ -251,6 +252,8 @@ int do_mag_calibration(int mavlink_fd)
 			warnx("Setting Z mag scale failed!\n");
 		}
 
+		mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");
+
 		/* auto-save to EEPROM */
 		int save_ret = param_save_default();
 
@@ -274,6 +277,7 @@ int do_mag_calibration(int mavlink_fd)
 		mavlink_log_info(mavlink_fd, buf);
 
 		mavlink_log_info(mavlink_fd, "mag calibration done");
+		mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
 
 		return OK;
 		/* third beep by cal end routine */