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 070d7377a0..1bb9551732 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 @@ -1452,7 +1452,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ if (ground_speed_length > FLT_EPSILON) { flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } - fwPosctrl::mTecs::LimitOverride limitOverride; + fwPosctrl::LimitOverride limitOverride; if (climbout_mode) { limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); } else { diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index c887223f4a..af155fe089 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -41,4 +41,5 @@ SRCS = fw_pos_control_l1_main.cpp \ fw_pos_control_l1_params.c \ landingslope.cpp \ mtecs/mTecs.cpp \ + mtecs/limitoverride.cpp \ mtecs/mTecs_params.c diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp new file mode 100644 index 0000000000..58795edb65 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 new file mode 100644 index 0000000000..64c2e7bbde --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 index 32f9f19cac..03353cbc1a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -58,6 +58,7 @@ mTecs::mTecs() : _controlEnergyDistribution(this, "PIT", true), _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), + _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), _pitchSp(0.0f), @@ -122,12 +123,18 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* time measurement */ updateTimeMeasurement(); + /* Filter arispeed */ + float airspeedFiltered = _airspeedLowpass.update(airspeed); + /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ - float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); + float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered); /* Debug output */ if (_counter % 10 == 0) { - debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f," + "accelerationLongitudinalSp%.4f", + (double)airspeedSp, (double)airspeed, + (double)airspeedFiltered, (double)accelerationLongitudinalSp); } /* Write part of the status message */ @@ -135,19 +142,20 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng _status.flightPathAngle = flightPathAngle; _status.airspeedSp = airspeedSp; _status.airspeed = airspeed; + _status.airspeedFiltered = airspeedFiltered; /* use longitudinal acceleration setpoint for total energy control */ - return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, + return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered, accelerationLongitudinalSp, mode, limitOverride); } -int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, +int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || - !isfinite(airspeed) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { + !isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { return -1; } /* time measurement */ @@ -160,7 +168,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float flightPathAngleError = flightPathAngleSp - flightPathAngle; float airspeedDerivative = 0.0f; if(_airspeedDerivative.getDt() > 0.0f) { - airspeedDerivative = _airspeedDerivative.update(airspeed); + airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); } float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; float airspeedDerivativeSp = accelerationLongitudinalSp; @@ -186,7 +194,7 @@ 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_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeed < _airspeedMin.get()) { + if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { mode = TECS_MODE_UNDERSPEED; } @@ -302,29 +310,4 @@ void mTecs::debug(const char *fmt, ...) { debug_print(fmt, args); } -bool mTecs::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/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index ddd6583e86..c102f5dee5 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -44,6 +44,7 @@ #define MTECS_H_ #include "mTecs_blocks.h" +#include "limitoverride.h" #include #include @@ -60,62 +61,6 @@ public: mTecs(); virtual ~mTecs(); - /* A small class which provides helper fucntions 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; }; - - - }; - /* * Control in altitude setpoint and speed mode */ @@ -131,7 +76,7 @@ public: /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ - int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, + int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); /* @@ -145,9 +90,10 @@ public: void resetDerivatives(float airspeed); /* Accessors */ - bool getEnabled() {return _mTecsEnabled.get() > 0;} - float getThrottleSetpoint() {return _throttleSp;} - float getPitchSetpoint() {return _pitchSp;} + bool getEnabled() { return _mTecsEnabled.get() > 0; } + float getThrottleSetpoint() { return _throttleSp; } + float getPitchSetpoint() { return _pitchSp; } + float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } protected: /* parameters */ @@ -160,11 +106,12 @@ protected: /* 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; /**< P controller for altitude: output is the flight path angle setpoint */ - BlockPLimited _controlAirSpeed; /**< P controller for airspeed: output is acceleration setpoint */ + 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::BlockDerivative _airspeedDerivative; + control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ + control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ /* Output setpoints */ float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ 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 39514b2238..c95bf1dc9d 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -222,6 +222,12 @@ PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f); */ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); +/** + * Lowpass (cutoff freq.) for airspeed + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); /** * P gain for the airspeed control @@ -233,6 +239,23 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); */ PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f); +/** + * D gain for the airspeed control + * Maps the change of airspeed error to the acceleration setpoint + * + * @min 0.0f + * @max 10.0f + * @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, 1.0f); + /** * Minimal acceleration (air) * diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c8d19af013..53cd6a7970 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1509,6 +1509,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a874351b30..c42ff0afec 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -355,6 +355,7 @@ struct log_TECS_s { float flightPathAngle; float airspeedSp; float airspeed; + float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; @@ -430,7 +431,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "ffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), + LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index fc530b2955..05310e9062 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -70,6 +70,7 @@ struct tecs_status_s { float flightPathAngle; float airspeedSp; float airspeed; + float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative;