aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_TECS: add TECS_LAND_I_GAIN and TECS_TKOFF_I_GAIN

Browse Source 
// more integral gain options for land
This commit is contained in:
Tom Pittenger 2016-02-23 16:20:25 -08:00
parent a139789693
commit fcb802cccc
2 changed files with 39 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_TECS/AP_TECS.cpp
View File

@ -210,6 +210,22 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
// @User: Advanced
AP_GROUPINFO("LAND_TDAMP", 23, AP_TECS, _land_throttle_damp, 0),
// @Param: LAND_IGAIN
// @DisplayName: Controller integrator during landing
// @Description: This is the integrator gain on the control loop during landing. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values lower than TECS_INTEG_GAIN work best
// @Range: 0.0 0.5
// @Increment: 0.02
// @User: Advanced
AP_GROUPINFO("LAND_IGAIN", 24, AP_TECS, _integGain_land, 0),
// @Param: TKOFF_IGAIN
// @DisplayName: Controller integrator during takeoff
// @Description: This is the integrator gain on the control loop during takeoff. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values higher than TECS_INTEG_GAIN work best
// @Range: 0.0 0.5
// @Increment: 0.02
// @User: Advanced
AP_GROUPINFO("TKOFF_IGAIN", 25, AP_TECS, _integGain_takeoff, 0),
AP_GROUPEND
};
@ -623,7 +639,7 @@ void AP_TECS::_update_throttle(void)
// Calculate integrator state, constraining state
// Set integrator to a max throttle value during climbout
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
_integ6_state = _integ6_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)
{
_integ6_state = integ_max;
@ -646,6 +662,21 @@ void AP_TECS::_update_throttle(void)
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
}
float AP_TECS::_get_i_gain(void)
{
float i_gain = _integGain;
if (_flight_stage == FLIGHT_TAKEOFF) {
if (!is_zero(_integGain_takeoff)) {
i_gain = _integGain_takeoff;
}
} else if (_is_doing_auto_land) {
if (!is_zero(_integGain_land)) {
i_gain = _integGain_land;
}
}
return i_gain;
}
void AP_TECS::_update_throttle_option(int16_t throttle_nudge)
{
// Calculate throttle demand by interpolating between pitch and throttle limits
@ -741,7 +772,7 @@ void AP_TECS::_update_pitch(void)
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
// Calculate integrator state, constraining input if pitch limits are exceeded
float integ7_input = SEB_error * _integGain;
float integ7_input = SEB_error * _get_i_gain();
if (_pitch_dem > _PITCHmaxf)
{
integ7_input = MIN(integ7_input, _PITCHmaxf - _pitch_dem);
@ -885,6 +916,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_is_doing_auto_land = is_doing_auto_land;
_distance_beyond_land_wp = distance_beyond_land_wp;
_flight_stage = flight_stage;
// Convert inputs
_hgt_dem = hgt_dem_cm * 0.01f;
@ -955,7 +987,6 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
// convert to radians
_PITCHmaxf = radians(_PITCHmaxf);
_PITCHminf = radians(_PITCHminf);
_flight_stage = flight_stage;
// initialise selected states and variables if DT > 1 second or in climbout
_initialise_states(ptchMinCO_cd, hgt_afe);

5
libraries/AP_TECS/AP_TECS.h
View File

@ -152,6 +152,8 @@ private:
AP_Float _thrDamp;
AP_Float _land_throttle_damp;
AP_Float _integGain;
AP_Float _integGain_takeoff;
AP_Float _integGain_land;
AP_Float _vertAccLim;
AP_Float _rollComp;
AP_Float _spdWeight;
@ -318,6 +320,9 @@ private:
// Update Demanded Throttle Non-Airspeed
void _update_throttle_option(int16_t throttle_nudge);
// get integral gain which is flight_stage dependent
float _get_i_gain(void);
// Detect Bad Descent
void _detect_bad_descent(void);