mtecs: add flightpathangle filter

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"),
+	_flightPathAngleLowpass(this, "FPA_LP"),
 	_airspeedLowpass(this, "A_LP"),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
@@ -123,7 +124,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	/* time measurement */
 	updateTimeMeasurement();
 
-	/* Filter arispeed */
+	/* Filter airspeed */
 	float airspeedFiltered = _airspeedLowpass.update(airspeed);
 
 	/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
@@ -138,8 +139,6 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	}
 
 	/* Write part of the status message */
-	_status.flightPathAngleSp = flightPathAngleSp;
-	_status.flightPathAngle = flightPathAngle;
 	_status.airspeedSp = airspeedSp;
 	_status.airspeed = airspeed;
 	_status.airspeedFiltered = airspeedFiltered;
@@ -164,8 +163,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	/* update parameters first */
 	updateParams();
 
+	/* Filter flightpathangle */
+	float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle);
+
 	/* calculate values (energies) */
-	float flightPathAngleError = flightPathAngleSp - flightPathAngle;
+	float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
 	float airspeedDerivative = 0.0f;
 	if(_airspeedDerivative.getDt() > 0.0f) {
 		airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
@@ -175,12 +177,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
 	float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
 
-	float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm;
+	float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm;
 	float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
 	float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
 	float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
 
-	float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm;
+	float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm;
 	float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
 	float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
 	float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
@@ -202,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
 	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
 	if (mode == TECS_MODE_TAKEOFF) {
-		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
+		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
 	} else if (mode == TECS_MODE_LAND) {
 		// only limit pitch but do not limit throttle
@@ -221,6 +223,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
 	/* Write part of the status message */
+	_status.flightPathAngleSp = flightPathAngleSp;
+	_status.flightPathAngle = flightPathAngle;
+	_status.flightPathAngleFiltered = flightPathAngleFiltered;
 	_status.airspeedDerivativeSp = airspeedDerivativeSp;
 	_status.airspeedDerivative = airspeedDerivative;
 	_status.totalEnergyRateSp = totalEnergyRateSp;

src/modules/fw_pos_control_l1/mtecs/mTecs.h

@@ -104,12 +104,17 @@ protected:
 	uORB::Publication<tecs_status_s> _status;	/**< publish internal values for logging */
 
 	/* 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;		/**< PD controller for altitude: output is the flight path angle setpoint */
-	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration setpoint */
+	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output
+								  is throttle */
+	BlockFFPILimitedCustom _controlEnergyDistribution;	/**< FFPI controller for energy distribution control:
+								  output is pitch */
+	BlockPDLimited	_controlAltitude;			/**< PD controller for altitude: output is the flight
+								  path angle setpoint */
+	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration
+								  setpoint */
 
 	/* Other calculation Blocks */
+	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
 	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
 	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
@@ -118,21 +123,22 @@ protected:
 	float _pitchSp;					/**< Pitch Setpoint from -pi to pi */
 
 	/* Output Limits in special modes */
-	BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle;		/**< Throttle Limits during takeoff */
-	BlockOutputLimiter _BlockOutputLimiterTakeoffPitch;      	/**< Pitch Limit during takeoff */
-	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;	/**< Throttle Limits when underspeed is detected */
-	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;   	/**< Pitch Limit when underspeed is detected */
-	BlockOutputLimiter _BlockOutputLimiterLandThrottle;		/**< Throttle Limits during landing (only in last phase)*/
-	BlockOutputLimiter _BlockOutputLimiterLandPitch;		/**< Pitch Limit during landing */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle;	    /**< Throttle Limits during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffPitch;	    /**< Pitch Limit during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;   /**< Throttle Limits when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;	    /**< Pitch Limit when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterLandThrottle;	    /**< Throttle Limits during landing (only in
+								      last phase)*/
+	BlockOutputLimiter _BlockOutputLimiterLandPitch;	    /**< Pitch Limit during landing */
 
 	/* Time measurements */
-	hrt_abstime timestampLastIteration;		/**< Saves the result of hrt_absolute_time() of the last iteration */
+	hrt_abstime timestampLastIteration;	/**< Saves the result of hrt_absolute_time() of the last iteration */
 
-	bool _firstIterationAfterReset;			/**< True during the first iteration after a reset */
-	bool _dtCalculated;				/**< True if dt has been calculated in this iteration */
+	bool _firstIterationAfterReset;		/**< True during the first iteration after a reset */
+	bool _dtCalculated;			/**< True if dt has been calculated in this iteration */
 
 	int _counter;
-	bool _debug;					///< Set true to enable debug output
+	bool _debug;				///< Set true to enable debug output
 
 
 	static void	debug_print(const char *fmt, va_list args);

src/modules/fw_pos_control_l1/mtecs/mTecs_params.c

@@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
  */
 PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
 
+/**
+ * Lowpass (cutoff freq.) for the flight path angle
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f);
+
 /**
  * P gain for the altitude control
  * Maps the altitude error to the flight path angle setpoint

src/modules/sdlog2/sdlog2.c

@@ -1507,6 +1507,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
 			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
+			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
 			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;

src/modules/sdlog2/sdlog2_messages.h

@@ -353,6 +353,7 @@ struct log_TECS_s {
 	float altitude;
 	float flightPathAngleSp;
 	float flightPathAngle;
+	float flightPathAngleFiltered;
 	float airspeedSp;
 	float airspeed;
 	float airspeedFiltered;
@@ -431,7 +432,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, "fffffffffffffB",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "ffffffffffffffB",	"AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */

src/modules/uORB/topics/tecs_status.h

@@ -68,6 +68,7 @@ struct tecs_status_s {
 	float altitude;
 	float flightPathAngleSp;
 	float flightPathAngle;
+	float flightPathAngleFiltered;
 	float airspeedSp;
 	float airspeed;
 	float airspeedFiltered;