mtecs: filter airspeed

6 files changed, 32 insertions, 13 deletions

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 5894333f3..03353cbc1 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/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;
 	}
 
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 0369640f2..c102f5dee 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -76,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);
 
 	/*
@@ -90,9 +90,10 @@ public:
 	void resetDerivatives(float airspeed);
 
 	/* Accessors */
-	bool getEnabled() {return _mTecsEnabled.get() > 0;}
-	float getThrottleSetpoint() {return _throttleSp;}
-	float getPitchSetpoint() {return _pitchSp;}
+	bool getEnabled() { return _mTecsEnabled.get() > 0; }
+	float getThrottleSetpoint() { return _throttleSp; }
+	float getPitchSetpoint() { return _pitchSp; }
+	float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
 
 protected:
 	/* parameters */
@@ -109,7 +110,8 @@ protected:
 	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 */
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 6165611b9..c95bf1dc9 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/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
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index c19579f0f..0813bf7b0 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1509,6 +1509,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;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a874351b3..c42ff0afe 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -355,6 +355,7 @@ struct log_TECS_s {
 	float flightPathAngle;
 	float airspeedSp;
 	float airspeed;
+	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
 
@@ -430,7 +431,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 */
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index fc530b295..05310e906 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/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;