diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index f739446fa7..6a0b615910 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -39,6 +39,7 @@ #include #include +#include #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 diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 979b9541be..786bfc06d3 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #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 */ 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 1144387253..fa12c5822a 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 @@ -81,6 +81,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index c07956fd6e..49e1d735e0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/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; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index e114cc3ae0..09d9eec1d5 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -49,11 +49,25 @@ #include #include #include + +#if defined(__PX4_NUTTX) #include +#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 _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 */