Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

TECS-WIP : Fixed bug in initialisation of DT value

Browse Source
This commit is contained in:
Paul Riseborough 2013-06-28 21:25:20 +10:00 committed by Andrew Tridgell
parent 904e6b5b8f
commit 8082122053
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_TECS/AP_TECS.cpp
View File

@ -178,6 +178,8 @@ void AP_TECS::update_50hz(void)
_integ3_state = _baro->get_altitude();
_integ2_state = 0.0f;
_integ1_state = 0.0f;
DT = 0.02f; // when first starting TECS, use a
// small time constant
}
_update_50hz_last_usec = now;
@ -194,7 +196,7 @@ void AP_TECS::update_50hz(void)
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) {
if (DT > 1.0) {
_integ3_state = _baro->get_altitude();
} else {
_integ3_state = _integ3_state + integ3_input*DT;
@ -234,6 +236,8 @@ void AP_TECS::_update_speed(void)
if (DT > 1.0) {
_integ5_state = (_EAS * EAS2TAS);
_integ4_state = 0.0f;
DT = 0.1f; // when first starting TECS, use a
// small time constant
}
// Get airspeed or default to halfway between min and max if