From 52a29468270fae607b97dd3cd8e766309a795b9a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Sep 2015 10:00:03 +0200 Subject: [PATCH] EKF: Add sensor priority support --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3d6c8ddd34..c75a9e547d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1219,9 +1219,12 @@ void AttitudePositionEstimatorEKF::pollData() // Feed validator with recent sensor data for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { - _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], _sensor_combined.gyro_errcount[i]); - _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], _sensor_combined.accelerometer_errcount[i]); - _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], _sensor_combined.magnetometer_errcount[i]); + _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], + _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); + _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], + _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); + _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], + _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); } // Get best measurement values