Merge remote-tracking branch 'px4/mtecs' into navigator_rewrite

5 files changed, 155 insertions, 31 deletions

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 31b3b3066..070d7377a 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
@@ -380,7 +380,7 @@ private:
 			bool climbout_mode, float climbout_pitch_min_rad,
 			float altitude,
 			const math::Vector<3> &ground_speed,
-			fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL);
+			tecs_mode mode = TECS_MODE_NORMAL);
 
 };
 
@@ -835,7 +835,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
-	_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
+	if (!_mTecs.getEnabled()) {
+		_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
+	}
+
 	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
 	/* no throttle limit as default */
@@ -851,6 +854,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
 			}
 		}
 
@@ -1008,7 +1012,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						0.0f, throttle_max, throttle_land,
 						false, flare_pitch_angle_rad,
 						_pos_sp_triplet.current.alt + relative_alt, ground_speed,
-						land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND);
+						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
 
 				if (!land_noreturn_vertical) {
 					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -1101,7 +1105,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 							math::radians(10.0f)),
 							_global_pos.alt,
 							ground_speed,
-							fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
+							TECS_MODE_TAKEOFF);
 
 					/* limit roll motion to ensure enough lift */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
@@ -1439,16 +1443,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		bool climbout_mode, float climbout_pitch_min_rad,
 		float altitude,
 		const math::Vector<3> &ground_speed,
-		fwPosctrl::mTecs::tecs_mode mode)
+		tecs_mode mode)
 {
 	if (_mTecs.getEnabled()) {
+		/* Using mtecs library: prepare arguments for mtecs call */
 		float flightPathAngle = 0.0f;
 		float ground_speed_length = ground_speed.length();
 		if (ground_speed_length > FLT_EPSILON) {
 			flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
 		}
-		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode);
+		fwPosctrl::mTecs::LimitOverride limitOverride;
+		if (climbout_mode) {
+			limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+		} else {
+			limitOverride.disablePitchMinOverride();
+		}
+		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
+				limitOverride);
 	} else {
+		/* Using tecs library */
 		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,
 					    climbout_mode, climbout_pitch_min_rad,
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 087e1b476..32f9f19ca 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -79,7 +79,8 @@ mTecs::~mTecs()
 {
 }
 
-int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode)
+int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
+		float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(altitude) ||
@@ -105,10 +106,12 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 
 
 	/* use flightpath angle setpoint for total energy control */
-	return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode);
+	return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
+			airspeedSp, mode, limitOverride);
 }
 
-int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode)
+int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
+		float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
@@ -135,10 +138,12 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
-	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode);
+	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed,
+			accelerationLongitudinalSp, mode, limitOverride);
 }
 
-int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode)
+int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
+		float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
@@ -180,8 +185,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 				(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
 	}
 
-	/* Check airspeed: if below safe value switch to underspeed mode */
-	if (airspeed < _airspeedMin.get()) {
+	/* 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()) {
 		mode = TECS_MODE_UNDERSPEED;
 	}
 
@@ -202,6 +207,16 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
 	}
 
+	/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
+	 * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
+	 * is running) */
+	bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ?
+			_controlTotalEnergy.getOutputLimiter() :
+			*outputLimiterThrottle,
+			outputLimiterPitch == NULL ?
+			_controlEnergyDistribution.getOutputLimiter() :
+			*outputLimiterPitch);
+
 	/* Write part of the status message */
 	_status.airspeedDerivativeSp = airspeedDerivativeSp;
 	_status.airspeedDerivative = airspeedDerivative;
@@ -209,6 +224,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	_status.totalEnergyRate = totalEnergyRate;
 	_status.energyDistributionRateSp = energyDistributionRateSp;
 	_status.energyDistributionRate = energyDistributionRate;
+	_status.mode = mode;
 
 	/** update control blocks **/
 	/* update total energy rate control block */
@@ -245,6 +261,12 @@ void mTecs::resetIntegrators()
 	_firstIterationAfterReset = true;
 }
 
+void mTecs::resetDerivatives(float airspeed)
+{
+	_airspeedDerivative.setU(airspeed);
+}
+
+
 void mTecs::updateTimeMeasurement()
 {
 	if (!_dtCalculated) {
@@ -280,5 +302,29 @@ void mTecs::debug(const char *fmt, ...) {
 	debug_print(fmt, args);
 }
 
-} /* namespace fwPosctrl */
+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 */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 376d39698..ddd6583e8 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -60,34 +60,89 @@ public:
 	mTecs();
 	virtual ~mTecs();
 
-	typedef enum {
-		TECS_MODE_NORMAL,
-		TECS_MODE_UNDERSPEED,
-		TECS_MODE_TAKEOFF,
-		TECS_MODE_LAND,
-		TECS_MODE_LAND_THROTTLELIM
-	} tecs_mode;
+	/* 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
 	 */
-	int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode);
+	int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
+			float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
 	 * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
 	 */
-	int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode);
+	int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
+			float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
 	 * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
 	 */
-	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode);
+	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
+			float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
 	 * Reset all integrators
 	 */
 	void resetIntegrators();
 
+	/*
+	 * Reset all derivative calculations
+	 */
+	void resetDerivatives(float airspeed);
 
 	/* Accessors */
 	bool getEnabled() {return _mTecsEnabled.get() > 0;}
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
index a7acd95de..e4e405227 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -89,6 +89,8 @@ public:
 	bool isAngularLimit() {return _isAngularLimit ;}
 	float getMin() { return _min.get(); }
 	float getMax() { return _max.get(); }
+	void setMin(float value) { _min.set(value); }
+	void setMax(float value) { _max.set(value); }
 protected:
 //attributes
 	bool _isAngularLimit;
@@ -96,7 +98,6 @@ protected:
 	control::BlockParamFloat _max;
 };
 
-typedef
 
 /* A combination of feed forward, P and I gain using the output limiter*/
 class BlockFFPILimited: public SuperBlock
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index 591257611..39514b223 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -58,7 +58,8 @@
 PARAM_DEFINE_INT32(MT_ENABLED, 1);
 
 /**
- * Total Energy Rate Control FF
+ * Total Energy Rate Control Feedforward
+ * Maps the total energy rate setpoint to the throttle setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -68,6 +69,7 @@ PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f);
 
 /**
  * Total Energy Rate Control P
+ * Maps the total energy rate error to the throttle setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -77,6 +79,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f);
 
 /**
  * Total Energy Rate Control I
+ * Maps the integrated total energy rate to the throttle setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -85,7 +88,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f);
 PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f);
 
 /**
- * Total Energy Rate Control OFF (Cruise throttle)
+ * Total Energy Rate Control Offset (Cruise throttle sp)
  *
  * @min 0.0
  * @max 10.0
@@ -94,7 +97,8 @@ PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f);
 PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
 
 /**
- * Energy Distribution Rate Control FF
+ * Energy Distribution Rate Control Feedforward
+ * Maps the energy distribution rate setpoint to the pitch setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -104,6 +108,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f);
 
 /**
  * Energy Distribution Rate Control P
+ * Maps the energy distribution rate error to the pitch setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -113,6 +118,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f);
 
 /**
  * Energy Distribution Rate Control I
+ * Maps the integrated energy distribution rate error to the pitch setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -122,7 +128,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f);
 
 
 /**
- * Total Energy Distribution OFF (Cruise pitch sp)
+ * Total Energy Distribution Offset (Cruise pitch sp)
  *
  * @min 0.0
  * @max 10.0
@@ -170,6 +176,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
 
 /**
  * P gain for the altitude control
+ * Maps the altitude error to the flight path angle setpoint
  *
  * @min 0.0f
  * @max 10.0f
@@ -179,6 +186,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f);
 
 /**
  * D gain for the altitude control
+ * Maps the change of altitude error to the flight path angle setpoint
  *
  * @min 0.0f
  * @max 10.0f
@@ -187,7 +195,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f);
 PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
 
 /**
- * Lowpass for FPA error derivative (see MT_FPA_D)
+ * Lowpass for FPA error derivative calculation (see MT_FPA_D)
  *
  * @group mTECS
  */
@@ -217,6 +225,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
 
 /**
  * P gain for the airspeed control
+ * Maps the airspeed error to the acceleration setpoint
  *
  * @min 0.0f
  * @max 10.0f
@@ -241,7 +250,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
 PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
 
 /**
- * Airspeed derivative lowpass
+ * Airspeed derivative calculation lowpass
  *
  * @group mTECS
  */