1 files changed, 16 insertions, 15 deletions

diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index f1afb0107..9cd2f3a19 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -50,8 +50,13 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
-void do_gyro_calibration(int mavlink_fd)
+int do_gyro_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
 
@@ -98,7 +103,7 @@ void do_gyro_calibration(int mavlink_fd)
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
 			mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
-			return;
+			return ERROR;
 		}
 	}
 
@@ -137,18 +142,17 @@ void do_gyro_calibration(int mavlink_fd)
 		if (save_ret != 0) {
 			warnx("WARNING: auto-save of params to storage failed");
 			mavlink_log_critical(mavlink_fd, "gyro store failed");
-			// XXX negative tune
-			return;
+			return ERROR;
 		}
 
 		mavlink_log_info(mavlink_fd, "gyro calibration done");
 
-		tune_positive();
+		tune_neutral();
 		/* third beep by cal end routine */
 
 	} else {
 		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
-		return;
+		return ERROR;
 	}
 
 
@@ -180,8 +184,7 @@ void do_gyro_calibration(int mavlink_fd)
 			&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
 			mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
 			mavlink_log_info(mavlink_fd, "gyro calibration done");
-			tune_positive();
-			return;
+			return OK;
 		}
 
 		/* wait blocking for new data */
@@ -221,7 +224,7 @@ void do_gyro_calibration(int mavlink_fd)
 			// operating near the 1e6/1e8 max sane resolution of float.
 			gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
 
-			warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral);
+//			warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
 
 		// } else if (poll_ret == 0) {
 		// 	/* any poll failure for 1s is a reason to abort */
@@ -232,8 +235,8 @@ void do_gyro_calibration(int mavlink_fd)
 
 	float gyro_scale = baseline_integral / gyro_integral;
 	float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
-	warnx("gyro scale: yaw (z): %6.4f", gyro_scale);
-	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale);
+	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
+	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
 
 
 	if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
@@ -272,12 +275,10 @@ void do_gyro_calibration(int mavlink_fd)
 		// mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "gyro calibration done");
 
-		tune_positive();
 		/* third beep by cal end routine */
-
+		return OK;
 	} else {
 		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
+		return ERROR;
 	}
-
-	close(sub_sensor_combined);
 }