Merge pull request #2976 from PX4/mtecs

Mtecs: small update and new filter block

src/modules/controllib/blocks.cpp

@@ -39,6 +39,7 @@
 
 #include <math.h>
 #include <stdio.h>
+#include <float.h>
 
 #include "blocks.hpp"
 
@@ -51,6 +52,7 @@ int basicBlocksTest()
 	blockLimitSymTest();
 	blockLowPassTest();
 	blockHighPassTest();
+	blockLowPass2Test();
 	blockIntegralTest();
 	blockIntegralTrapTest();
 	blockDerivativeTest();
@@ -198,6 +200,47 @@ int blockHighPassTest()
 	return 0;
 }
 
+float BlockLowPass2::update(float input)
+{
+	if (!isfinite(getState())) {
+		setState(input);
+	}
+
+	if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
+		_lp.set_cutoff_frequency(_fs, getFCutParam());
+	}
+	_state = _lp.apply(input);
+	return _state;
+}
+
+int blockLowPass2Test()
+{
+	printf("Test BlockLowPass2\t\t: ");
+	BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
+	// test initial state
+	ASSERT(equal(10.0f, lowPass.getFCutParam()));
+	ASSERT(equal(0.0f, lowPass.getState()));
+	ASSERT(equal(0.0f, lowPass.getDt()));
+	// set dt
+	lowPass.setDt(0.1f);
+	ASSERT(equal(0.1f, lowPass.getDt()));
+	// set state
+	lowPass.setState(1.0f);
+	ASSERT(equal(1.0f, lowPass.getState()));
+	// test update
+	ASSERT(equal(1.06745527f, lowPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		lowPass.update(2.0f);
+	}
+
+	ASSERT(equal(2.0f, lowPass.getState()));
+	ASSERT(equal(2.0f, lowPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+};
+
 float BlockIntegral::update(float input)
 {
 	// trapezoidal integration

src/modules/controllib/blocks.hpp

@@ -45,6 +45,7 @@
 #include <stdlib.h>
 #include <math.h>
 #include <mathlib/math/test/test.hpp>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #include "block/Block.hpp"
 #include "block/BlockParam.hpp"
@@ -163,6 +164,36 @@ protected:
 
 int __EXPORT blockHighPassTest();
 
+/**
+ * A 2nd order low pass filter block which uses the default px4 2nd order low pass filter
+ */
+class __EXPORT BlockLowPass2 : public Block
+{
+public:
+// methods
+	BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) :
+		Block(parent, name),
+		_state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */),
+		_fCut(this, ""), // only one parameter, no need to name
+		_fs(sample_freq),
+		_lp(_fs, _fCut.get())
+	{};
+	virtual ~BlockLowPass2() {};
+	float update(float input);
+// accessors
+	float getState() { return _state; }
+	float getFCutParam() { return _fCut.get(); }
+	void setState(float state) { _state = _lp.reset(state); }
+protected:
+// attributes
+	float _state;
+	control::BlockParamFloat _fCut;
+	float _fs;
+	math::LowPassFilter2p _lp;
+};
+
+int __EXPORT blockLowPass2Test();
+
 /**
  * A rectangular integrator.
  * A limiter is built into the class to bound the
@@ -263,6 +294,7 @@ public:
 	void setU(float u) {_u = u;}
 	float getU() {return _u;}
 	float getLP() {return _lowPass.getFCut();}
+	float getO() { return _lowPass.getState(); }
 protected:
 // attributes
 	float _u; /**< previous input */

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -81,6 +81,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/tecs_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -52,15 +52,17 @@ mTecs::mTecs() :
 	_mTecsEnabled(this, "ENABLED"),
 	_airspeedMin(this, "FW_AIRSPD_MIN", false),
 	/* Publications */
+#if defined(__PX4_NUTTX)
 	_status(ORB_ID(tecs_status), &getPublications()),
+#endif // defined(__PX4_NUTTX)
 	/* control blocks */
 	_controlTotalEnergy(this, "THR"),
 	_controlEnergyDistribution(this, "PIT", true),
 	_controlAltitude(this, "FPA", true),
 	_controlAirSpeed(this, "ACC"),
-	_flightPathAngleLowpass(this, "FPA_LP"),
-	_altitudeLowpass(this, "ALT_LP"),
-	_airspeedLowpass(this, "A_LP"),
+	_flightPathAngleLowpass(this, "FPA_LP", 50),
+	_altitudeLowpass(this, "ALT_LP", 50),
+	_airspeedLowpass(this, "A_LP", 50),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
 	_pitchSp(0.0f),
@@ -107,9 +109,11 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* Write part of the status message */
 	_status.altitudeSp = altitudeSp;
 	_status.altitude_filtered = altitudeFiltered;
+#endif // defined(__PX4_NUTTX)
 
 
 	/* use flightpath angle setpoint for total energy control */
@@ -143,9 +147,11 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 				(double)airspeedFiltered, (double)accelerationLongitudinalSp);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* Write part of the status message */
 	_status.airspeedSp = airspeedSp;
 	_status.airspeed_filtered = airspeedFiltered;
+#endif // defined(__PX4_NUTTX)
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
@@ -200,23 +206,23 @@ 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_status_s::TECS_MODE_LAND && mode != tecs_status_s::TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
-		mode = tecs_status_s::TECS_MODE_UNDERSPEED;
+	if (mode != MTECS_MODE_LAND && mode != MTECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
+		mode = MTECS_MODE_UNDERSPEED;
 	}
 
-	/* Set special output limiters if we are not in TECS_MODE_NORMAL */
+	/* Set special output limiters if we are not in MTECS_MODE_NORMAL */
 	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
 	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
-	if (mode == tecs_status_s::TECS_MODE_TAKEOFF) {
+	if (mode == MTECS_MODE_TAKEOFF) {
 		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_LAND) {
+	} else if (mode == MTECS_MODE_LAND) {
 		// only limit pitch but do not limit throttle
 		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM) {
+	} else if (mode == MTECS_MODE_LAND_THROTTLELIM) {
 		outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_UNDERSPEED) {
+	} else if (mode == MTECS_MODE_UNDERSPEED) {
 		outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
 	}
@@ -226,6 +232,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	 * is running) */
 	limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
+// #if defined(__PX4_NUTTX)
 	// /* Write part of the status message */
 	// _status.flightPathAngleSp = flightPathAngleSp;
 	// _status.flightPathAngle = flightPathAngle;
@@ -237,6 +244,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	// _status.energyDistributionRateSp = energyDistributionRateSp;
 	// _status.energyDistributionRate = energyDistributionRate;
 	// _status.mode = mode;
+// #endif // defined(__PX4_NUTTX)
 
 	/** update control blocks **/
 	/* update total energy rate control block */
@@ -253,8 +261,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 				(double)accelerationLongitudinalSp, (double)airspeedDerivative);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* publish status message */
 	_status.update();
+#endif // defined(__PX4_NUTTX)
 
 	/* clean up */
 	_firstIterationAfterReset = false;

src/modules/fw_pos_control_l1/mtecs/mTecs.h

@@ -49,11 +49,25 @@
 #include <controllib/block/BlockParam.hpp>
 #include <drivers/drv_hrt.h>
 #include <uORB/Publication.hpp>
+
+#if defined(__PX4_NUTTX)
 #include <uORB/topics/tecs_status.h>
+#endif // defined(__PX4_NUTTX)
 
 namespace fwPosctrl
 {
 
+/* corresponds to TECS_MODE in tecs_status.msg */
+enum MTECS_MODE {
+	MTECS_MODE_NORMAL = 0,
+	MTECS_MODE_UNDERSPEED = 1,
+	MTECS_MODE_TAKEOFF = 2,
+	MTECS_MODE_LAND = 3,
+	MTECS_MODE_LAND_THROTTLELIM = 4,
+	MTECS_MODE_BAD_DESCENT = 5,
+	MTECS_MODE_CLIMBOUT = 6
+};
+
 /* Main class of the mTecs */
 class mTecs : public control::SuperBlock
 {
@@ -94,6 +108,10 @@ public:
 	float getThrottleSetpoint() { return _throttleSp; }
 	float getPitchSetpoint() { return _pitchSp; }
 	float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
+	float getFlightPathAngleLowpassState() { return _flightPathAngleLowpass.getState(); }
+	float getAltitudeLowpassState() { return _altitudeLowpass.getState(); }
+	float getAirspeedLowpassState() { return _airspeedLowpass.getState(); }
+	float getAirspeedDerivativeLowpassState() { return _airspeedDerivative.getO(); }
 
 protected:
 	/* parameters */
@@ -101,7 +119,9 @@ protected:
 	control::BlockParamFloat _airspeedMin;		/**< minimal airspeed */
 
 	/* Publications */
+#if defined(__PX4_NUTTX)
 	uORB::Publication<tecs_status_s> _status;	/**< publish internal values for logging */
+#endif // defined(__PX4_NUTTX)
 
 	/* control blocks */
 	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output
@@ -114,9 +134,9 @@ protected:
 								  setpoint */
 
 	/* Other calculation Blocks */
-	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
-	control::BlockLowPass _altitudeLowpass;	/**< low pass filter for altitude */
-	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
+	control::BlockLowPass2 _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
+	control::BlockLowPass2 _altitudeLowpass;	/**< low pass filter for altitude */
+	control::BlockLowPass2 _airspeedLowpass;		/**< low pass filter for airspeed */
 	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
 	/* Output setpoints */