From b7847e13a689440515c80d7ee1b197a58021ca48 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 8 Aug 2015 09:25:01 +0200 Subject: [PATCH 1/7] mtecs: quick hack: only write tecs topic on nuttx, need to solve this cleanly with inheritance. Introduced own mode enum. --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 24 +++++++++++++------ src/modules/fw_pos_control_l1/mtecs/mTecs.h | 16 +++++++++++++ 2 files changed, 33 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index c07956fd6e..36ce58f2ff 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -52,7 +52,9 @@ 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), @@ -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..990cb62e2d 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 { @@ -101,7 +115,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 From 040d1da8a146727cf9e581583845cb45e18a582f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 6 Sep 2015 15:46:59 +0200 Subject: [PATCH 2/7] add 2nd order low pass block --- src/modules/controllib/blocks.cpp | 8 ++++++++ src/modules/controllib/blocks.hpp | 28 ++++++++++++++++++++++++++++ 2 files changed, 36 insertions(+) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index f739446fa7..ed6bfb0f85 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -198,6 +198,14 @@ int blockHighPassTest() return 0; } +float BlockLowPass2::update(float input) +{ + if (_lp.get_cutoff_freq() != getFCutParam()) { + _lp.set_cutoff_frequency(_fs, getFCutParam()); + } + return _lp.apply(input); +} + float BlockIntegral::update(float input) { // trapezoidal integration diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 979b9541be..ded54e9988 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,33 @@ protected: int __EXPORT blockHighPassTest(); +/** + * A 2nd order low pass filter block which uses the 2nd order low pass filter used by px4 + */ +class __EXPORT BlockLowPass2 : public Block +{ +public: +// methods + BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) : + Block(parent, name), + _fCut(this, ""), // only one parameter, no need to name + _fs(sample_freq), + _lp(_fs, _fCut.get()) + {}; + virtual ~BlockLowPass2() {}; + float update(float input); +// accessors + float getFCutParam() { return _fCut.get(); } + void setState(float state) { _lp.reset(state); } +protected: +// attributes + control::BlockParamFloat _fCut; + float _fs; + math::LowPassFilter2p _lp; +}; + +// XXX missing test function for BlockLowPass2 + /** * A rectangular integrator. * A limiter is built into the class to bound the From 2c5d810b065e9fc93be5a93109c527c733e0ab7d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 6 Sep 2015 15:47:16 +0200 Subject: [PATCH 3/7] make mtecs use 2nd order low pass block --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 6 +++--- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 36ce58f2ff..49e1d735e0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -60,9 +60,9 @@ mTecs::mTecs() : _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), diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 990cb62e2d..0d6f2613a6 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -130,9 +130,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 */ From b4ee05da03043aed4e6d165eb77b3e02933f3127 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 9 Sep 2015 06:24:00 +0200 Subject: [PATCH 4/7] add more accessors for 2nd order lp block and derivative block --- src/modules/controllib/blocks.cpp | 7 ++++++- src/modules/controllib/blocks.hpp | 6 +++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index ed6bfb0f85..d97fa76974 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -200,10 +200,15 @@ int blockHighPassTest() float BlockLowPass2::update(float input) { + if (!isfinite(getState())) { + setState(input); + } + if (_lp.get_cutoff_freq() != getFCutParam()) { _lp.set_cutoff_frequency(_fs, getFCutParam()); } - return _lp.apply(input); + _state = _lp.apply(input); + return _state; } float BlockIntegral::update(float input) diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index ded54e9988..7084e69e0d 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -173,6 +173,7 @@ 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()) @@ -180,10 +181,12 @@ public: virtual ~BlockLowPass2() {}; float update(float input); // accessors + float getState() { return _state; } float getFCutParam() { return _fCut.get(); } - void setState(float state) { _lp.reset(state); } + void setState(float state) { _state = _lp.reset(state); } protected: // attributes + float _state; control::BlockParamFloat _fCut; float _fs; math::LowPassFilter2p _lp; @@ -291,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 */ From bd2ed0c93bb4d496fe37a669f08c9b0f70aa6266 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 9 Sep 2015 06:25:00 +0200 Subject: [PATCH 5/7] mtecs: add getters for some internal values --- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 0d6f2613a6..09d9eec1d5 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -108,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 */ From ee001597ff86e5bed90a99eaf7a7d030964dab76 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 11 Oct 2015 15:20:40 +0200 Subject: [PATCH 6/7] fix posix build --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 + 1 file changed, 1 insertion(+) 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 c3816f5374..d8bc71c463 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 From a87776eb81b0517053d847dbe1c5fd4e5843add9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 11 Oct 2015 16:06:41 +0200 Subject: [PATCH 7/7] add test for BlockLowPass2 --- src/modules/controllib/blocks.cpp | 32 ++++++++++++++++++++++++++++++- src/modules/controllib/blocks.hpp | 4 ++-- 2 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index d97fa76974..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(); @@ -204,13 +206,41 @@ float BlockLowPass2::update(float input) setState(input); } - if (_lp.get_cutoff_freq() != getFCutParam()) { + 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 7084e69e0d..786bfc06d3 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -165,7 +165,7 @@ protected: int __EXPORT blockHighPassTest(); /** - * A 2nd order low pass filter block which uses the 2nd order low pass filter used by px4 + * A 2nd order low pass filter block which uses the default px4 2nd order low pass filter */ class __EXPORT BlockLowPass2 : public Block { @@ -192,7 +192,7 @@ protected: math::LowPassFilter2p _lp; }; -// XXX missing test function for BlockLowPass2 +int __EXPORT blockLowPass2Test(); /** * A rectangular integrator.