Merged master into airspeed_test_fix

1 files changed, 14 insertions, 18 deletions

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index d894c9db0..e90c77af4 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -71,8 +71,6 @@
 #endif
 static const int ERROR = -1;
 
-static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
-
 // This array defines the arming state transitions. The rows are the new state, and the columns
 // are the current state. Using new state and current  state you can index into the array which
 // will be true for a valid transition or false for a invalid transition. In some cases even
@@ -623,12 +621,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 {
 	int ret;
+	bool failed = false;
 
 	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
 		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
-		ret = fd;
+		failed = true;
 		goto system_eval;
 	}
 
@@ -636,6 +635,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 
 	if (ret != OK) {
 		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
+		failed = true;
 		goto system_eval;
 	}
 
@@ -645,29 +645,25 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 
 	if (ret == sizeof(acc)) {
 		/* evaluate values */
-		float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
-
-		if (accel_scale < 9.78f || accel_scale > 9.83f) {
-			mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
-		}
+		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
 
-		if (accel_scale > 30.0f /* m/s^2 */) {
+		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
 			mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
+			mavlink_log_info(mavlink_fd, "#audio: hold still while arming");
 			/* this is frickin' fatal */
-			ret = ERROR;
+			failed = true;
 			goto system_eval;
-		} else {
-			ret = OK;
 		}
 	} else {
 		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
 		/* this is frickin' fatal */
-		ret = ERROR;
+		failed = true;
 		goto system_eval;
 	}
 
 	if (!status->is_rotary_wing) {
-
+		/* accel done, close it */
+		close(fd);
 		fd = orb_subscribe(ORB_ID(airspeed));
 
 		struct airspeed_s airspeed;
@@ -675,17 +671,17 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) ||
 			hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) {
 			mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
+			failed = true;
 			goto system_eval;
 		}
 
-		if (fabsf(airspeed.indicated_airspeed_m_s > 5.0f)) {
-			mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
+		if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
+			mavlink_log_critical(mavlink_fd, "#audio: WIND OR CALIBRATION MISSING");
 			// XXX do not make this fatal yet
-			ret = OK;
 		}
 	}
 
 system_eval:
 	close(fd);
-	return ret;
+	return (failed);
 }