Merge remote-tracking branch 'upstream/navigator_rewrite' into navigator_rewrite_estimator

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1452,7 +1452,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		if (ground_speed_length > FLT_EPSILON) {
 			flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
 		}
-		fwPosctrl::mTecs::LimitOverride limitOverride;
+		fwPosctrl::LimitOverride limitOverride;
 		if (climbout_mode) {
 			limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
 		} else {

src/modules/fw_pos_control_l1/module.mk

@@ -41,4 +41,5 @@ SRCS		= fw_pos_control_l1_main.cpp \
 		  fw_pos_control_l1_params.c \
 		  landingslope.cpp \
 		  mtecs/mTecs.cpp \
+		  mtecs/limitoverride.cpp \
 		  mtecs/mTecs_params.c

src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp

@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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 limitoverride.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "limitoverride.h"
+
+namespace fwPosctrl {
+
+bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+		BlockOutputLimiter &outputLimiterPitch)
+{
+	bool ret = false;
+
+	if (overrideThrottleMinEnabled)	{
+		outputLimiterThrottle.setMin(overrideThrottleMin);
+		ret = true;
+	}
+	if (overrideThrottleMaxEnabled)	{
+		outputLimiterThrottle.setMax(overrideThrottleMax);
+		ret = true;
+	}
+	if (overridePitchMinEnabled)	{
+		outputLimiterPitch.setMin(overridePitchMin);
+		ret = true;
+	}
+	if (overridePitchMaxEnabled)	{
+		outputLimiterPitch.setMax(overridePitchMax);
+		ret = true;
+	}
+
+	return ret;
+}
+
+} /* namespace fwPosctrl */

src/modules/fw_pos_control_l1/mtecs/limitoverride.h

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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 limitoverride.h
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+
+#ifndef LIMITOVERRIDE_H_
+#define LIMITOVERRIDE_H_
+
+#include "mTecs_blocks.h"
+
+namespace fwPosctrl
+{
+
+/* A small class which provides helper functions to override control output limits which are usually set by
+* parameters in special cases
+*/
+class LimitOverride
+{
+public:
+	LimitOverride() :
+		overrideThrottleMinEnabled(false),
+		overrideThrottleMaxEnabled(false),
+		overridePitchMinEnabled(false),
+		overridePitchMaxEnabled(false)
+	{};
+
+	~LimitOverride() {};
+
+	/*
+	* Override the limits of the outputlimiter instances given by the arguments with the limits saved in
+	* this class (if enabled)
+	* @return true if the limit was applied
+	*/
+	bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+			BlockOutputLimiter &outputLimiterPitch);
+
+	/* Functions to enable or disable the override */
+	void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
+			&overrideThrottleMin, value); }
+	void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
+	void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
+			&overrideThrottleMax, value); }
+	void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
+	void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
+			&overridePitchMin, value); }
+	void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
+	void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
+			&overridePitchMax, value); }
+	void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
+
+protected:
+	bool overrideThrottleMinEnabled;
+	float overrideThrottleMin;
+	bool overrideThrottleMaxEnabled;
+	float overrideThrottleMax;
+	bool overridePitchMinEnabled;
+	float overridePitchMin; //in degrees (replaces param values)
+	bool overridePitchMaxEnabled;
+	float overridePitchMax; //in degrees (replaces param values)
+
+	/* Enable a specific limit override */
+	void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; };
+
+	/* Disable a specific limit override */
+	void disable(bool *flag) { *flag = false; };
+};
+
+} /* namespace fwPosctrl */
+
+#endif /* LIMITOVERRIDE_H_ */

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -58,6 +58,7 @@ mTecs::mTecs() :
 	_controlEnergyDistribution(this, "PIT", true),
 	_controlAltitude(this, "FPA", true),
 	_controlAirSpeed(this, "ACC"),
+	_airspeedLowpass(this, "A_LP"),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
 	_pitchSp(0.0f),
@@ -122,12 +123,18 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	/* time measurement */
 	updateTimeMeasurement();
 
+	/* Filter arispeed */
+	float airspeedFiltered = _airspeedLowpass.update(airspeed);
+
 	/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
-	float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed);
+	float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered);
 
 	/* Debug output */
 	if (_counter % 10 == 0) {
-		debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp);
+		debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f,"
+				"accelerationLongitudinalSp%.4f",
+				(double)airspeedSp, (double)airspeed,
+				(double)airspeedFiltered, (double)accelerationLongitudinalSp);
 	}
 
 	/* Write part of the status message */
@@ -135,19 +142,20 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	_status.flightPathAngle = flightPathAngle;
 	_status.airspeedSp = airspeedSp;
 	_status.airspeed = airspeed;
+	_status.airspeedFiltered = airspeedFiltered;
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
-	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed,
+	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered,
 			accelerationLongitudinalSp, mode, limitOverride);
 }
 
-int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
+int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
 		float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
-			!isfinite(airspeed) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
+			!isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
 		return -1;
 	}
 	/* time measurement */
@@ -160,7 +168,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	float flightPathAngleError = flightPathAngleSp - flightPathAngle;
 	float airspeedDerivative = 0.0f;
 	if(_airspeedDerivative.getDt() > 0.0f) {
-		airspeedDerivative = _airspeedDerivative.update(airspeed);
+		airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
 	}
 	float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G;
 	float airspeedDerivativeSp = accelerationLongitudinalSp;
@@ -186,7 +194,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	}
 
 	/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
-	if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeed < _airspeedMin.get()) {
+	if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
 		mode = TECS_MODE_UNDERSPEED;
 	}
 
@@ -302,29 +310,4 @@ void mTecs::debug(const char *fmt, ...) {
 	debug_print(fmt, args);
 }
 
-bool mTecs::LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
-		BlockOutputLimiter &outputLimiterPitch)
-{
-	bool ret = false;
-
-	if (overrideThrottleMinEnabled)	{
-		outputLimiterThrottle.setMin(overrideThrottleMin);
-		ret = true;
-	}
-	if (overrideThrottleMaxEnabled)	{
-		outputLimiterThrottle.setMax(overrideThrottleMax);
-		ret = true;
-	}
-	if (overridePitchMinEnabled)	{
-		outputLimiterPitch.setMin(overridePitchMin);
-		ret = true;
-	}
-	if (overridePitchMaxEnabled)	{
-		outputLimiterPitch.setMax(overridePitchMax);
-		ret = true;
-	}
-
-	return ret;
-}
-
 } /* namespace fwPosctrl */

src/modules/fw_pos_control_l1/mtecs/mTecs.h

@@ -44,6 +44,7 @@
 #define MTECS_H_
 
 #include "mTecs_blocks.h"
+#include "limitoverride.h"
 
 #include <controllib/block/BlockParam.hpp>
 #include <drivers/drv_hrt.h>
@@ -60,62 +61,6 @@ public:
 	mTecs();
 	virtual ~mTecs();
 
-	/* A small class which provides helper fucntions to override control output limits which are usually set by
-	 * parameters in special cases
-	 */
-	class LimitOverride
-	{
-	public:
-		LimitOverride() :
-			overrideThrottleMinEnabled(false),
-			overrideThrottleMaxEnabled(false),
-			overridePitchMinEnabled(false),
-			overridePitchMaxEnabled(false)
-		{};
-
-		~LimitOverride() {};
-
-		/*
-		 * Override the limits of the outputlimiter instances given by the arguments with the limits saved in
-		 * this class (if enabled)
-		 * @return true if the limit was applied
-		 */
-		bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
-				BlockOutputLimiter &outputLimiterPitch);
-
-		/* Functions to enable or disable the override */
-		void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
-				&overrideThrottleMin, value); }
-		void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
-		void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
-				&overrideThrottleMax, value); }
-		void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
-		void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
-				&overridePitchMin, value); }
-		void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
-		void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
-				&overridePitchMax, value); }
-		void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
-
-	protected:
-		bool overrideThrottleMinEnabled;
-		float overrideThrottleMin;
-		bool overrideThrottleMaxEnabled;
-		float overrideThrottleMax;
-		bool overridePitchMinEnabled;
-		float overridePitchMin; //in degrees (replaces param values)
-		bool overridePitchMaxEnabled;
-		float overridePitchMax; //in degrees (replaces param values)
-
-		/* Enable a specific limit override */
-		void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value;
-		};
-		/* Disable a specific limit override */
-		void disable(bool *flag) { *flag = false; };
-
-
-	};
-
 	/*
 	 * Control in altitude setpoint and speed mode
 	 */
@@ -131,7 +76,7 @@ public:
 	/*
 	 * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
 	 */
-	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
+	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
 			float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
@@ -145,9 +90,10 @@ public:
 	void resetDerivatives(float airspeed);
 
 	/* Accessors */
-	bool getEnabled() {return _mTecsEnabled.get() > 0;}
+	bool getEnabled() { return _mTecsEnabled.get() > 0; }
-	float getThrottleSetpoint() {return _throttleSp;}
+	float getThrottleSetpoint() { return _throttleSp; }
-	float getPitchSetpoint() {return _pitchSp;}
+	float getPitchSetpoint() { return _pitchSp; }
+	float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
 
 protected:
 	/* parameters */
@@ -160,11 +106,12 @@ protected:
 	/* control blocks */
 	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output is throttle */
 	BlockFFPILimitedCustom _controlEnergyDistribution;	/**< FFPI controller for energy distribution control: output is pitch */
-	BlockPDLimited	_controlAltitude;		/**< P controller for altitude: output is the flight path angle setpoint */
+	BlockPDLimited	_controlAltitude;		/**< PD controller for altitude: output is the flight path angle setpoint */
-	BlockPLimited	_controlAirSpeed;			/**< P controller for airspeed: output is acceleration setpoint */
+	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration setpoint */
 
 	/* Other calculation Blocks */
-	control::BlockDerivative _airspeedDerivative;
+	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
+	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
 	/* Output setpoints */
 	float _throttleSp;				/**< Throttle Setpoint from 0 to 1 */

src/modules/fw_pos_control_l1/mtecs/mTecs_params.c

@@ -222,6 +222,12 @@ PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f);
  */
 PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
 
+/**
+ * Lowpass (cutoff freq.) for airspeed
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
 
 /**
  * P gain for the airspeed control
@@ -233,6 +239,23 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
  */
 PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f);
 
+/**
+ * D gain for the airspeed control
+ * Maps the change of airspeed error to the acceleration setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
+
+/**
+ * Lowpass for ACC error derivative calculation (see MT_ACC_D)
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
+
 /**
  * Minimal acceleration (air)
  *

src/modules/sdlog2/sdlog2.c

@@ -1516,6 +1516,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
 			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
 			log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
+			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
 			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
 			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
 			log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;

src/modules/sdlog2/sdlog2_messages.h

@@ -347,6 +347,7 @@ struct log_TECS_s {
 	float flightPathAngle;
 	float airspeedSp;
 	float airspeed;
+	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
 
@@ -439,7 +440,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "ffffffffffffB",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"),
+	LOG_FORMAT(TECS, "fffffffffffffB",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */

src/modules/uORB/topics/tecs_status.h

@@ -70,6 +70,7 @@ struct tecs_status_s {
 	float flightPathAngle;
 	float airspeedSp;
 	float airspeed;
+	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;