dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Clean implementation of filter startup delay implementation

This commit is contained in:
Lorenz Meier 2014-05-11 20:16:04 +02:00
parent eeba000b87
commit 077de5eb0b
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
View File

@ -97,6 +97,7 @@ __EXPORT uint32_t millis();
static uint64_t last_run = 0;
static uint64_t IMUmsec = 0;
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
uint32_t millis()
{
@ -194,6 +195,7 @@ private:
bool _baro_init;
bool _gps_initialized;
uint64_t _gps_start_time;
uint64_t _filter_start_time;
bool _gyro_valid;
bool _accel_valid;
bool _mag_valid;
@ -511,6 +513,7 @@ FixedwingEstimator::task_main()
_ekf = new AttPosEKF();
float dt = 0.0f; // time lapsed since last covariance prediction
_filter_start_time = hrt_absolute_time();
if (!_ekf) {
errx(1, "failed allocating EKF filter - out of RAM!");
@ -636,6 +639,7 @@ FixedwingEstimator::task_main()
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
_filter_start_time = last_sensor_timestamp;
/* now skip this loop and get data on the next one, which will also re-init the filter */
continue;
@ -1020,7 +1024,7 @@ FixedwingEstimator::task_main()
* PART TWO: EXECUTE THE FILTER
**/
if (hrt_absolute_time() > 2 * 1000 * 1000 && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
float initVelNED[3];