From f9946c98a809d18e0a037ee45f39195fd92c62fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Jun 2014 21:03:27 +0200 Subject: mtecs: filter airspeed --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 22 +++++++++++++++------- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 12 +++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 6 ++++++ src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- src/modules/uORB/topics/tecs_status.h | 1 + 6 files changed, 32 insertions(+), 13 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 5894333f3..03353cbc1 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; } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 0369640f2..c102f5dee 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -76,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); /* @@ -90,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 */ @@ -109,7 +110,8 @@ protected: 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 6165611b9..c95bf1dc9 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 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c19579f0f..0813bf7b0 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 a874351b3..c42ff0afe 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 fc530b295..05310e906 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; -- cgit v1.2.3