diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-12-30 12:20:23 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-12-30 12:20:23 +0100 |
commit | c82996850be2df523394c2ac49db55258819a8a3 (patch) | |
tree | c92ca25713c00908a3ae152c72aab5c10aa29594 /src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | |
parent | 4942883ddcb5d1a09e96335b1edbbf2d937937b4 (diff) | |
download | px4-firmware-c82996850be2df523394c2ac49db55258819a8a3.tar.gz px4-firmware-c82996850be2df523394c2ac49db55258819a8a3.tar.bz2 px4-firmware-c82996850be2df523394c2ac49db55258819a8a3.zip |
Update EKF estimator to most recent version from Paul Riseborough
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 | 19 |
1 files changed, 12 insertions, 7 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 e7805daa9..9e32045ac 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 @@ -84,7 +84,7 @@ #include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> -#include "estimator_23states.h" +#include "estimator_22states.h" @@ -1371,10 +1371,15 @@ FixedwingEstimator::task_main() } if (newRangeData) { - _ekf->fuseRngData = true; - _ekf->useRangeFinder = true; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); - _ekf->GroundEKF(); + + if (_ekf->Tnb.z.z > 0.9f) { + // _ekf->rngMea is set in sensor readout already + _ekf->fuseRngData = true; + _ekf->fuseOptFlowData = false; + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->OpticalFlowEKF(); + _ekf->fuseRngData = false; + } } @@ -1514,8 +1519,8 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->windSpdFiltNorth; - _wind.windspeed_east = _ekf->windSpdFiltEast; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; // XXX we need to do something smart about the covariance here // but we default to the estimate covariance for now _wind.covariance_north = _ekf->P[14][14]; |