From 6cb96d074d282af92540b6481d843c2295426773 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 17:01:32 +0200 Subject: EKF: Introduce proper check flags for sensor init states --- .../fw_att_pos_estimator_main.cpp | 40 ++++++++++++++++++---- 1 file changed, 34 insertions(+), 6 deletions(-) (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp') diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index b87338282..40729e94b 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -194,6 +194,9 @@ private: bool _baro_init; bool _gps_initialized; uint64_t _gps_start_time; + bool _gyro_valid; + bool _accel_valid; + bool _mag_valid; int _mavlink_fd; @@ -341,6 +344,9 @@ FixedwingEstimator::FixedwingEstimator() : _initialized(false), _baro_init(false), _gps_initialized(false), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), _mavlink_fd(-1), _ekf(nullptr) { @@ -617,10 +623,30 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); - /* Reset baro reference if switching to HIL */ + /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + /* system is in HIL now, we got plenty of time - make sure real sensors are off */ + usleep(250000); + +#ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); +#else + /* now read all sensor publications to ensure all real sensor data is purged */ + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); +#endif + + /* set sensors to de-initialized state */ + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + _baro_init = false; _gps_initialized = false; + + /* now skip this loop and get data on the next one */ + continue; } /** @@ -663,12 +689,14 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; + _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; + _accel_valid = true; } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; @@ -713,12 +741,14 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + _accel_valid = true; } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; @@ -837,6 +867,8 @@ FixedwingEstimator::task_main() if (mag_updated) { + _mag_valid = true; + perf_count(_perf_mag); #ifndef SENSOR_COMBINED_SUB @@ -961,11 +993,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - // Wait long enough to ensure all sensors updated once - // XXX we rather want to check all updated - - - if (hrt_elapsed_time(&_gps_start_time) > 50000) { + if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { // bool home_set; // orb_check(_home_sub, &home_set); -- cgit v1.2.3