Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

AttPosEKF: Fix error_count comparison

Browse Source
This commit is contained in:
Johan Jansen
2015-02-10 16:51:47 +01:00
committed by Lorenz Meier
parent c7d0cb6bd7
commit d4c7e66212
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+3 -3
View File
@@ -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