diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-17 17:38:04 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-17 17:38:04 +0200 |
commit | facffe2b9eeee24f712119be91ff8aea99cfde0f (patch) | |
tree | e80fe9a8f668c8151e269d55e5316b72f2826222 /src | |
parent | 213fe0cc20ada8b8581a65f73c68b4efe6508405 (diff) | |
download | px4-firmware-facffe2b9eeee24f712119be91ff8aea99cfde0f.tar.gz px4-firmware-facffe2b9eeee24f712119be91ff8aea99cfde0f.tar.bz2 px4-firmware-facffe2b9eeee24f712119be91ff8aea99cfde0f.zip |
mtecs: add altitude prefiltering
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 10 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs.h | 1 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 7 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 1 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 3 | ||||
-rw-r--r-- | src/modules/uORB/topics/tecs_status.h | 1 |
6 files changed, 20 insertions, 3 deletions
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 2e37d166e..749f57a2b 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -59,6 +59,7 @@ mTecs::mTecs() : _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), _flightPathAngleLowpass(this, "FPA_LP"), + _altitudeLowpass(this, "ALT_LP"), _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), @@ -93,18 +94,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* time measurement */ updateTimeMeasurement(); + /* Filter altitude */ + float altitudeFiltered = _altitudeLowpass.update(altitude); + + /* calculate flight path angle setpoint from altitude setpoint */ - float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered); /* Debug output */ if (_counter % 10 == 0) { debug("***"); - debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); } /* Write part of the status message */ _status.altitudeSp = altitudeSp; _status.altitude = altitude; + _status.altitudeFiltered = altitudeFiltered; /* use flightpath angle setpoint for total energy control */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index efa89a5d3..ae6867d38 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -115,6 +115,7 @@ protected: /* 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::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ 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 5b9238780..4ca31fe20 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -175,6 +175,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); /** + * Lowpass (cutoff freq.) for altitude + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f); + +/** * Lowpass (cutoff freq.) for the flight path angle * * @group mTECS diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f534c0f4c..e0a3e7603 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1595,6 +1595,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; + log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index b14ef04cc..853a3811f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -334,6 +334,7 @@ struct log_GS1B_s { struct log_TECS_s { float altitudeSp; float altitude; + float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; @@ -454,7 +455,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, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), 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 c4d0c1874..33055018c 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -66,6 +66,7 @@ struct tecs_status_s { float altitudeSp; float altitude; + float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; |