From 66d60953df22f4da4c7a2979c80b7d10d846d30d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 10 Jul 2013 13:32:13 +1000 Subject: [PATCH] TECS: added height update_pitch call --- libraries/AP_SpdHgtControl/AP_SpdHgtControl.h | 3 ++- libraries/AP_TECS/AP_TECS.cpp | 13 +++++++------ libraries/AP_TECS/AP_TECS.h | 5 +++-- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h b/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h index 6c273a3967..5a3bf52454 100644 --- a/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h +++ b/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h @@ -45,7 +45,8 @@ public: int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd, - int16_t throttle_nudge) = 0; + int16_t throttle_nudge, + float hgt_afe) = 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 242d4b04ca..a408dc2f2f 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -164,7 +164,7 @@ void AP_TECS::update_50hz(float hgt_afe) // If more than 1 second has elapsed since last update then reset the integrator state // to the measured height if (DT > 1.0) { - _integ3_state = _baro->get_altitude(); + _integ3_state = hgt_afe; } else { _integ3_state = _integ3_state + integ3_input*DT; } @@ -534,7 +534,7 @@ void AP_TECS::_update_pitch(void) _last_pitch_dem = _pitch_dem; } -void AP_TECS::_initialise_states(int32_t ptchMinCO_cd) +void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) { // Initialise states and variables if DT > 1 second or in climbout if (_DT > 1.0) @@ -543,7 +543,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd) _integ7_state = 0.0f; _last_throttle_dem = aparm.throttle_cruise * 0.01f; _last_pitch_dem = _ahrs->pitch; - _hgt_dem_adj_last = _baro->get_altitude(); + _hgt_dem_adj_last = hgt_afe; _hgt_dem_adj = _hgt_dem_adj_last; _hgt_dem_prev = _hgt_dem_adj_last; _hgt_dem_in_old = _hgt_dem_adj_last; @@ -558,7 +558,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd) { _PITCHminf = 0.000174533f * ptchMinCO_cd; _THRminf = _THRmaxf - 0.01f; - _hgt_dem_adj_last = _baro->get_altitude(); + _hgt_dem_adj_last = hgt_afe; _hgt_dem_adj = _hgt_dem_adj_last; _hgt_dem_prev = _hgt_dem_adj_last; _TAS_dem_last = _TAS_dem; @@ -580,7 +580,8 @@ 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) + int16_t throttle_nudge, + float hgt_afe) { // Calculate time in seconds since last update uint32_t now = hal.scheduler->micros(); @@ -600,7 +601,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _climbOutDem = climbOutDem; // initialise selected states and variables if DT > 1 second or in climbout - _initialise_states(ptchMinCO_cd); + _initialise_states(ptchMinCO_cd, hgt_afe); // Calculate Specific Total Energy Rate Limits _update_STE_rate_lim(); diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 384a4e97d9..17b3264e8d 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -49,7 +49,8 @@ public: int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd, - int16_t throttle_nudge); + int16_t throttle_nudge, + float hgt_afe); // demanded throttle in percentage // should return 0 to 100 @@ -256,7 +257,7 @@ private: void _update_pitch(void); // Initialise states and variables - void _initialise_states(int32_t ptchMinCO_cd); + void _initialise_states(int32_t ptchMinCO_cd, float hgt_afe); // Calculate specific total energy rate limits void _update_STE_rate_lim(void);