dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1090 from PX4/mtecsfpafilter 

mtecs: add flightpathangle filter
This commit is contained in:
Lorenz Meier 2014-06-29 15:19:00 +02:00
parent 7f0b35a1b4 263a2fa9b2
commit 9388b04557
6 changed files with 43 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -58,6 +58,7 @@ mTecs::mTecs() :
_controlEnergyDistribution(this, "PIT", true),
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
@ -123,7 +124,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
/* time measurement */
updateTimeMeasurement();
/* Filter arispeed */
/* Filter airspeed */
float airspeedFiltered = _airspeedLowpass.update(airspeed);
/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
@ -138,8 +139,6 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
}
/* Write part of the status message */
_status.flightPathAngleSp = flightPathAngleSp;
_status.flightPathAngle = flightPathAngle;
_status.airspeedSp = airspeedSp;
_status.airspeed = airspeed;
_status.airspeedFiltered = airspeedFiltered;
@ -164,8 +163,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
/* update parameters first */
updateParams();
/* Filter flightpathangle */
float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle);
/* calculate values (energies) */
float flightPathAngleError = flightPathAngleSp - flightPathAngle;
float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
float airspeedDerivative = 0.0f;
if(_airspeedDerivative.getDt() > 0.0f) {
airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
@ -175,12 +177,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm;
float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm;
float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm;
float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm;
float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
@ -202,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
if (mode == TECS_MODE_TAKEOFF) {
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
} else if (mode == TECS_MODE_LAND) {
// only limit pitch but do not limit throttle
@ -221,6 +223,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
/* Write part of the status message */
_status.flightPathAngleSp = flightPathAngleSp;
_status.flightPathAngle = flightPathAngle;
_status.flightPathAngleFiltered = flightPathAngleFiltered;
_status.airspeedDerivativeSp = airspeedDerivativeSp;
_status.airspeedDerivative = airspeedDerivative;
_status.totalEnergyRateSp = totalEnergyRateSp;

34
src/modules/fw_pos_control_l1/mtecs/mTecs.h
View File

@ -104,12 +104,17 @@ protected:
uORB::Publication<tecs_status_s> _status; /**< publish internal values for logging */
/* 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 */
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::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
@ -118,21 +123,22 @@ protected:
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 */
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 */
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 */
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
bool _debug; ///< Set true to enable debug output
static void debug_print(const char *fmt, va_list args);

7
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
View File

@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
*/
PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
/**
* Lowpass (cutoff freq.) for the flight path angle
*
* @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

1
src/modules/sdlog2/sdlog2.c
View File

@ -1507,6 +1507,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
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;
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;

3
src/modules/sdlog2/sdlog2_messages.h
View File

@ -353,6 +353,7 @@ struct log_TECS_s {
float altitude;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
@ -431,7 +432,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, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,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 */

1
src/modules/uORB/topics/tecs_status.h
View File

@ -68,6 +68,7 @@ struct tecs_status_s {
float altitude;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;