diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp deleted file mode 100644 index 66dbf81f70..0000000000 --- a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp +++ /dev/null @@ -1,75 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - - -/** - * @file limitoverride.cpp - * - * @author Thomas Gubler - */ - -#include "limitoverride.h" - -namespace fwPosctrl -{ - -bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, - BlockOutputLimiter &outputLimiterPitch) -{ - bool ret = false; - - if (overrideThrottleMinEnabled) { - outputLimiterThrottle.setMin(overrideThrottleMin); - ret = true; - } - - if (overrideThrottleMaxEnabled) { - outputLimiterThrottle.setMax(overrideThrottleMax); - ret = true; - } - - if (overridePitchMinEnabled) { - outputLimiterPitch.setMin(overridePitchMin); - ret = true; - } - - if (overridePitchMaxEnabled) { - outputLimiterPitch.setMax(overridePitchMax); - ret = true; - } - - return ret; -} - -} /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h deleted file mode 100644 index bba8b9cb66..0000000000 --- a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h +++ /dev/null @@ -1,119 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - - -/** - * @file limitoverride.h - * - * @author Thomas Gubler - */ - - -#ifndef LIMITOVERRIDE_H_ -#define LIMITOVERRIDE_H_ - -#include "mTecs_blocks.h" - -namespace fwPosctrl -{ - -/* A small class which provides helper functions to override control output limits which are usually set by -* parameters in special cases -*/ -class LimitOverride -{ -public: - LimitOverride() : - overrideThrottleMinEnabled(false), - overrideThrottleMaxEnabled(false), - overridePitchMinEnabled(false), - overridePitchMaxEnabled(false) - {} - - ~LimitOverride() {} - - /* - * Override the limits of the outputlimiter instances given by the arguments with the limits saved in - * this class (if enabled) - * @return true if the limit was applied - */ - bool applyOverride(BlockOutputLimiter &outputLimiterThrottle, - BlockOutputLimiter &outputLimiterPitch); - - /* Functions to enable or disable the override */ - void enableThrottleMinOverride(float value) - { - enable(&overrideThrottleMinEnabled, - &overrideThrottleMin, value); - } - void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); } - void enableThrottleMaxOverride(float value) - { - enable(&overrideThrottleMaxEnabled, - &overrideThrottleMax, value); - } - void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); } - void enablePitchMinOverride(float value) - { - enable(&overridePitchMinEnabled, - &overridePitchMin, value); - } - void disablePitchMinOverride() { disable(&overridePitchMinEnabled); } - void enablePitchMaxOverride(float value) - { - enable(&overridePitchMaxEnabled, - &overridePitchMax, value); - } - void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); } - -protected: - bool overrideThrottleMinEnabled; - float overrideThrottleMin; - bool overrideThrottleMaxEnabled; - float overrideThrottleMax; - bool overridePitchMinEnabled; - float overridePitchMin; //in degrees (replaces param values) - bool overridePitchMaxEnabled; - float overridePitchMax; //in degrees (replaces param values) - - /* Enable a specific limit override */ - void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; }; - - /* Disable a specific limit override */ - void disable(bool *flag) { *flag = false; }; -}; - -} /* namespace fwPosctrl */ - -#endif /* LIMITOVERRIDE_H_ */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp deleted file mode 100644 index fdd5ca04d4..0000000000 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ /dev/null @@ -1,342 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - - -/** - * @file mTecs.cpp - * - * @author Thomas Gubler - */ - -#include "mTecs.h" - -#include -#include - -namespace fwPosctrl -{ - -mTecs::mTecs() : - SuperBlock(NULL, "MT"), - /* Parameters */ - _mTecsEnabled(this, "ENABLED"), - _airspeedMin(this, "FW_AIRSPD_MIN", false), - /* Publications */ -#if defined(__PX4_NUTTX) - _status(ORB_ID(tecs_status), -1, &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", 50), - _altitudeLowpass(this, "ALT_LP", 50), - _airspeedLowpass(this, "A_LP", 50), - _airspeedDerivative(this, "AD"), - _throttleSp(0.0f), - _pitchSp(0.0f), - _BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), - _BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), - _BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"), - _BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true), - _BlockOutputLimiterLandThrottle(this, "LND_THR"), - _BlockOutputLimiterLandPitch(this, "LND_PIT", true), - timestampLastIteration(hrt_absolute_time()), - _firstIterationAfterReset(true), - _dtCalculated(false), - _counter(0), - _debug(false) -{ -} - -mTecs::~mTecs() -{ -} - -int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, - float airspeedSp, unsigned mode, LimitOverride limitOverride) -{ - /* check if all input arguments are numbers and abort if not so */ - if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) || - !PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { - return -1; - } - - /* time measurement */ - updateTimeMeasurement(); - - /* Filter altitude */ - float altitudeFiltered = _altitudeLowpass.update(altitude); - - - /* calculate flight path angle setpoint from altitude setpoint */ - float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered); - - /* Debug output */ - if (_counter % 10 == 0) { - debug("***"); - 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.get().altitudeSp = altitudeSp; - _status.get().altitude_filtered = altitudeFiltered; -#endif // defined(__PX4_NUTTX) - - - /* use flightpath angle setpoint for total energy control */ - return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, - airspeedSp, mode, limitOverride); -} - -int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, - float airspeedSp, unsigned mode, LimitOverride limitOverride) -{ - /* check if all input arguments are numbers and abort if not so */ - if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || - !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { - return -1; - } - - /* time measurement */ - updateTimeMeasurement(); - - /* Filter airspeed */ - float airspeedFiltered = _airspeedLowpass.update(airspeed); - - /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ - float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered); - - /* Debug output */ - if (_counter % 10 == 0) { - debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f," - "accelerationLongitudinalSp%.4f", - (double)airspeedSp, (double)airspeed, - (double)airspeedFiltered, (double)accelerationLongitudinalSp); - } - -#if defined(__PX4_NUTTX) - /* Write part of the status message */ - _status.get().airspeedSp = airspeedSp; - _status.get().airspeed_filtered = airspeedFiltered; -#endif // defined(__PX4_NUTTX) - - - /* use longitudinal acceleration setpoint for total energy control */ - return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered, - accelerationLongitudinalSp, mode, limitOverride); -} - -int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, - float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride) -{ - /* check if all input arguments are numbers and abort if not so */ - if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || - !PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) { - return -1; - } - - /* time measurement */ - updateTimeMeasurement(); - - /* update parameters first */ - updateParams(); - - /* Filter flightpathangle */ - float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle); - - /* calculate values (energies) */ - float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered; - float airspeedDerivative = 0.0f; - - if (_airspeedDerivative.getDt() > 0.0f) { - airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); - } - - float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; - float airspeedDerivativeSp = accelerationLongitudinalSp; - float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; - float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm; - - float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm; - float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError; - float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp; - float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; - - float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm; - float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError; - float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp; - float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; - - /* Debug output */ - if (_counter % 10 == 0) { - debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", - (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, - (double)airspeedDerivativeNorm); - debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", - (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, - (double)energyDistributionRateError2); - } - - /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ - 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 MTECS_MODE_NORMAL */ - BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); - BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); - - if (mode == MTECS_MODE_TAKEOFF) { - outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; - outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; - - } else if (mode == MTECS_MODE_LAND) { - // only limit pitch but do not limit throttle - outputLimiterPitch = &_BlockOutputLimiterLandPitch; - - } else if (mode == MTECS_MODE_LAND_THROTTLELIM) { - outputLimiterThrottle = &_BlockOutputLimiterLandThrottle; - outputLimiterPitch = &_BlockOutputLimiterLandPitch; - - } else if (mode == MTECS_MODE_UNDERSPEED) { - outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; - outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; - } - - /* Apply override given by the limitOverride argument (this is used for limits which are not given by - * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector - * is running) */ - limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); - -// #if defined(__PX4_NUTTX) - // /* Write part of the status message */ - // _status.flightPathAngleSp = flightPathAngleSp; - // _status.flightPathAngle = flightPathAngle; - // _status.flightPathAngleFiltered = flightPathAngleFiltered; - // _status.airspeedDerivativeSp = airspeedDerivativeSp; - // _status.airspeedDerivative = airspeedDerivative; - // _status.totalEnergyRateSp = totalEnergyRateSp; - // _status.totalEnergyRate = totalEnergyRate; - // _status.energyDistributionRateSp = energyDistributionRateSp; - // _status.energyDistributionRate = energyDistributionRate; - // _status.mode = mode; -// #endif // defined(__PX4_NUTTX) - - /** update control blocks **/ - /* update total energy rate control block */ - _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle); - - /* update energy distribution rate control block */ - _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch); - - - if (_counter % 10 == 0) { - debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", - (double)_throttleSp, (double)_pitchSp, - (double)flightPathAngleSp, (double)flightPathAngle, - (double)accelerationLongitudinalSp, (double)airspeedDerivative); - } - -#if defined(__PX4_NUTTX) - /* publish status message */ - _status.update(); -#endif // defined(__PX4_NUTTX) - - /* clean up */ - _firstIterationAfterReset = false; - _dtCalculated = false; - - _counter++; - - return 0; -} - -void mTecs::resetIntegrators() -{ - _controlTotalEnergy.getIntegral().setY(0.0f); - _controlEnergyDistribution.getIntegral().setY(0.0f); - timestampLastIteration = hrt_absolute_time(); - _firstIterationAfterReset = true; -} - -void mTecs::resetDerivatives(float airspeed) -{ - _airspeedDerivative.setU(airspeed); -} - - -void mTecs::updateTimeMeasurement() -{ - if (!_dtCalculated) { - float deltaTSeconds = 0.0f; - - if (!_firstIterationAfterReset) { - hrt_abstime timestampNow = hrt_absolute_time(); - deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; - timestampLastIteration = timestampNow; - } - - setDt(deltaTSeconds); - - _dtCalculated = true; - } -} - -void mTecs::debug_print(const char *fmt, va_list args) -{ - fprintf(stderr, "%s: ", "[mtecs]"); - vfprintf(stderr, fmt, args); - - fprintf(stderr, "\n"); -} - -void mTecs::debug(const char *fmt, ...) -{ - - if (!_debug) { - return; - } - - va_list args; - - va_start(args, fmt); - debug_print(fmt, args); - va_end(args); -} - -} /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h deleted file mode 100644 index 09d9eec1d5..0000000000 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ /dev/null @@ -1,176 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - - -/** - * @file mTecs.h - * - * @author Thomas Gubler - */ - - -#ifndef MTECS_H_ -#define MTECS_H_ - -#include "mTecs_blocks.h" -#include "limitoverride.h" - -#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 -{ -public: - mTecs(); - virtual ~mTecs(); - - /* - * Control in altitude setpoint and speed mode - */ - int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, - float airspeedSp, unsigned mode, LimitOverride limitOverride); - - /* - * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode - */ - int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, - float airspeedSp, unsigned mode, LimitOverride limitOverride); - - /* - * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) - */ - int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, - float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride); - - /* - * Reset all integrators - */ - void resetIntegrators(); - - /* - * Reset all derivative calculations - */ - void resetDerivatives(float airspeed); - - /* Accessors */ - bool getEnabled() { return _mTecsEnabled.get() > 0; } - 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 */ - control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ - 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 - is throttle */ - BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: - output is pitch */ - BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight - path angle setpoint */ - BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration - setpoint */ - - /* Other calculation Blocks */ - 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 */ - float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ - 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 _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ - BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in - last phase)*/ - BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ - - /* 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 */ - - int _counter; - bool _debug; ///< Set true to enable debug output - - - static void debug_print(const char *fmt, va_list args); - void debug(const char *fmt, ...); - - /* - * Measure and update the time step dt if this was not already done in the current iteration - */ - void updateTimeMeasurement(); -}; - -} /* namespace fwPosctrl */ - -#endif /* MTECS_H_ */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h deleted file mode 100644 index b82eb08404..0000000000 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ /dev/null @@ -1,230 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - - -/** - * @file mTecs_blocks.h - * - * Custom blocks for the mTecs - * - * @author Thomas Gubler - */ - -#pragma once - -#include -#include - -namespace fwPosctrl -{ - -using namespace control; - -/* An block which can be used to limit the output */ -class BlockOutputLimiter: public SuperBlock -{ -public: -// methods - BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) : - SuperBlock(parent, name), - _isAngularLimit(fAngularLimit), - _min(this, "MIN"), - _max(this, "MAX") - {} - virtual ~BlockOutputLimiter() {} - /* - * Imposes the limits given by _min and _max on value - * - * @param value is changed to be on the interval _min to _max - * @param difference if the value is changed this corresponds to the change of value * (-1) - * otherwise unchanged - * @return: true if the limit is applied, false otherwise - */ - bool limit(float &value, float &difference) - { - float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); - float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); - - if (value < minimum) { - difference = value - minimum; - value = minimum; - return true; - - } else if (value > maximum) { - difference = value - maximum; - value = maximum; - return true; - } - - return false; - } -//accessor: - bool getIsAngularLimit() {return _isAngularLimit ;} - float getMin() { return _min.get(); } - float getMax() { return _max.get(); } - void setMin(float value) { _min.set(value); } - void setMax(float value) { _max.set(value); } -protected: -//attributes - bool _isAngularLimit; - control::BlockParamFloat _min; - control::BlockParamFloat _max; -}; - - -/* A combination of feed forward, P and I gain using the output limiter*/ -class BlockFFPILimited: public SuperBlock -{ -public: -// methods - BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : - SuperBlock(parent, name), - _outputLimiter(this, "", isAngularLimit), - _integral(this, "I"), - _kFF(this, "FF"), - _kP(this, "P"), - _kI(this, "I"), - _offset(this, "OFF") - {} - virtual ~BlockFFPILimited() {} - float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); } -// accessors - BlockIntegral &getIntegral() { return _integral; } - float getKFF() { return _kFF.get(); } - float getKP() { return _kP.get(); } - float getKI() { return _kI.get(); } - float getOffset() { return _offset.get(); } - BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; -protected: - BlockOutputLimiter _outputLimiter; - - float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);} - float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) - { - float difference = 0.0f; - float integralYPrevious = _integral.getY(); - float output = calcUnlimitedOutput(inputValue, inputError); - - if (outputLimiter.limit(output, difference) && - (((difference < 0) && (getKI() * getIntegral().getY() < 0)) || - ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) { - getIntegral().setY(integralYPrevious); - } - - return output; - } -private: - BlockIntegral _integral; - BlockParamFloat _kFF; - BlockParamFloat _kP; - BlockParamFloat _kI; - BlockParamFloat _offset; -}; - -/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/ -class BlockFFPILimitedCustom: public BlockFFPILimited -{ -public: -// methods - BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) : - BlockFFPILimited(parent, name, isAngularLimit) - {} - virtual ~BlockFFPILimitedCustom() {} - float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) - { - return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter); - } -}; - -/* A combination of P gain and output limiter */ -class BlockPLimited: public SuperBlock -{ -public: -// methods - BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : - SuperBlock(parent, name), - _kP(this, "P"), - _outputLimiter(this, "", isAngularLimit) - {} - virtual ~BlockPLimited() {} - float update(float input) - { - float difference = 0.0f; - float output = getKP() * input; - getOutputLimiter().limit(output, difference); - return output; - } -// accessors - BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; - float getKP() { return _kP.get(); } -private: - control::BlockParamFloat _kP; - BlockOutputLimiter _outputLimiter; -}; - -/* A combination of P, D gains and output limiter */ -class BlockPDLimited: public SuperBlock -{ -public: -// methods - BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : - SuperBlock(parent, name), - _kP(this, "P"), - _kD(this, "D"), - _derivative(this, "D"), - _outputLimiter(this, "", isAngularLimit) - {} - virtual ~BlockPDLimited() {} - float update(float input) - { - float difference = 0.0f; - float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f); - getOutputLimiter().limit(output, difference); - - return output; - } -// accessors - float getKP() { return _kP.get(); } - float getKD() { return _kD.get(); } - BlockDerivative &getDerivative() { return _derivative; } - BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; -private: - control::BlockParamFloat _kP; - control::BlockParamFloat _kD; - BlockDerivative _derivative; - BlockOutputLimiter _outputLimiter; -}; - -} - diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c deleted file mode 100644 index a6a7b210c0..0000000000 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ /dev/null @@ -1,419 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mTecs_params.c - * - * @author Thomas Gubler - */ - -/* - * Controller parameters, accessible via MAVLink - */ - -/** - * mTECS enabled - * - * @boolean - * @group mTECS - */ -PARAM_DEFINE_INT32(MT_ENABLED, 0); - -/** - * Total Energy Rate Control Feedforward - * Maps the total energy rate setpoint to the throttle setpoint - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_THR_FF, 0.7f); - -/** - * Total Energy Rate Control P - * Maps the total energy rate error to the throttle setpoint - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); - -/** - * Total Energy Rate Control I - * Maps the integrated total energy rate to the throttle setpoint - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_THR_I, 0.25f); - -/** - * Total Energy Rate Control Offset (Cruise throttle sp) - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); - -/** - * Energy Distribution Rate Control Feedforward - * Maps the energy distribution rate setpoint to the pitch setpoint - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.4f); - -/** - * Energy Distribution Rate Control P - * Maps the energy distribution rate error to the pitch setpoint - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f); - -/** - * Energy Distribution Rate Control I - * Maps the integrated energy distribution rate error to the pitch setpoint - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f); - -/** - * Total Energy Distribution Offset (Cruise pitch sp) - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f); - -/** - * Minimal Throttle Setpoint - * - * @min 0.0 - * @max 1.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f); - -/** - * Maximal Throttle Setpoint - * - * @min 0.0 - * @max 1.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f); - -/** - * Minimal Pitch Setpoint in Degrees - * - * @min -90.0 - * @max 90.0 - * @unit deg - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); - -/** - * Maximal Pitch Setpoint in Degrees - * - * @min -90.0 - * @max 90.0 - * @unit deg - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); - -/** - * Lowpass (cutoff freq.) for altitude - * - * @unit Hz - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f); - -/** - * Lowpass (cutoff freq.) for the flight path angle - * - * @unit Hz - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f); - -/** - * P gain for the altitude control - * Maps the altitude error to the flight path angle setpoint - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f); - -/** - * D gain for the altitude control - * Maps the change of altitude error to the flight path angle setpoint - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f); - -/** - * Lowpass for FPA error derivative calculation (see MT_FPA_D) - * - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); - -/** - * Minimal flight path angle setpoint - * - * @min -90.0 - * @max 90.0 - * @unit deg - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f); - -/** - * Maximal flight path angle setpoint - * - * @min -90.0 - * @max 90.0 - * @unit deg - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); - -/** - * Lowpass (cutoff freq.) for airspeed - * - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f); - -/** - * Airspeed derivative calculation lowpass - * - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f); - -/** - * P gain for the airspeed control - * Maps the airspeed error to the acceleration setpoint - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f); - -/** - * D gain for the airspeed control - * Maps the change of airspeed error to the acceleration setpoint - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); - -/** - * Lowpass for ACC error derivative calculation (see MT_ACC_D) - * - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f); - -/** - * Minimal acceleration (air) - * - * @unit m/s/s - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); - -/** - * Maximal acceleration (air) - * - * @unit m/s/s - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); - -/** - * Minimal throttle during takeoff - * - * @min 0.0 - * @max 1.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f); - -/** - * Maximal throttle during takeoff - * - * @min 0.0 - * @max 1.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f); - -/** - * Minimal pitch during takeoff - * - * @min -90.0 - * @max 90.0 - * @unit deg - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f); - -/** - * Maximal pitch during takeoff - * - * @min -90.0 - * @max 90.0 - * @unit deg - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f); - -/** - * Minimal throttle in underspeed mode - * - * @min 0.0 - * @max 1.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f); - -/** - * Maximal throttle in underspeed mode - * - * @min 0.0 - * @max 1.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f); - -/** - * Minimal pitch in underspeed mode - * - * @min -90.0 - * @max 90.0 - * @unit deg - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f); - -/** - * Maximal pitch in underspeed mode - * - * @min -90.0 - * @max 90.0 - * @unit deg - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f); - -/** - * Minimal throttle in landing mode (only last phase of landing) - * - * @min 0.0 - * @max 1.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f); - -/** - * Maximal throttle in landing mode (only last phase of landing) - * - * @min 0.0 - * @max 1.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f); - -/** - * Minimal pitch in landing mode - * - * @min -90.0 - * @max 90.0 - * @unit deg - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f); - -/** - * Maximal pitch in landing mode - * - * @min -90.0 - * @max 90.0 - * @unit deg - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f); - -/** - * Integrator Limit for Total Energy Rate Control - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f); - -/** - * Integrator Limit for Energy Distribution Rate Control - * - * @min 0.0 - * @max 10.0 - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f);