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

AP_TECS: add flight stage LAND_PREFLARE

This commit is contained in:
Tom Pittenger 2016-01-29 22:33:59 -08:00 committed by Andrew Tridgell
parent b8d5369ebd
commit 5338a76a58
1 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_TECS/AP_TECS.cpp
View File

@ -445,7 +445,7 @@ void AP_TECS::_update_height_demand(void)
// us to consistently be above the desired glide slope. This will
// be replaced with a better zero-lag filter in the future.
float new_hgt_dem = _hgt_dem_adj;
if (_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage == FLIGHT_LAND_FINAL) {
if (_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage == FLIGHT_LAND_FINAL || _flight_stage == FLIGHT_LAND_PREFLARE) {
new_hgt_dem += (_hgt_dem_adj - _hgt_dem_adj_last)*10.0f*(timeConstant()+1);
}
_hgt_dem_adj_last = _hgt_dem_adj;
@ -495,6 +495,7 @@ void AP_TECS::_update_energies(void)
float AP_TECS::timeConstant(void) const
{
if (_flight_stage==FLIGHT_LAND_FINAL ||
_flight_stage==FLIGHT_LAND_PREFLARE ||
_flight_stage==FLIGHT_LAND_APPROACH) {
if (_landTimeConst < 0.1f) {
return 0.1f;
@ -600,7 +601,7 @@ void AP_TECS::_update_throttle_option(int16_t throttle_nudge)
float nomThr;
//If landing and we don't have an airspeed sensor and we have a non-zero
//TECS_LAND_THR param then use it
if ((_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage== FLIGHT_LAND_FINAL) &&
if ((_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage == FLIGHT_LAND_FINAL || _flight_stage == FLIGHT_LAND_PREFLARE) &&
_landThrottle >= 0) {
nomThr = (_landThrottle + throttle_nudge) * 0.01f;
} else { //not landing or not using TECS_LAND_THR parameter
@ -668,7 +669,9 @@ void AP_TECS::_update_pitch(void)
SKE_weighting = 0.0f;
} else if ( _underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
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_PREFLARE ||
_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
if (_spdWeightLand < 0) {
// use sliding scale from normal weight down to zero at landing
float scaled_weight = _spdWeight * (1.0f - _path_proportion);
@ -849,7 +852,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
// and allow zero throttle
_THRminf = 0;
} else if (flight_stage == FLIGHT_LAND_APPROACH && (-_climb_rate) > _land_sink) {
} else if ((flight_stage == FLIGHT_LAND_APPROACH || flight_stage == FLIGHT_LAND_PREFLARE) && (-_climb_rate) > _land_sink) {
// constrain the pitch in landing as we get close to the flare
// point. Use a simple linear limit from 15 meters after the
// landing point