Merge branch 'master' of github.com:PX4/Firmware into dataman_state_nav_rewrite

4 files changed, 57 insertions, 59 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index a82655c9b..e14dff197 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -288,15 +288,22 @@ int commander_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("\tcommander is running");
-			print_status();
+	/* commands needing the app to run below */
+	if (!thread_running) {
+		warnx("\tcommander not started");
+		exit(1);
+	}
 
-		} else {
-			warnx("\tcommander not started");
-		}
+	if (!strcmp(argv[1], "status")) {
+		print_status();
+		exit(0);
+	}
 
+	if (!strcmp(argv[1], "check")) {
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		int checkres = prearm_check(&status, mavlink_fd_local);
+		close(mavlink_fd_local);
+		warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
 		exit(0);
 	}
 
@@ -305,7 +312,7 @@ int commander_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "2")) {
+	if (!strcmp(argv[1], "disarm")) {
 		arm_disarm(false, mavlink_fd, "command line");
 		exit(0);
 	}
@@ -326,6 +333,7 @@ void usage(const char *reason)
 
 void print_status()
 {
+	warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
 	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
 
 	/* read all relevant states */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 6b96e3a3f..363f5e8eb 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -70,8 +70,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
@@ -622,12 +620,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;
 	}
 
@@ -635,6 +634,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;
 	}
 
@@ -644,33 +644,31 @@ 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 = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
 
-		if (fd < 0) {
+		if (fd <= 0) {
 			mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
-			ret = fd;
+			failed = true;
 			goto system_eval;
 		}
 
@@ -679,20 +677,19 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		ret = read(fd, &diff_pres, sizeof(diff_pres));
 
 		if (ret == sizeof(diff_pres)) {
-			if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
+			if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) {
 				mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
 				// XXX do not make this fatal yet
-				ret = OK;
 			}
 		} else {
 			mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
 			/* this is frickin' fatal */
-			ret = ERROR;
+			failed = true;
 			goto system_eval;
 		}
 	}
 
 system_eval:
 	close(fd);
-	return ret;
+	return (failed);
 }
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 11072403e..ddc5bf154 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -67,4 +67,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 
 bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
 
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
+
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 667be87b7..c7ad605c5 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1051,10 +1051,16 @@ protected:
 			uint32_t mavlink_custom_mode;
 			get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
+			float out[8];
+
+			const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
+
+			/* scale outputs depending on system type */
 			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
 				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
 				mavlink_system.type == MAV_TYPE_OCTOROTOR) {
-				/* set number of valid outputs depending on vehicle type */
+				/* multirotors: set number of rotor outputs depending on type */
+
 				unsigned n;
 
 				switch (mavlink_system.type) {
@@ -1071,59 +1077,44 @@ protected:
 					break;
 				}
 
-				/* scale / assign outputs depending on system type */
-				float out[8];
-
 				for (unsigned i = 0; i < 8; i++) {
-					if (i < n) {
-						if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-							/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
+					if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+						if (i < n) {
+							/* scale PWM out 900..2100 us to 0..1 for rotors */
 							out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
 
 						} else {
-							/* send 0 when disarmed */
-							out[i] = 0.0f;
+							/* scale PWM out 900..2100 us to -1..1 for other channels */
+							out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
 						}
 
 					} else {
-						out[i] = -1.0f;
+						/* send 0 when disarmed */
+						out[i] = 0.0f;
 					}
 				}
 
-				mavlink_msg_hil_controls_send(_channel,
-							      hrt_absolute_time(),
-							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
-							      mavlink_base_mode,
-							      0);
 			} else {
-
-				/* fixed wing: scale all channels except throttle -1 .. 1
-				 * because we know that we set the mixers up this way
-				 */
-
-				float out[8];
-
-				const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
+				/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
 
 				for (unsigned i = 0; i < 8; i++) {
 					if (i != 3) {
-						/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
+						/* scale PWM out 900..2100 us to -1..1 for normal channels */
 						out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
 
 					} else {
-
-						/* scale fake PWM out 900..2100 us to 0..1 for throttle */
+						/* scale PWM out 900..2100 us to 0..1 for throttle */
 						out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
 					}
 
 				}
-
-				mavlink_msg_hil_controls_send(_channel,
-							      hrt_absolute_time(),
-							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
-							      mavlink_base_mode,
-							      0);
 			}
+
+			mavlink_msg_hil_controls_send(_channel,
+						      hrt_absolute_time(),
+						      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+						      mavlink_base_mode,
+						      0);
 		}
 	}
 };