Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: add TECS_LAND_PDAMP

Browse Source 
// @Description: This is the damping gain for the pitch demand loop. Increase to add damping  to correct for oscillations in speed and height. If set to 0 then TECS_PTCH_DAMP will be used instead.
This commit is contained in:
Tom Pittenger 2016-02-24 18:18:14 -08:00
parent fcb802cccc
commit 67e2db7975
2 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_TECS/AP_TECS.cpp
View File

@ -226,6 +226,13 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("TKOFF_IGAIN", 25, AP_TECS, _integGain_takeoff, 0), AP_GROUPINFO("TKOFF_IGAIN", 25, AP_TECS, _integGain_takeoff, 0),
// @Param: LAND_PDAMP
// @Description: This is the damping gain for the pitch demand loop. Increase to add damping to correct for oscillations in speed and height. If set to 0 then TECS_PTCH_DAMP will be used instead.
// @Range: 0.1 1.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("LAND_PDAMP", 26, AP_TECS, _land_pitch_damp, 0),
AP_GROUPEND AP_GROUPEND
}; };
@ -801,11 +808,15 @@ void AP_TECS::_update_pitch(void)
// During flare a different damping gain is used // 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_dem * timeConstant(); float temp = SEB_error + SEBdot_dem * timeConstant();
float pitch_damp = _ptchDamp;
if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) { if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
temp += SEBdot_error * _landDamp; pitch_damp = _landDamp;
} else { } else if (!is_zero(_land_pitch_damp) && is_on_land_approach(false)) {
temp += SEBdot_error * _ptchDamp; pitch_damp = _land_pitch_damp;
} }
temp += SEBdot_error * pitch_damp;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
temp += _PITCHminf * gainInv; temp += _PITCHminf * gainInv;
} }

1
libraries/AP_TECS/AP_TECS.h
View File

@ -148,6 +148,7 @@ private:
AP_Float _timeConst; AP_Float _timeConst;
AP_Float _landTimeConst; AP_Float _landTimeConst;
AP_Float _ptchDamp; AP_Float _ptchDamp;
AP_Float _land_pitch_damp;
AP_Float _landDamp; AP_Float _landDamp;
AP_Float _thrDamp; AP_Float _thrDamp;
AP_Float _land_throttle_damp; AP_Float _land_throttle_damp;