diff options
| author | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-10 16:51:47 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-10 17:47:23 +0100 |
| commit | d4c7e66212b5f36c1393ca95708495a750cb6e70 (patch) | |
| tree | 2e04e16efb4ac7ae4565beda809bed4e33ed6311 | |
| parent | c7d0cb6bd72deef810cfe1a16ac7b78810f8036b (diff) | |
| download | px4-firmware-d4c7e66212b5f36c1393ca95708495a750cb6e70.tar.gz px4-firmware-d4c7e66212b5f36c1393ca95708495a750cb6e70.tar.bz2 px4-firmware-d4c7e66212b5f36c1393ca95708495a750cb6e70.zip | |
AttPosEKF: Fix error_count comparison
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 |
1 files changed, 3 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 b10c583c6..62965976d 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 @@ -971,7 +971,7 @@ FixedwingEstimator::task_main() if (isfinite(_sensor_combined.gyro_rad_s[0]) && isfinite(_sensor_combined.gyro_rad_s[1]) && isfinite(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount < _sensor_combined.gyro1_errcount)) { + (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; @@ -1009,7 +1009,7 @@ FixedwingEstimator::task_main() int last_accel_main = _accel_main; /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount < _sensor_combined.accelerometer1_errcount) { + if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { _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]; @@ -1192,7 +1192,7 @@ FixedwingEstimator::task_main() // XXX we compensate the offsets upfront - should be close to zero. /* fail over to the 2nd mag if we know the first is down */ - if (_sensor_combined.magnetometer_errcount < _sensor_combined.magnetometer1_errcount) { + if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) { _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset |