From 7c4d31c481572eee53cd0e0742260a670cc45f48 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Aug 2014 11:46:01 +1000 Subject: [PATCH] AP_TECS: added TECS_PITCH_MAX and TECS_PITCH_MIN this allows the TECS controlled pitch limits to be smaller than the FBWA limits. It is common for a human pilot to want a bit more discretion over pitch than you want the automatic controller to use --- libraries/AP_TECS/AP_TECS.cpp | 36 +++++++++++++++++++++++++++++++++-- libraries/AP_TECS/AP_TECS.h | 2 ++ 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index a7cee03c40..a0d32e6fc4 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -133,6 +133,22 @@ const AP_Param::GroupInfo AP_TECS::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("LAND_SPDWGT", 14, AP_TECS, _spdWeightLand, 1.0f), + // @Param: PITCH_MAX + // @DisplayName: Maximum pitch in auto flight + // @Description: This controls maximum pitch up in automatic throttle modes. If this is set to zero then LIM_PITCH_MAX is used instead. The purpose of this parameter is to allow the use of a smaller pitch range when in automatic flight than what is used in FBWA mode. + // @Range: 0 45 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("PITCH_MAX", 15, AP_TECS, _pitch_max, 0), + + // @Param: PITCH_MIN + // @DisplayName: Minimum pitch in auto flight + // @Description: This controls minimum pitch in automatic throttle modes. If this is set to zero then LIM_PITCH_MIN is used instead. The purpose of this parameter is to allow the use of a smaller pitch range when in automatic flight than what is used in FBWA mode. Note that TECS_PITCH_MIN should be a negative number. + // @Range: -45 0 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("PITCH_MIN", 16, AP_TECS, _pitch_min, 0), + AP_GROUPEND }; @@ -656,8 +672,24 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _EAS_dem = EAS_dem_cm * 0.01f; _THRmaxf = aparm.throttle_max * 0.01f; _THRminf = aparm.throttle_min * 0.01f; - _PITCHmaxf = 0.000174533f * aparm.pitch_limit_max_cd; - _PITCHminf = 0.000174533f * aparm.pitch_limit_min_cd; + + // work out the maximum and minimum pitch + // if TECS_PITCH_{MAX,MIN} isn't set then use + // LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be + // larger than LIM_PITCH_{MAX,MIN} + if (_pitch_max <= 0) { + _PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f; + } else { + _PITCHmaxf = min(_pitch_max, aparm.pitch_limit_max_cd * 0.01f); + } + if (_pitch_min >= 0) { + _PITCHminf = aparm.pitch_limit_min_cd * 0.01f; + } else { + _PITCHminf = max(_pitch_min, aparm.pitch_limit_min_cd * 0.01f); + } + // convert to radians + _PITCHmaxf = radians(_PITCHmaxf); + _PITCHminf = radians(_PITCHminf); _flight_stage = flight_stage; // initialise selected states and variables if DT > 1 second or in climbout diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 85caa26056..4d73291f0f 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -120,6 +120,8 @@ private: AP_Float _spdWeightLand; AP_Float _landThrottle; AP_Float _landAirspeed; + AP_Int8 _pitch_max; + AP_Int8 _pitch_min; // throttle demand in the range from 0.0 to 1.0 float _throttle_dem;