From 2c5db9a1659eaf619a276fa273af28a16edaa004 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 9 Jul 2013 20:50:37 +1000 Subject: [PATCH] TECS: added support for throttle nudging --- libraries/AP_SpdHgtControl/AP_SpdHgtControl.h | 6 +++++- libraries/AP_TECS/AP_TECS.cpp | 12 ++++++++---- libraries/AP_TECS/AP_TECS.h | 8 ++++++-- 3 files changed, 19 insertions(+), 7 deletions(-) diff --git a/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h b/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h index 523be0114b..6c273a3967 100644 --- a/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h +++ b/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h @@ -41,7 +41,11 @@ public: // Update of the pitch and throttle demands // Should be called at 10Hz or faster - virtual void update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd) = 0; + virtual void update_pitch_throttle( int32_t hgt_dem_cm, + int32_t EAS_dem_cm, + bool climbOutDem, + int32_t ptchMinCO_cd, + int16_t throttle_nudge) = 0; // demanded throttle in percentage // should return 0 to 100 diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 7b68af4ecc..242d4b04ca 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -424,10 +424,10 @@ void AP_TECS::_update_throttle(void) _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf); } -void AP_TECS::_update_throttle_option(void) +void AP_TECS::_update_throttle_option(int16_t throttle_nudge) { // Calculate throttle demand by interpolating between pitch and throttle limits - float nomThr = aparm.throttle_cruise * 0.01f; + float nomThr = (aparm.throttle_cruise + throttle_nudge)* 0.01f; if (_climbOutDem) { _throttle_dem = _THRmaxf; @@ -576,7 +576,11 @@ void AP_TECS::_update_STE_rate_lim(void) _STEdot_min = - _minSinkRate * GRAVITY_MSS; } -void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd) +void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, + int32_t EAS_dem_cm, + bool climbOutDem, + int32_t ptchMinCO_cd, + int16_t throttle_nudge) { // Calculate time in seconds since last update uint32_t now = hal.scheduler->micros(); @@ -617,7 +621,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool if (_ahrs->airspeed_sensor_enabled()) { _update_throttle(); } else { - _update_throttle_option(); + _update_throttle_option(throttle_nudge); } // Detect bad descent due to demanded airspeed being too high diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 63872a4b3b..384a4e97d9 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -45,7 +45,11 @@ public: void update_50hz(float hgt_afe); // Update the control loop calculations - void update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd); + void update_pitch_throttle(int32_t hgt_dem_cm, + int32_t EAS_dem_cm, + bool climbOutDem, + int32_t ptchMinCO_cd, + int16_t throttle_nudge); // demanded throttle in percentage // should return 0 to 100 @@ -243,7 +247,7 @@ private: void _update_throttle(void); // Update Demanded Throttle Non-Airspeed - void _update_throttle_option(void); + void _update_throttle_option(int16_t throttle_nudge); // Detect Bad Descent void _detect_bad_descent(void);