diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-03-08 12:15:39 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-03-08 12:15:39 +0100 |
commit | 34f6cf9eb6e5374b9fbeed914509f44d2a0a914e (patch) | |
tree | 6a718ae6fbaf6d1924bd81ed97d36c9877019976 /src | |
parent | 11568c77d49285b7ea376e2696cdce1fdb57d13c (diff) | |
download | px4-firmware-34f6cf9eb6e5374b9fbeed914509f44d2a0a914e.tar.gz px4-firmware-34f6cf9eb6e5374b9fbeed914509f44d2a0a914e.tar.bz2 px4-firmware-34f6cf9eb6e5374b9fbeed914509f44d2a0a914e.zip |
AttPosEKF: Inhibit mag state if not fixed wing and not accelerating forwards
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 19 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.h | 1 |
2 files changed, 13 insertions, 7 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index a22253dd2..0e931e666 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -186,7 +186,8 @@ AttPosEKF::AttPosEKF() : moCompR_LOS(0.0f), _isFixedWing(false), - _onGround(true) + _onGround(true), + _accNavMagHorizontal(0.0f) { memset(&last_ekf_error, 0, sizeof(last_ekf_error)); @@ -350,6 +351,11 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // variance estimation) accNavMag = delVelNav.length()/dtIMU; + //First order low-pass filtered magnitude of horizontal nav acceleration + Vector3f derivativeNav = (delVelNav / dtIMU); + float derivativeVelNavMagnitude = sqrtf(sq(derivativeNav.x) + sq(derivativeNav.y)); + _accNavMagHorizontal = _accNavMagHorizontal * 0.95f + derivativeVelNavMagnitude * 0.05f; + // If calculating position save previous velocity float lastVelocity[3]; lastVelocity[0] = states[4]; @@ -2539,15 +2545,14 @@ void AttPosEKF::setOnGround(const bool isLanded) if (_onGround || !useAirspeed) { inhibitWindStates = true; } else { - inhibitWindStates =false; + inhibitWindStates = false; } + //Check if we are accelerating forward, only then is the mag offset is observable + bool isMovingForward = _accNavMagHorizontal > 0.5f; + // don't update magnetic field states if on ground or not using compass - if (_onGround || !useCompass) { - inhibitMagStates = true; - } else { - inhibitMagStates = false; - } + inhibitMagStates = useCompass && !_onGround && (_isFixedWing || isMovingForward); // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS if ((_onGround || !useGPS) && !useRangeFinder) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 8dd1d74e9..c5517e38b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -413,6 +413,7 @@ protected: private: bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + float _accNavMagHorizontal; ///< First-order low-pass filtered rate of change maneuver velocity }; uint32_t millis(); |