Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: bring the flare height rate demand in slowly 

bring it in over 0.5s to give less of a bounce
This commit is contained in:
Andrew Tridgell 2014-08-27 20:13:01 +10:00
parent 500e20b08d
commit 7547cd45f5
2 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_TECS/AP_TECS.cpp
View File

@ -187,7 +187,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] PROGMEM = {
void AP_TECS::update_50hz(float hgt_afe) void AP_TECS::update_50hz(float hgt_afe)
{ {
// Implement third order complementary filter for height and height rate // Implement third order complementary filter for height and height rate
// estimted height rate = _integ2_state // estimted height rate = _climb_rate
// estimated height above field elevation = _integ3_state // estimated height above field elevation = _integ3_state
// Reference Paper : // Reference Paper :
// Optimising the Gains of the Baro-Inertial Vertical Channel // Optimising the Gains of the Baro-Inertial Vertical Channel
@ -199,7 +199,7 @@ void AP_TECS::update_50hz(float hgt_afe)
float DT = max((now - _update_50hz_last_usec),0)*1.0e-6f; float DT = max((now - _update_50hz_last_usec),0)*1.0e-6f;
if (DT > 1.0) { if (DT > 1.0) {
_integ3_state = hgt_afe; _integ3_state = hgt_afe;
_integ2_state = 0.0f; _climb_rate = 0.0f;
_integ1_state = 0.0f; _integ1_state = 0.0f;
DT = 0.02f; // when first starting TECS, use a DT = 0.02f; // when first starting TECS, use a
// small time constant // small time constant
@ -209,7 +209,7 @@ void AP_TECS::update_50hz(float hgt_afe)
// USe inertial nav verical velocity and height if available // USe inertial nav verical velocity and height if available
Vector3f posned, velned; Vector3f posned, velned;
if (_ahrs.get_velocity_NED(velned) && _ahrs.get_relative_position_NED(posned)) { if (_ahrs.get_velocity_NED(velned) && _ahrs.get_relative_position_NED(posned)) {
_integ2_state = - velned.z; _climb_rate = - velned.z;
_integ3_state = - posned.z; _integ3_state = - posned.z;
} else { } else {
// Get height acceleration // Get height acceleration
@ -221,8 +221,8 @@ void AP_TECS::update_50hz(float hgt_afe)
float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega; float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;
_integ1_state = _integ1_state + integ1_input * DT; _integ1_state = _integ1_state + integ1_input * DT;
float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f; float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f;
_integ2_state = _integ2_state + integ2_input * DT; _climb_rate = _climb_rate + integ2_input * DT;
float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f; float integ3_input = _climb_rate + hgt_err * _hgtCompFiltOmega * 3.0f;
// If more than 1 second has elapsed since last update then reset the integrator state // If more than 1 second has elapsed since last update then reset the integrator state
// to the measured height // to the measured height
if (DT > 1.0) { if (DT > 1.0) {
@ -367,14 +367,24 @@ void AP_TECS::_update_height_demand(void)
// Apply first order lag to height demand // Apply first order lag to height demand
_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
_hgt_dem_adj_last = _hgt_dem_adj; _hgt_dem_adj_last = _hgt_dem_adj;
// in final landing stage force height rate demand to the // in final landing stage force height rate demand to the
// configured sink rate // configured sink rate
if (_flight_stage == FLIGHT_LAND_FINAL) { if (_flight_stage == FLIGHT_LAND_FINAL) {
if (_flare_counter == 0) {
_hgt_rate_dem = _climb_rate;
}
if (_flare_counter < 5) {
_hgt_rate_dem = _hgt_rate_dem * 0.8f - 0.2f * _land_sink;
_flare_counter++;
} else {
_hgt_rate_dem = - _land_sink; _hgt_rate_dem = - _land_sink;
} }
} else {
_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
_flare_counter = 0;
}
} }
void AP_TECS::_detect_underspeed(void) void AP_TECS::_detect_underspeed(void)
@ -407,7 +417,7 @@ void AP_TECS::_update_energies(void)
_SKE_est = 0.5f * _integ5_state * _integ5_state; _SKE_est = 0.5f * _integ5_state * _integ5_state;
// Calculate specific energy rate // Calculate specific energy rate
_SPEdot = _integ2_state * GRAVITY_MSS; _SPEdot = _climb_rate * GRAVITY_MSS;
_SKEdot = _integ5_state * _vel_dot; _SKEdot = _integ5_state * _vel_dot;
} }
@ -629,6 +639,8 @@ void AP_TECS::_update_pitch(void)
// Constrain pitch demand // Constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
// Rate limit the pitch demand to comply with specified vertical // Rate limit the pitch demand to comply with specified vertical
// acceleration limit // acceleration limit
float ptchRateIncr = _DT * _vertAccLim / _integ5_state; float ptchRateIncr = _DT * _vertAccLim / _integ5_state;
@ -766,7 +778,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
log_tuning.hgt_dem = _hgt_dem_adj; log_tuning.hgt_dem = _hgt_dem_adj;
log_tuning.hgt = _integ3_state; log_tuning.hgt = _integ3_state;
log_tuning.dhgt_dem = _hgt_rate_dem; log_tuning.dhgt_dem = _hgt_rate_dem;
log_tuning.dhgt = _integ2_state; log_tuning.dhgt = _climb_rate;
log_tuning.spd_dem = _TAS_dem_adj; log_tuning.spd_dem = _TAS_dem_adj;
log_tuning.spd = _integ5_state; log_tuning.spd = _integ5_state;
log_tuning.dspd = _vel_dot; log_tuning.dspd = _vel_dot;

5
libraries/AP_TECS/AP_TECS.h
View File

@ -138,7 +138,7 @@ private:
float _integ1_state; float _integ1_state;
// Integrator state 2 - height rate // Integrator state 2 - height rate
float _integ2_state; float _climb_rate;
// Integrator state 3 - height // Integrator state 3 - height
float _integ3_state; float _integ3_state;
@ -240,6 +240,9 @@ private:
// Time since last update of main TECS loop (seconds) // Time since last update of main TECS loop (seconds)
float _DT; float _DT;
// counter for demanded sink rate on land final
uint8_t _flare_counter;
// Update the airspeed internal state using a second order complementary filter // Update the airspeed internal state using a second order complementary filter
void _update_speed(void); void _update_speed(void);