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

TECS: use altitude supplied by mainline code 

this allows for use of ALT_OFFSET and ALT_MIX
This commit is contained in:
Paul Riseborough 2013-07-09 09:07:13 +10:00 committed by Andrew Tridgell
parent d8fedf994a
commit 9c431b4a04
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_TECS/AP_TECS.cpp
View File

@ -128,11 +128,11 @@ const AP_Param::GroupInfo AP_TECS::var_info[] PROGMEM = {
*
*/
void AP_TECS::update_50hz(void)
void AP_TECS::update_50hz(float hgt_afe)
{
// Implement third order complementary filter for height and height rate
// estimted height rate = _integ2_state
// estimated height = _integ3_state
// estimated height above field elevation = _integ3_state
// Reference Paper :
// Optimising the Gains of the Baro-Inertial Vertical Channel
// Widnall W.S, Sinha P.K,
@ -142,7 +142,7 @@ void AP_TECS::update_50hz(void)
uint32_t now = hal.scheduler->micros();
float DT = max((now - _update_50hz_last_usec),0)*1.0e-6f;
if (DT > 1.0) {
_integ3_state = _baro->get_altitude();
_integ3_state = hgt_afe;
_integ2_state = 0.0f;
_integ1_state = 0.0f;
DT = 0.02f; // when first starting TECS, use a
@ -155,7 +155,7 @@ void AP_TECS::update_50hz(void)
// Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega
float omega2 = _hgtCompFiltOmega*_hgtCompFiltOmega;
float hgt_err = _baro->get_altitude() - _integ3_state;
float hgt_err = hgt_afe - _integ3_state;
float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;
_integ1_state = _integ1_state + integ1_input * DT;
float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f;

3
libraries/AP_TECS/AP_TECS.h
View File

@ -41,7 +41,8 @@ public:
// Update of the estimated height and height rate internal state
// Update of the inertial speed rate internal state
// Should be called at 50Hz or greater
void update_50hz(void);
// hgt_afe is the height above field elevation (takeoff height)
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);