diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-07 16:06:18 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-07 16:06:18 +0200 |
commit | f4075b5623bb9c92dd46e92b97bc363a91498ff6 (patch) | |
tree | c39c5ac9ad70c6b313f6270cf94474dc10f1cbae /src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | |
parent | b9a3fa60bc06b31d4862919398c1161e6a35f360 (diff) | |
download | px4-firmware-f4075b5623bb9c92dd46e92b97bc363a91498ff6.tar.gz px4-firmware-f4075b5623bb9c92dd46e92b97bc363a91498ff6.tar.bz2 px4-firmware-f4075b5623bb9c92dd46e92b97bc363a91498ff6.zip |
Switching back to 23 states, fixed mag update logic
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 15 |
1 files changed, 11 insertions, 4 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 86f7eb99f..9cff9fab0 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 @@ -83,7 +83,7 @@ #include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> -#include "estimator_21states.h" +#include "estimator_23states.h" @@ -451,6 +451,8 @@ FixedwingEstimator::~FixedwingEstimator() } while (_estimator_task != -1); } + delete _ekf; + estimator::g_estimator = nullptr; } @@ -564,7 +566,7 @@ FixedwingEstimator::task_main() #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 4); + orb_set_interval(_sensor_combined_sub, 9); #endif /* sets also parameters in the EKF object */ @@ -809,6 +811,8 @@ FixedwingEstimator::task_main() #endif + //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -1247,12 +1251,15 @@ FixedwingEstimator::task_main() _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + _ekf->magstate.obsIndex = 0; + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + } else { _ekf->fuseMagData = false; } - if (_ekf->statesInitialised) _ekf->FuseMagnetometer(); - // Fuse Airspeed Measurements if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { _ekf->fuseVtasData = true; |