Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: Add parameter to adjust height rate gain during flare

Browse Source
This commit is contained in:
priseborough 2014-11-28 19:32:15 +11:00 committed by Andrew Tridgell
parent 11aefa6858
commit 0912f05e7c
2 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_TECS/AP_TECS.cpp
View File

@ -168,6 +168,14 @@ const AP_Param::GroupInfo AP_TECS::var_info[] PROGMEM = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("LAND_TCONST", 18, AP_TECS, _landTimeConst, 2.0f), AP_GROUPINFO("LAND_TCONST", 18, AP_TECS, _landTimeConst, 2.0f),
// @Param: LAND_DAMP
// @DisplayName: Controller sink rate to pitch gain during flare
// @Description: This is the sink rate gain for the pitch demand loop when in final landing stage of flight. It should be larger than TECS_PTCH_DAMP to allow for better sink rate control during flare.
// @Range: 0.1 1.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("LAND_DAMP", 19, AP_TECS, _landDamp, 0.0f),
AP_GROUPEND AP_GROUPEND
}; };
@ -647,11 +655,16 @@ void AP_TECS::_update_pitch(void)
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout. // integrator has to catch up before the nose can be raised to reduce speed during climbout.
// During flare a different damping gain is used
float gainInv = (_integ5_state * timeConstant() * GRAVITY_MSS); float gainInv = (_integ5_state * timeConstant() * GRAVITY_MSS);
float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * timeConstant(); float temp = SEB_error + SEBdot_dem * timeConstant();
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF) if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
{ temp += SEBdot_error * _landDamp;
temp += _PITCHminf * gainInv; } else {
temp += SEBdot_error * _ptchDamp;
}
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF) {
temp += _PITCHminf * gainInv;
} }
_integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); _integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);

3
libraries/AP_TECS/AP_TECS.h
View File

@ -117,6 +117,7 @@ private:
AP_Float _timeConst; AP_Float _timeConst;
AP_Float _landTimeConst; AP_Float _landTimeConst;
AP_Float _ptchDamp; AP_Float _ptchDamp;
AP_Float _landDamp;
AP_Float _thrDamp; AP_Float _thrDamp;
AP_Float _integGain; AP_Float _integGain;
AP_Float _vertAccLim; AP_Float _vertAccLim;
@ -186,7 +187,7 @@ private:
float _hgt_dem_adj_last; float _hgt_dem_adj_last;
float _hgt_rate_dem; float _hgt_rate_dem;
float _hgt_dem_prev; float _hgt_dem_prev;
float _initial_land_hgt; float _land_hgt_dem;
// Speed demand after application of rate limiting // Speed demand after application of rate limiting
// This is the demand tracked by the TECS control loops // This is the demand tracked by the TECS control loops