fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 01:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mtecs: filter airspeed

Browse Source
This commit is contained in:
Thomas Gubler
2014-06-11 21:03:27 +02:00
parent a1bcd5d313
commit f9946c98a8
6 changed files with 32 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+15 -7
View File
@@ -58,6 +58,7 @@ mTecs::mTecs() :
_controlEnergyDistribution(this, "PIT", true), _controlEnergyDistribution(this, "PIT", true),
_controlAltitude(this, "FPA", true), _controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"), _controlAirSpeed(this, "ACC"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"), _airspeedDerivative(this, "AD"),
_throttleSp(0.0f), _throttleSp(0.0f),
_pitchSp(0.0f), _pitchSp(0.0f),
@@ -122,12 +123,18 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
/* time measurement */ /* time measurement */
updateTimeMeasurement(); updateTimeMeasurement();
/* Filter arispeed */
float airspeedFiltered = _airspeedLowpass.update(airspeed);
/* calculate longitudinal acceleration setpoint from airspeed setpoint*/ /* calculate longitudinal acceleration setpoint from airspeed setpoint*/
float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered);
/* Debug output */ /* Debug output */
if (_counter % 10 == 0) { 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 */ /* Write part of the status message */
@@ -135,19 +142,20 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
_status.flightPathAngle = flightPathAngle; _status.flightPathAngle = flightPathAngle;
_status.airspeedSp = airspeedSp; _status.airspeedSp = airspeedSp;
_status.airspeed = airspeed; _status.airspeed = airspeed;
_status.airspeedFiltered = airspeedFiltered;
/* use longitudinal acceleration setpoint for total energy control */ /* use longitudinal acceleration setpoint for total energy control */
return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered,
accelerationLongitudinalSp, mode, limitOverride); 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) float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
{ {
/* check if all input arguments are numbers and abort if not so */ /* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
!isfinite(airspeed) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { !isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
return -1; return -1;
} }
/* time measurement */ /* time measurement */
@@ -160,7 +168,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
float flightPathAngleError = flightPathAngleSp - flightPathAngle; float flightPathAngleError = flightPathAngleSp - flightPathAngle;
float airspeedDerivative = 0.0f; float airspeedDerivative = 0.0f;
if(_airspeedDerivative.getDt() > 0.0f) { if(_airspeedDerivative.getDt() > 0.0f) {
airspeedDerivative = _airspeedDerivative.update(airspeed); airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
} }
float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G;
float airspeedDerivativeSp = accelerationLongitudinalSp; 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) */ /* 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; mode = TECS_MODE_UNDERSPEED;
} }
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+7 -5
View File
@@ -76,7 +76,7 @@ public:
/* /*
* Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) * 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); float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
/* /*
@@ -90,9 +90,10 @@ public:
void resetDerivatives(float airspeed); void resetDerivatives(float airspeed);
/* Accessors */ /* Accessors */
bool getEnabled() {return _mTecsEnabled.get() > 0;} bool getEnabled() { return _mTecsEnabled.get() > 0; }
float getThrottleSetpoint() {return _throttleSp;} float getThrottleSetpoint() { return _throttleSp; }
float getPitchSetpoint() {return _pitchSp;} float getPitchSetpoint() { return _pitchSp; }
float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
protected: protected:
/* parameters */ /* parameters */
@@ -109,7 +110,8 @@ protected:
BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */ BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */
/* Other calculation Blocks */ /* Other calculation Blocks */
control::BlockDerivative _airspeedDerivative; control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
/* Output setpoints */ /* Output setpoints */
float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ float _throttleSp; /**< Throttle Setpoint from 0 to 1 */
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+6
View File
@@ -222,6 +222,12 @@ PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f);
*/ */
PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.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 * P gain for the airspeed control
src/modules/sdlog2/sdlog2.c
+1
View File
@@ -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.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; 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.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.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
src/modules/sdlog2/sdlog2_messages.h
+2 -1
View File
@@ -355,6 +355,7 @@ struct log_TECS_s {
float flightPathAngle; float flightPathAngle;
float airspeedSp; float airspeedSp;
float airspeed; float airspeed;
float airspeedFiltered;
float airspeedDerivativeSp; float airspeedDerivativeSp;
float airspeedDerivative; 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(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(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(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"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
/* system-level messages, ID >= 0x80 */ /* system-level messages, ID >= 0x80 */
src/modules/uORB/topics/tecs_status.h
+1
View File
@@ -70,6 +70,7 @@ struct tecs_status_s {
float flightPathAngle; float flightPathAngle;
float airspeedSp; float airspeedSp;
float airspeed; float airspeed;
float airspeedFiltered;
float airspeedDerivativeSp; float airspeedDerivativeSp;
float airspeedDerivative; float airspeedDerivative;