Merge branch 'daregger_rate_control' of github.com:PX4/Firmware into calibration

1 files changed, 34 insertions, 23 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index f11c583d4..545569a65 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -292,10 +292,16 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	struct sensor_combined_s raw;
 
 	/* 30 seconds */
-	uint64_t calibration_interval = 30 * 1000 * 1000;
+	uint64_t calibration_interval = 45 * 1000 * 1000;
 	unsigned int calibration_counter = 0;
 
-	float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN};
+	/*
+	 * FLT_MIN is not the most negative float number,
+	 * but the smallest number by magnitude float can
+	 * represent. Use -FLT_MAX to initialize the most
+	 * negative number
+	 */
+	float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
 	float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
 
 	int fd = open(MAG_DEVICE_PATH, 0);
@@ -322,8 +328,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	uint64_t axis_deadline = hrt_absolute_time();
 	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
 
-	const char axislabels[3] = { 'X', 'Y', 'Z'};
-	int axis_index = 0;
+	const char axislabels[3] = { 'Z', 'X', 'Y'};
+	int axis_index = -1;
 
 	while (hrt_absolute_time() < calibration_deadline) {
 
@@ -331,18 +337,21 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
 
 		/* user guidance */
-		if (hrt_absolute_time() > axis_deadline &&
+		if (hrt_absolute_time() >= axis_deadline &&
 			axis_index < 3) {
+
+			axis_index++;
+
 			char buf[50];
 			sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]);
 			mavlink_log_info(mavlink_fd, buf);
+			ioctl(buzzer, TONE_SET_ALARM, 2);
 		
 			axis_deadline += calibration_interval / 3;
-			axis_index++;
 		}
 
 		if (!(axis_index < 3)) {
-			continue;
+			break;
 		}
 
 		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
@@ -358,26 +367,26 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 			/* get min/max values */
 
 			/* ignore other axes */
-			if (raw.magnetometer_ga[axis_index] < mag_min[axis_index]) {
-				mag_min[axis_index] = raw.magnetometer_ga[axis_index];
+			if (raw.magnetometer_ga[0] < mag_min[0]) {
+				mag_min[0] = raw.magnetometer_ga[0];
 			}
-			else if (raw.magnetometer_ga[axis_index] > mag_max[axis_index]) {
-				mag_max[axis_index] = raw.magnetometer_ga[axis_index];
+			else if (raw.magnetometer_ga[0] > mag_max[0]) {
+				mag_max[0] = raw.magnetometer_ga[0];
 			}
 
-			// if (raw.magnetometer_ga[1] < mag_min[1]) {
-			// 	mag_min[1] = raw.magnetometer_ga[1];
-			// }
-			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
-			// 	mag_max[1] = raw.magnetometer_ga[1];
-			// }
+			if (raw.magnetometer_ga[1] < mag_min[1]) {
+				mag_min[1] = raw.magnetometer_ga[1];
+			}
+			else if (raw.magnetometer_ga[1] > mag_max[1]) {
+				mag_max[1] = raw.magnetometer_ga[1];
+			}
 
-			// if (raw.magnetometer_ga[2] < mag_min[2]) {
-			// 	mag_min[2] = raw.magnetometer_ga[2];
-			// }
-			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
-			// 	mag_max[2] = raw.magnetometer_ga[2];
-			// }
+			if (raw.magnetometer_ga[2] < mag_min[2]) {
+				mag_min[2] = raw.magnetometer_ga[2];
+			}
+			else if (raw.magnetometer_ga[2] > mag_max[2]) {
+				mag_max[2] = raw.magnetometer_ga[2];
+			}
 
 			calibration_counter++;
 		} else {
@@ -840,7 +849,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 
 				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
 					mavlink_log_info(mavlink_fd, "[commander] CMD starting accel calibration");
+					ioctl(buzzer, TONE_SET_ALARM, 2);
 					do_accel_calibration(status_pub, &current_status);
+					ioctl(buzzer, TONE_SET_ALARM, 2);
 					mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration");
 					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
 					result = MAV_RESULT_ACCEPTED;