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

AP_TECS: improved support for VTOL transitions

Browse Source
This commit is contained in:
Andrew Tridgell 2017-10-21 17:47:35 +11:00
parent bf0e1ac554
commit 8a4e84e099
1 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_TECS/AP_TECS.cpp
View File

@ -589,6 +589,15 @@ void AP_TECS::_update_throttle_with_airspeed(void)
float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem; float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem;
float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem; float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem;
if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_VTOL) {
/*
when we are in a VTOL state then we ignore potential energy
errors as we have vertical motors that interfere with the
total energy calculation.
*/
SPE_err_max = SPE_err_min = 0;
}
// Calculate total energy error // Calculate total energy error
_STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est; _STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est;
float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
@ -769,6 +778,12 @@ 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 (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_VTOL) {
// if we are in VTOL mode then control pitch without regard to
// speed. Speed is also taken care of independently of
// height. This is needed as the usual relationship of speed
// and height is broken by the VTOL motors
SKE_weighting = 0.0f;
} else if ( _flags.underspeed || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { } else if ( _flags.underspeed || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
SKE_weighting = 2.0f; SKE_weighting = 2.0f;
} else if (_flags.is_doing_auto_land) { } else if (_flags.is_doing_auto_land) {