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

AP_TECS : Use NavEKF height and height rate data if available

This commit is contained in:
Paul Riseborough 2014-01-31 09:30:53 +11:00 committed by Andrew Tridgell
parent 0599817aa9
commit 9bbddb2f66
1 changed files with 24 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_TECS/AP_TECS.cpp
View File

@ -150,24 +150,30 @@ void AP_TECS::update_50hz(float hgt_afe)
}
_update_50hz_last_usec = now;
// Get height acceleration
float hgt_ddot_mea = -(_ahrs.get_accel_ef().z + GRAVITY_MSS);
// Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega
float omega2 = _hgtCompFiltOmega*_hgtCompFiltOmega;
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;
_integ2_state = _integ2_state + integ2_input * DT;
float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f;
// 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 = hgt_afe;
} else {
_integ3_state = _integ3_state + integ3_input*DT;
}
// USe inertial nav verical velocity and height if available
if (_ahrs.have_inertial_nav()) {
_integ2_state = -_ahrs.get_velocity_NED().z;
_integ3_state = -_ahrs.get_relative_position_NED().z;
} else {
// Get height acceleration
float hgt_ddot_mea = -(_ahrs.get_accel_ef().z + GRAVITY_MSS);
// Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega
float omega2 = _hgtCompFiltOmega*_hgtCompFiltOmega;
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;
_integ2_state = _integ2_state + integ2_input * DT;
float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f;
// 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 = hgt_afe;
} else {
_integ3_state = _integ3_state + integ3_input*DT;
}
}
// Update and average speed rate of change
// Get DCM