Merge remote-tracking branch 'px4/beta' into acro2

15 files changed, 74 insertions, 67 deletions

diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x
index 42b64a68e..0f1288dec 100644
--- a/ROMFS/px4fmu_common/init.d/4001_quad_x
+++ b/ROMFS/px4fmu_common/init.d/4001_quad_x
@@ -48,5 +48,5 @@ set MIXER FMU_quad_x
 set PWM_OUTPUTS 1234
 set PWM_RATE 400
 set PWM_DISARMED 900
-set PWM_MIN 1000
+set PWM_MIN 1100
 set PWM_MAX 2000
diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x
index 270f51a58..fd5756586 100644
--- a/ROMFS/px4fmu_common/init.d/6001_hexa_x
+++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x
@@ -9,4 +9,6 @@ sh /etc/init.d/4001_quad_x
 
 set MIXER FMU_hexa_x
 
-set PWM_OUTPUTS 123456
+# We only can run one channel group with one rate,
+# so all 8 at 400 Hz
+set PWM_OUTPUTS 12345678
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index dcf5bbced..c5aef8273 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -8,7 +8,7 @@ then
 	if hw_ver compare PX4FMU_V1
 	then
 		echo "Start sdlog2 at 50Hz"
-		sdlog2 start -r 50 -a -b 16 -t
+		sdlog2 start -r 50 -a -b 8 -t
 	else
 		echo "Start sdlog2 at 200Hz"
 		sdlog2 start -r 200 -a -b 16 -t
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 84dc7ec64..178bcaeba 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -474,7 +474,7 @@ then
 			set MAV_TYPE 2
 			
 			# Use mixer to detect vehicle type
-			if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ]
+			if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ]
 			then
 				set MAV_TYPE 13
 			fi
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
deleted file mode 100644
index f8f9f0e4d..000000000
--- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
+++ /dev/null
@@ -1,18 +0,0 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a hexacopter in the + configuration. All controls
-are mixed 100%.
-
-R: 6+ 10000 10000 10000 0
-
-Gimbal / payload mixer for last two channels
------------------------------------------------------
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 6  10000  10000      0 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
deleted file mode 100644
index 26b40b9e9..000000000
--- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
+++ /dev/null
@@ -1,18 +0,0 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a hexacopter in the X configuration.  All controls
-are mixed 100%.
-
-R: 6x 10000 10000 10000 0
-
-Gimbal / payload mixer for last two channels
------------------------------------------------------
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 6  10000  10000      0 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix
new file mode 100644
index 000000000..b5e38ce9e
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a hexacopter in the + configuration. All controls
+are mixed 100%.
+
+R: 6+ 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix
new file mode 100644
index 000000000..8e8d122ad
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a hexacopter in the X configuration.  All controls
+are mixed 100%.
+
+R: 6x 10000 10000 10000 0
diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h
index e56b14ba4..850043ddf 100755
--- a/nuttx-configs/px4fmu-v2/include/board.h
+++ b/nuttx-configs/px4fmu-v2/include/board.h
@@ -209,8 +209,8 @@
 
 #define GPIO_USART3_RX	GPIO_USART3_RX_3
 #define GPIO_USART3_TX	GPIO_USART3_TX_3
-#define GPIO_USART2_RTS	GPIO_USART2_RTS_2
-#define GPIO_USART2_CTS	GPIO_USART2_CTS_2
+#define GPIO_USART3_RTS	GPIO_USART3_RTS_2
+#define GPIO_USART3_CTS	GPIO_USART3_CTS_2
 
 #define GPIO_UART4_RX	GPIO_UART4_RX_1
 #define GPIO_UART4_TX	GPIO_UART4_TX_1
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 3ef1871a8..45fdaa355 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
@@ -236,7 +236,7 @@ private:
 		float land_slope_length;
 		float land_H1_virt;
 		float land_flare_alt_relative;
-		float land_thrust_lim_horizontal_distance;
+		float land_thrust_lim_alt_relative;
 		float land_heading_hold_horizontal_distance;
 
 	}		_parameters;			/**< local copies of interesting parameters */
@@ -281,7 +281,7 @@ private:
 		param_t land_slope_length;
 		param_t land_H1_virt;
 		param_t land_flare_alt_relative;
-		param_t land_thrust_lim_horizontal_distance;
+		param_t land_thrust_lim_alt_relative;
 		param_t land_heading_hold_horizontal_distance;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
@@ -434,7 +434,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
 	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
-	_parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST");
+	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
@@ -523,7 +523,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
 	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
 	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
-	param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance));
+	param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
@@ -558,7 +558,7 @@ FixedwingPositionControl::parameters_update()
 	}
 
 	/* Update the landing slope */
-	landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt);
+	landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
 
 	/* Update and publish the navigation capabilities */
 	_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
@@ -836,6 +836,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 
+			float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
+
 			/* Horizontal landing control */
 			/* switch to heading hold for the last meters, continue heading hold after */
 			float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -846,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 				if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
 					if (pos_sp_triplet.previous.valid) {
-						target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
+						target_bearing = bearing_lastwp_currwp;
 					} else {
 						target_bearing = _att.yaw;
 					}
@@ -888,10 +890,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
 			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
-			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
-			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
-
-
+			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
+			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
 			if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 
@@ -903,7 +904,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;
 
-				 if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) {
+				 if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 					if (!land_motor_lim) {
 						land_motor_lim  = true;
@@ -912,7 +913,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 				 }
 
-				float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt);
+				float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
 				/* avoid climbout */
 				if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 7954d75c2..ee8721ff9 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -172,5 +172,5 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
 PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
 PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
 PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
-PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
+PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
 PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp
index b139a6397..e5f7023ae 100644
--- a/src/modules/fw_pos_control_l1/landingslope.cpp
+++ b/src/modules/fw_pos_control_l1/landingslope.cpp
@@ -48,13 +48,13 @@
 
 void Landingslope::update(float landing_slope_angle_rad,
 		float flare_relative_alt,
-		float motor_lim_horizontal_distance,
+		float motor_lim_relative_alt,
 		float H1_virt)
 {
 
 	_landing_slope_angle_rad = landing_slope_angle_rad;
 	_flare_relative_alt = flare_relative_alt;
-	_motor_lim_horizontal_distance = motor_lim_horizontal_distance;
+	_motor_lim_relative_alt = motor_lim_relative_alt;
 	_H1_virt = H1_virt;
 
 	calculateSlopeValues();
@@ -74,9 +74,21 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_
 	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
 }
 
-float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude)
+float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
 {
-	return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
+	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude);
+	else
+		return wp_altitude;
+}
 
+float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
+{
+	/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
+	else
+		return wp_landing_altitude;
 }
 
diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h
index 1a149fc7c..76d65a55f 100644
--- a/src/modules/fw_pos_control_l1/landingslope.h
+++ b/src/modules/fw_pos_control_l1/landingslope.h
@@ -49,7 +49,7 @@ private:
 	/* see Documentation/fw_landing.png for a plot of the landing slope */
 	float _landing_slope_angle_rad;			/**< phi in the plot */
 	float _flare_relative_alt;				/**< h_flare,rel in the plot */
-	float _motor_lim_horizontal_distance;
+	float _motor_lim_relative_alt;
 	float _H1_virt;							/**< H1 in the plot */
 	float _H0;								/**< h_flare,rel + H1 in the plot */
 	float _d1;								/**< d1 in the plot */
@@ -63,7 +63,18 @@ public:
 	Landingslope() {}
 	~Landingslope() {}
 
-	float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude);
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
+
+	/**
+	 *
+	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 * Performs check if aircraft is in front of waypoint to avoid climbout
+	 */
+	float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);
 
 	/**
 	 *
@@ -85,17 +96,17 @@ public:
 	}
 
 
-	float getFlareCurveAltitude(float wp_distance, float wp_altitude);
+	float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
 
 	void update(float landing_slope_angle_rad,
 			float flare_relative_alt,
-			float motor_lim_horizontal_distance,
+			float motor_lim_relative_alt,
 			float H1_virt);
 
 
 	inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
 	inline float flare_relative_alt() {return _flare_relative_alt;}
-	inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;}
+	inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;}
 	inline float H1_virt() {return _H1_virt;}
 	inline float H0() {return _H0;}
 	inline float d1() {return _d1;}
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
index a55ddf8a3..b05273c0d 100644
--- a/src/modules/systemlib/mixer/mixer_load.c
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -52,7 +52,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 	/* open the mixer definition file */
 	fp = fopen(fname, "r");
 	if (fp == NULL) {
-		return 1;
+		warnx("file not found");
+		return -1;
 	}
 
 	/* read valid lines from the file into a buffer */
@@ -88,7 +89,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 
 		/* if the line is too long to fit in the buffer, bail */
 		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
-			return 1;
+			warnx("line too long");
+			return -1;
 		}
 
 		/* add the line to the buffer */
diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp
index 6da39d371..2f5ed3265 100644
--- a/src/systemcmds/mixer/mixer.cpp
+++ b/src/systemcmds/mixer/mixer.cpp
@@ -102,7 +102,8 @@ load(const char *devname, const char *fname)
 	if (ioctl(dev, MIXERIOCRESET, 0))
 		err(1, "can't reset mixers on %s", devname);
 
-	load_mixer_file(fname, &buf[0], sizeof(buf));
+	if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0)
+		err(1, "can't load mixer: %s", fname);
 
 	/* XXX pass the buffer to the device */
 	int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);