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

AP_TECS: rely on single flag for all land stage differences 

recent fixes in Plane have made the stage more accurate so exceptions/hacks are no longer needed to differentiate between knowing if executing NAV_LAND vs being in stage_approach.
This commit is contained in:
Tom Pittenger 2016-04-19 17:09:17 -07:00
parent 6a83ad419a
commit 595badce3e
2 changed files with 6 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_TECS/AP_TECS.cpp
View File

@ -447,7 +447,7 @@ void AP_TECS::_update_height_demand(void)
_hgt_dem_in_old = _hgt_dem;
float max_sink_rate = _maxSinkRate;
if (_maxSinkRate_approach > 0 && is_on_land_approach(true)) {
if (_maxSinkRate_approach > 0 && _flags.is_doing_auto_land) {
// special sink rate for approach to accommodate steep slopes and reverse thrust.
// A special check must be done to see if we're LANDing on approach but also if
// we're in that tiny window just starting NAV_LAND but still in NORMAL mode. If
@ -503,7 +503,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 || _flight_stage == FLIGHT_LAND_PREFLARE) {
if (_flags.is_doing_auto_land) {
new_hgt_dem += (_hgt_dem_adj - _hgt_dem_adj_last)*10.0f*(timeConstant()+1);
}
_hgt_dem_adj_last = _hgt_dem_adj;
@ -565,9 +565,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 (_flags.is_doing_auto_land) {
if (_landTimeConst < 0.1f) {
return 0.1f;
}
@ -693,8 +691,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 || _flight_stage == FLIGHT_LAND_PREFLARE) &&
_landThrottle >= 0) {
if (_flags.is_doing_auto_land && _landThrottle >= 0) {
nomThr = (_landThrottle + throttle_nudge) * 0.01f;
} else { //not landing or not using TECS_LAND_THR parameter
nomThr = (aparm.throttle_cruise + throttle_nudge)* 0.01f;
@ -761,9 +758,7 @@ void AP_TECS::_update_pitch(void)
SKE_weighting = 0.0f;
} else if ( _flags.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_PREFLARE ||
_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
} else if (_flags.is_doing_auto_land) {
if (_spdWeightLand < 0) {
// use sliding scale from normal weight down to zero at landing
float scaled_weight = _spdWeight * (1.0f - _path_proportion);
@ -817,7 +812,7 @@ void AP_TECS::_update_pitch(void)
float pitch_damp = _ptchDamp;
if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
pitch_damp = _landDamp;
} else if (!is_zero(_land_pitch_damp) && is_on_land_approach(false)) {
} else if (!is_zero(_land_pitch_damp) && _flags.is_doing_auto_land) {
pitch_damp = _land_pitch_damp;
}
temp += SEBdot_error * pitch_damp;
@ -893,27 +888,6 @@ void AP_TECS::_update_STE_rate_lim(void)
_STEdot_min = - _minSinkRate * GRAVITY_MSS;
}
// return true if on landing approach or pre-flare approach flight stages. Optional
// argument allows to be true while in normal stage just before switching to approach
bool AP_TECS::is_on_land_approach(bool include_segment_between_NORMAL_and_APPROACH)
{
if (!_flags.is_doing_auto_land) {
return false;
}
bool on_land_approach = false;
on_land_approach |= (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH);
on_land_approach |= (_flight_stage == AP_TECS::FLIGHT_LAND_PREFLARE);
if (include_segment_between_NORMAL_and_APPROACH) {
// include the brief time where we are performing NAV_LAND but still
// on NORMAL stage until we line up with the approach slope
on_land_approach |= (_flight_stage == AP_TECS::FLIGHT_NORMAL);
}
return on_land_approach;
}
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm,

3
libraries/AP_TECS/AP_TECS.h
View File

@ -355,9 +355,6 @@ private:
// current time constant
float timeConstant(void) const;
// return true if on landing approach
bool is_on_land_approach(bool include_segment_between_NORMAL_and_APPROACH);
};
#define TECS_LOG_FORMAT(msg) { msg, sizeof(AP_TECS::log_TECS_Tuning), \