From 4f9927bedab349a62302012c06b6bbe68623e772 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 9 Jan 2016 21:50:40 +1100 Subject: [PATCH] AP_TECS: added set_pitch_max_limit() API --- libraries/AP_TECS/AP_TECS.cpp | 5 +++++ libraries/AP_TECS/AP_TECS.h | 8 ++++++++ 2 files changed, 13 insertions(+) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 19aff0da84..7254f8e3cb 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -828,6 +828,11 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } else { _PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f); } + + // apply temporary pitch limit and clear + _PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit); + _pitch_max_limit = 90; + if (_pitch_min >= 0) { _PITCHminf = aparm.pitch_limit_min_cd * 0.01f; } else { diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 489b16b8ae..433dbbf9b1 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -97,6 +97,11 @@ public: _path_proportion = constrain_float(path_proportion, 0.0f, 1.0f); } + // set pitch max limit in degrees + void set_pitch_max_limit(int8_t pitch_limit) { + _pitch_max_limit = pitch_limit; + } + // this supports the TECS_* user settable parameters static const struct AP_Param::GroupInfo var_info[]; @@ -155,6 +160,9 @@ private: AP_Int8 _pitch_min; AP_Int8 _land_pitch_max; + // temporary _pitch_max_limit. Cleared on each loop. Clear when >= 90 + int8_t _pitch_max_limit = 90; + // current height estimate (above field elevation) float _height;