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

Tools/check_submodules.sh

@@ -2,12 +2,19 @@
 
 if [ -d NuttX/nuttx ];
 	then
-	STATUSRETVAL=$(git status --porcelain | grep -i "NuttX")
+	STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<")
 	if [ -z "$STATUSRETVAL" ]; then
 		echo "Checked NuttX submodule, correct version found"
 	else
-		echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'"
+		echo ""
+		echo ""
+		echo "NuttX sub repo not at correct version. Try 'git submodule update'"
 		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
+		echo ""
+		echo ""
+		echo "New commits required:"
+		echo "$(git submodule summary)"
+		echo ""
 		exit 1
 	fi
 else
@@ -18,12 +25,19 @@ fi
 
 if [ -d mavlink/include/mavlink/v1.0 ];
 	then
-	STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0")
+	STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<")
 	if [ -z "$STATUSRETVAL" ]; then
 		echo "Checked mavlink submodule, correct version found"
 	else
-		echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'"
+		echo ""
+		echo ""
+		echo "mavlink sub repo not at correct version. Try 'git submodule update'"
 		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
+		echo ""
+		echo ""
+		echo "New commits required:"
+		echo "$(git submodule summary)"
+		echo ""
 		exit 1
 	fi
 else

src/drivers/drv_px4flow.h

@@ -46,10 +46,18 @@
 
 #define PX4FLOW_DEVICE_PATH	"/dev/px4flow"
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 /**
  * Optical flow in NED body frame in SI units.
  *
  * @see http://en.wikipedia.org/wiki/International_System_of_Units
+ *
+ * @warning If possible the usage of the raw flow and performing rotation-compensation
+ *		using the autopilot angular rate estimate is recommended.
  */
 struct px4flow_report {
 
@@ -57,14 +65,18 @@ struct px4flow_report {
 
 	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
 	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
-	float flow_comp_x_m;		/**< speed over ground in meters, rotation-compensated */
-	float flow_comp_y_m;		/**< speed over ground in meters, rotation-compensated */
+	float flow_comp_x_m;		/**< speed over ground in meters per second, rotation-compensated */
+	float flow_comp_y_m;		/**< speed over ground in meters per second, rotation-compensated */
 	float ground_distance_m;	/**< Altitude / distance to ground in meters */
 	uint8_t	quality;		/**< Quality of the measurement, 0: bad quality, 255: maximum quality */
 	uint8_t sensor_id;		/**< id of the sensor emitting the flow value */
 
 };
 
+/**
+ * @}
+ */
+
 /*
  * ObjDev tag for px4flow data.
  */

src/drivers/drv_range_finder.h

@@ -50,6 +50,11 @@ enum RANGE_FINDER_TYPE {
 	RANGE_FINDER_TYPE_LASER = 0,
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 /**
  * range finder report structure.  Reads from the device must be in multiples of this
  * structure.
@@ -64,6 +69,10 @@ struct range_finder_report {
 	uint8_t valid;				/**< 1 == within sensor range, 0 = outside sensor range */
 };
 
+/**
+ * @}
+ */
+
 /*
  * ObjDev tag for raw range finder data.
  */

src/drivers/drv_rc_input.h

@@ -67,6 +67,11 @@
  */
 #define RC_INPUT_RSSI_MAX	255
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 /**
  * Input signal type, value is a control position from zero to 100
  * percent.
@@ -141,6 +146,10 @@ struct rc_input_values {
 	rc_input_t		values[RC_INPUT_MAX_CHANNELS];
 };
 
+/**
+ * @}
+ */
+
 /*
  * ObjDev tag for R/C inputs.
  */

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 */

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);
 }

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_ */

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);
 		}
 	}
 };