diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-18 08:33:58 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-18 08:33:58 +0100 |
commit | 447d159964e3af6c4ac2004d3a9e3217c0f269b2 (patch) | |
tree | c1d20fb7f3debde3cfbc26ba726711138abf19b6 | |
parent | 7e9fcac057616ba535e09d49f7d0f47f5ce9977b (diff) | |
download | px4-firmware-447d159964e3af6c4ac2004d3a9e3217c0f269b2.tar.gz px4-firmware-447d159964e3af6c4ac2004d3a9e3217c0f269b2.tar.bz2 px4-firmware-447d159964e3af6c4ac2004d3a9e3217c0f269b2.zip |
Initialize the filter immediately, re-init once GPS becomes available (needs in-flight testing)
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 46 |
1 files changed, 40 insertions, 6 deletions
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 4f5e3c9b4..2b7cdc118 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -681,11 +681,26 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) { - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - InitialiseFilter(velNED); + // Wait long enough to ensure all sensors updated once + // XXX we rather want to check all updated + + + if (hrt_elapsed_time(&start_time) > 100000) { + + if (!_gps_initialized && (GPSstatus == 3)) { + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + InitialiseFilter(velNED); + + _gps_initialized = true; + + } else if (!statesInitialised) { + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + InitialiseFilter(velNED); + } } // If valid IMU data and states initialised, predict states and covariances @@ -730,7 +745,7 @@ FixedwingEstimator::task_main() } // Fuse GPS Measurements - if (newDataGps && statesInitialised) { + if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED velNED[0] = _gps.vel_n_m_s; velNED[1] = _gps.vel_e_m_s; @@ -748,6 +763,25 @@ FixedwingEstimator::task_main() // run the fusion step FuseVelposNED(); + } else if (statesInitialised) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + posNED[0] = 0.0f; + posNED[1] = 0.0f; + posNED[2] = 0.0f; + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + // set fusion flags + fuseVelData = true; + fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); + RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); + // run the fusion step + FuseVelposNED(); } else { fuseVelData = false; fusePosData = false; |