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

AP_TECS: added flight stage FLIGHT_LAND_ABORT

Browse Source 
- do what TAKEOFF does in tecs, push the throttle up
This commit is contained in:
Tom Pittenger 2015-08-29 13:20:07 -07:00 committed by Andrew Tridgell
parent 0961e1d907
commit 46a83c7ae9
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_TECS/AP_TECS.cpp
View File

@ -551,7 +551,7 @@ void AP_TECS::_update_throttle(void)
// Calculate integrator state, constraining state // Calculate integrator state, constraining state
// Set integrator to a max throttle value during climbout // 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 * _integGain) * _DT * K_STE2Thr;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF) if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)
{ {
_integ6_state = integ_max; _integ6_state = integ_max;
} }
@ -645,7 +645,7 @@ void AP_TECS::_update_pitch(void)
float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f); float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f);
if (!_ahrs.airspeed_sensor_enabled()) { if (!_ahrs.airspeed_sensor_enabled()) {
SKE_weighting = 0.0f; SKE_weighting = 0.0f;
} else if ( _underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF) { } else if ( _underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
SKE_weighting = 2.0f; SKE_weighting = 2.0f;
} else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH || _flight_stage == AP_TECS::FLIGHT_LAND_FINAL) { } else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH || _flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f); SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
@ -694,7 +694,7 @@ void AP_TECS::_update_pitch(void)
} else { } else {
temp += SEBdot_error * _ptchDamp; temp += SEBdot_error * _ptchDamp;
} }
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF) { if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
temp += _PITCHminf * gainInv; 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);
@ -744,7 +744,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_DT = 0.1f; // when first starting TECS, use a _DT = 0.1f; // when first starting TECS, use a
// small time constant // small time constant
} }
else if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF) else if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)
{ {
_PITCHminf = 0.000174533f * ptchMinCO_cd; _PITCHminf = 0.000174533f * ptchMinCO_cd;
_hgt_dem_adj_last = hgt_afe; _hgt_dem_adj_last = hgt_afe;
@ -784,7 +784,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
// Convert inputs // Convert inputs
_hgt_dem = hgt_dem_cm * 0.01f; _hgt_dem = hgt_dem_cm * 0.01f;
_EAS_dem = EAS_dem_cm * 0.01f; _EAS_dem = EAS_dem_cm * 0.01f;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF && aparm.takeoff_throttle_max != 0) { if (aparm.takeoff_throttle_max != 0 &&
(_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)) {
_THRmaxf = aparm.takeoff_throttle_max * 0.01f; _THRmaxf = aparm.takeoff_throttle_max * 0.01f;
} else { } else {
_THRmaxf = aparm.throttle_max * 0.01f; _THRmaxf = aparm.throttle_max * 0.01f;