From 671c7a115a7c01ed89266a6631fb3929af84ffcf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 21 Apr 2014 16:20:20 +0200 Subject: simple underspeed protection for mtecs --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 27 ++++++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 10 ++++-- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 40 +++++++++++++++++++++- 3 files changed, 65 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 63e60995e..1fb3153e9 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -50,6 +50,7 @@ mTecs::mTecs() : SuperBlock(NULL, "MT"), /* Parameters */ _mTecsEnabled(this, "ENABLED"), + _airspeedMin(this, "FW_AIRSPD_MIN", false), /* control blocks */ _controlTotalEnergy(this, "THR"), _controlEnergyDistribution(this, "PIT", true), @@ -58,11 +59,13 @@ mTecs::mTecs() : _airspeedDerivative(this, "AD"), _throttleSp(0.0f), _pitchSp(0.0f), - BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), - BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), + _BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), + _BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), + _BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"), + _BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true), timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), - dtCalculated(false), + _dtCalculated(false), _counter(0) { } @@ -133,12 +136,20 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } + /* Check airspeed: if below safe value switch to underspeed mode */ + if (airspeed < _airspeedMin.get()) { + mode = TECS_MODE_UNDERSPEED; + } + /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits if (mode == TECS_MODE_TAKEOFF) { - outputLimiterThrottle = &BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector - outputLimiterPitch = &BlockOutputLimiterTakeoffPitch; + outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector + outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; + } else if (mode == TECS_MODE_UNDERSPEED) { + outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; + outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; } /** update control blocks **/ @@ -159,7 +170,7 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh /* clean up */ _firstIterationAfterReset = false; - dtCalculated = false; + _dtCalculated = false; _counter++; } @@ -174,7 +185,7 @@ void mTecs::resetIntegrators() void mTecs::updateTimeMeasurement() { - if (!dtCalculated) { + if (!_dtCalculated) { float deltaTSeconds = 0.0f; if (!_firstIterationAfterReset) { hrt_abstime timestampNow = hrt_absolute_time(); @@ -183,7 +194,7 @@ void mTecs::updateTimeMeasurement() } setDt(deltaTSeconds); - dtCalculated = true; + _dtCalculated = true; } } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 6c30a458a..393dd294d 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -60,6 +60,7 @@ public: typedef enum { TECS_MODE_NORMAL, + TECS_MODE_UNDERSPEED, TECS_MODE_TAKEOFF } tecs_mode; @@ -92,6 +93,7 @@ public: protected: /* parameters */ control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ + control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ /* control blocks */ BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ @@ -107,14 +109,16 @@ 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 _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit during takeoff */ /* Time measurements */ 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 _dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; 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 85a4f3d48..fd501c17a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -247,7 +247,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); */ PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); - /** * Minimal throttle during takeoff * @@ -285,3 +284,42 @@ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f); * @group mTECS */ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f); + +/** + * Minimal throttle in underspeed mode + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f); + +/** + * Maximal throttle in underspeed mode + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f); + +/** + * Minimal pitch in underspeed mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f); + +/** + * Maximal pitch in underspeed mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f); + -- cgit v1.2.3