From 447d159964e3af6c4ac2004d3a9e3217c0f269b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 08:33:58 +0100 Subject: Initialize the filter immediately, re-init once GPS becomes available (needs in-flight testing) --- .../fw_att_pos_estimator_main.cpp | 46 +++++++++++++++++++--- 1 file changed, 40 insertions(+), 6 deletions(-) (limited to 'src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp') 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; -- cgit v1.2.3