From 323759bb5279af62dcc4a0454d3ec06debae36ea Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 21 May 2015 22:06:54 +0200
Subject: [PATCH] commander: Fix error checking and handling of level routine

---
 src/modules/commander/accelerometer_calibration.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 32c0ffd8cc..f250eef475 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -557,7 +557,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
 
 int do_level_calibration(int mavlink_fd) {
 	const unsigned cal_time = 5;
-	const unsigned cal_hz = 250;
+	const unsigned cal_hz = 100;
 	const unsigned settle_time = 30;
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	struct vehicle_attitude_s att;
@@ -612,6 +612,8 @@ int do_level_calibration(int mavlink_fd) {
 		pitch_mean /= counter;
 	} else {
 		mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken");
+		success = false;
+		goto out;
 	}
 
 	if (fabsf(roll_mean) > 0.8f ) {
@@ -626,6 +628,7 @@ int do_level_calibration(int mavlink_fd) {
 		success = true;
 	}
 
+out:
 	if (success) {
 		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level");
 		return 0;