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

6 files changed, 203 insertions, 78 deletions

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index a95e041a1..c9c27892f 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -194,6 +194,8 @@ public:
 	 */
 	void			print_info();
 
+	void			print_registers();
+
 protected:
 	virtual int		probe();
 
@@ -1414,6 +1416,21 @@ MPU6000::print_info()
 	_gyro_reports->print_info("gyro queue");
 }
 
+void
+MPU6000::print_registers()
+{
+	printf("MPU6000 registers\n");
+	for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
+		uint8_t v = read_reg(reg);
+		printf("%02x:%02x ",(unsigned)reg, (unsigned)v);
+		if ((reg - (MPUREG_PRODUCT_ID-1)) % 13 == 0) {
+			printf("\n");
+		}
+	}
+	printf("\n");
+}
+
+
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
 	CDev("MPU6000_gyro", path),
 	_parent(parent),
@@ -1479,6 +1496,7 @@ void	start(bool, enum Rotation);
 void	test(bool);
 void	reset(bool);
 void	info(bool);
+void	regdump(bool);
 void	usage();
 
 /**
@@ -1654,10 +1672,26 @@ info(bool external_bus)
 	exit(0);
 }
 
+/**
+ * Dump the register information
+ */
+void
+regdump(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr == nullptr)
+		errx(1, "driver not running");
+
+	printf("regdump @ %p\n", *g_dev_ptr);
+	(*g_dev_ptr)->print_registers();
+
+	exit(0);
+}
+
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -1714,5 +1748,11 @@ mpu6000_main(int argc, char *argv[])
 	if (!strcmp(verb, "info"))
 		mpu6000::info(external_bus);
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	/*
+	 * Print register information.
+	 */
+	if (!strcmp(verb, "regdump"))
+		mpu6000::regdump(external_bus);
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
 }
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 948b29bcb..dbf15d98a 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -200,9 +200,11 @@ private:
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
 	fwPosctrl::mTecs				_mTecs;
-	bool						_was_pos_control_mode;
-	bool						_was_velocity_control_mode;
-	bool						_was_alt_control_mode;
+	enum FW_POSCTRL_MODE {
+		FW_POSCTRL_MODE_AUTO,
+		FW_POSCTRL_MODE_POSITION,
+		FW_POSCTRL_MODE_OTHER
+	} _control_mode_current;			///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
 
 	struct {
 		float l1_period;
@@ -471,7 +473,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_global_pos_valid(false),
 	_l1_control(),
 	_mTecs(),
-	_was_pos_control_mode(false)
+	_control_mode_current(FW_POSCTRL_MODE_OTHER)
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
@@ -908,22 +910,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
 
-	/* AUTONOMOUS FLIGHT */
+	if (_control_mode.flag_control_auto_enabled &&
+			pos_sp_triplet.current.valid) {
+		/* AUTONOMOUS FLIGHT */
 
-	// XXX this should only execute if auto AND safety off (actuators active),
-	// else integrators should be constantly reset.
-	if (pos_sp_triplet.current.valid) {
-
-		if (!_was_pos_control_mode) {
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
 				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
 			}
 		}
-
-		_was_pos_control_mode = true;
-		_was_velocity_control_mode = false;
+		_control_mode_current = FW_POSCTRL_MODE_AUTO;
 
 		/* reset hold altitude */
 		_hold_alt = _global_pos.alt;
@@ -1208,15 +1207,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
 							calculate_target_airspeed(_parameters.airspeed_trim),
 							eas2tas,
-								math::radians(_parameters.pitch_limit_min),
-								math::radians(_parameters.pitch_limit_max),
-								_parameters.throttle_min,
-								takeoff_throttle,
-								_parameters.throttle_cruise,
-								false,
-								math::radians(_parameters.pitch_limit_min),
-								_global_pos.alt,
-								ground_speed);
+							math::radians(_parameters.pitch_limit_min),
+							math::radians(_parameters.pitch_limit_max),
+							_parameters.throttle_min,
+							takeoff_throttle,
+							_parameters.throttle_cruise,
+							false,
+							math::radians(_parameters.pitch_limit_min),
+							_global_pos.alt,
+							ground_speed);
 				}
 			} else {
 				/* Tell the attitude controller to stop integrating while we are waiting
@@ -1248,53 +1247,63 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_reset_integral = true;
 		}
 
-	} else if (_control_mode.flag_control_velocity_enabled) {
+	} else if (_control_mode.flag_control_velocity_enabled &&
+			_control_mode.flag_control_altitude_enabled) {
+		/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
+
 		const float deadBand = (60.0f/1000.0f);
 		const float factor = 1.0f - deadBand;
-		if (!_was_velocity_control_mode) {
+		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
+			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _global_pos.alt;
-			_was_alt_control_mode = false;
 		}
-		_was_velocity_control_mode = true;
-		_was_pos_control_mode = false;
-		// Get demanded airspeed
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
+			/* reset integrators */
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			}
+		}
+		_control_mode_current = FW_POSCTRL_MODE_POSITION;
+
+		/* Get demanded airspeed */
 		float altctrl_airspeed = _parameters.airspeed_min +
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
 					  _manual.z;
 
-		// Get demanded vertical velocity from pitch control
-		float pitch = 0.0f;
+		/* Get demanded vertical velocity from pitch control */
+		static bool was_in_deadband = false;
 		if (_manual.x > deadBand) {
-			pitch = (_manual.x - deadBand) / factor;
-		} else if (_manual.x < - deadBand) {
-			pitch = (_manual.x + deadBand) / factor;
-		}
-		if (pitch > 0.0f) {
+			float pitch = (_manual.x - deadBand) / factor;
 			_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
-			_was_alt_control_mode = false;
-		} else if (pitch < 0.0f) {
+			was_in_deadband = false;
+		} else if (_manual.x < - deadBand) {
+			float pitch = (_manual.x + deadBand) / factor;
 			_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
-			_was_alt_control_mode = false;
-		} else if (!_was_alt_control_mode) {
+			was_in_deadband = false;
+		} else if (!was_in_deadband) {
+			 /* store altitude at which manual.x was inside deadBand
+			  * The aircraft should immediately try to fly at this altitude
+			  * as this is what the pilot expects when he moves the stick to the center */
 			_hold_alt = _global_pos.alt;
-			_was_alt_control_mode = true;
+			was_in_deadband = true;
 		}
 		tecs_update_pitch_throttle(_hold_alt,
-									altctrl_airspeed,
-									eas2tas,
-									math::radians(_parameters.pitch_limit_min),
-									math::radians(_parameters.pitch_limit_max),
-									_parameters.throttle_min,
-									_parameters.throttle_max,
-									_parameters.throttle_cruise,
-									false,
-									math::radians(_parameters.pitch_limit_min),
-									_global_pos.alt,
-									ground_speed,
-									TECS_MODE_NORMAL);
+				altctrl_airspeed,
+				eas2tas,
+				math::radians(_parameters.pitch_limit_min),
+				math::radians(_parameters.pitch_limit_max),
+				_parameters.throttle_min,
+				_parameters.throttle_max,
+				_parameters.throttle_cruise,
+				false,
+				math::radians(_parameters.pitch_limit_min),
+				_global_pos.alt,
+				ground_speed,
+				TECS_MODE_NORMAL);
 	} else {
-		_was_velocity_control_mode = false;
-		_was_pos_control_mode = false;
+		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 
 		/** MANUAL FLIGHT **/
 
@@ -1311,10 +1320,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}
 	}
 
+	/* Copy thrust output for publication */
 	if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
 		/* Set thrust to 0 to minimize damage */
 		_att_sp.thrust = 0.0f;
-	} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
+			pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
 			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
@@ -1327,8 +1338,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	/* During a takeoff waypoint while waiting for launch the pitch sp is set
 	 * already (not by tecs) */
-	if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
-			launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
+	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+				pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+				launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
 		_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 	}
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 296919c04..0af01cba1 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -1052,10 +1052,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				landed = false;
 				landed_time = 0;
 			}
-			/* reset xy velocity estimates when landed */
-			x_est[1] = 0.0f;
-			y_est[1] = 0.0f;
-
 		} else {
 			if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
 				if (landed_time == 0) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index c7e9ae3eb..bd777428f 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include <inttypes.h>
+
 /**
  * @file protocol.h
  *
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 24800cce8..958907d1b 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -238,11 +238,11 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params.idle_pwm_mc = PWM_LOWEST_MIN;
 	_params.vtol_motor_count = 0;
 
-	_params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC");
-	_params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT");
-	_params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN");
-	_params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX");
-	_params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM");
+	_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
+	_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
+	_params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN");
+	_params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX");
+	_params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM");
 
 	/* fetch initial parameter values */
 	parameters_update();
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
index 44157acba..e21bccb0c 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -1,13 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vtol_att_control_params.c
+ * Parameters for vtol attitude controller.
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
 #include <systemlib/param/param.h>
 
-// number of engines
-PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0);
-// idle pwm in multicopter mode
-PARAM_DEFINE_INT32(IDLE_PWM_MC,900);
-// min airspeed in multicopter mode
-PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2);
-// max airspeed in multicopter mode
-PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30);
-// trim airspeed in multicopter mode
-PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10);
+/**
+ * VTOL number of engines
+ *
+ * @min 1.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_MOT_COUNT,0);
+
+/**
+ * Idle speed of VTOL when in multicopter mode
+ *
+ * @min 900
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900);
+
+/**
+ * Minimum airspeed in multicopter mode
+ *
+ * This is the minimum speed of the air flowing over the control surfaces.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f);
+
+/**
+ * Maximum airspeed in multicopter mode
+ *
+ * This is the maximum speed of the air flowing over the control surfaces.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f);
+
+/**
+ * Trim airspeed when in multicopter mode
+ *
+ * This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f);