Add filter control logic

CMakeLists.txt

@@ -54,7 +54,8 @@ px4_add_module(
 		EKF/covariance.cpp
 		EKF/vel_pos_fusion.cpp
 		EKF/mag_fusion.cpp
-                EKF/gps_checks.cpp
+               EKF/gps_checks.cpp
+		EKF/control.cpp
 	DEPENDS
 		platforms__common
 	)

EKF/control.cpp

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file control.cpp
+ * Control functions for ekf attitude and position estimator.
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ *
+ */
+
+#include "ekf.h"
+
+void Ekf::controlFusionModes()
+{
+    // Determine the vehicle status
+    calculateVehicleStatus();
+
+    // optical flow fusion mode selection logic
+    _control_status.flags.opt_flow = false;
+
+    // GPS fusion mode selection logic
+    // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
+    if (!_control_status.flags.gps) {
+        if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _gps_initialised) {
+            _control_status.flags.gps = true;
+            resetPosition();
+            resetVelocity();
+        }
+    }
+
+    // decide when to start using optical flow data
+    if (!_control_status.flags.opt_flow) {
+        // TODO optical flow start logic
+    }
+
+    // handle the case when we are relying on GPS fusion and lose it
+    if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
+        // We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
+        if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
+            if (_time_last_imu - _time_last_gps > 5e5) {
+                // if we don't have gps then we need to switch to the non-aiding mode, zero the veloity states
+                // and set the synthetic GPS position to the current estimate
+                _control_status.flags.gps = false;
+                _last_known_posNE(0) = _state.pos(0);
+                _last_known_posNE(1) = _state.pos(1);
+                _state.vel.setZero();
+            } else {
+                // Reset states to the last GPS measurement
+                resetPosition();
+                resetVelocity();
+            }
+        }
+    }
+
+    // handle the case when we are relying on optical flow fusion and lose it
+    if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
+        // TODO
+    }
+
+    // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
+    // or the more accurate 3-axis fusion
+    if (!_control_status.flags.armed) {
+        // always use simple mag fusion for initial startup
+        _control_status.flags.mag_hdg = true;
+        _control_status.flags.mag_3D = false;
+    } else {
+        if (_control_status.flags.in_air) {
+            // always use 3-axis mag fusion when airborne
+            _control_status.flags.mag_hdg = false;
+            _control_status.flags.mag_3D = true;
+        } else {
+            // always use simple heading fusion when on the ground
+            _control_status.flags.mag_hdg = true;
+            _control_status.flags.mag_3D = false;
+        }
+    }
+}
+
+void Ekf::calculateVehicleStatus()
+{
+    // determine if the vehicle is armed
+    _control_status.flags.armed = _vehicle_armed;
+
+    // record vertical position whilst disarmed to use as a height change reference
+    if (!_control_status.flags.armed) {
+        _last_disarmed_posD = _state.pos(2);
+    }
+
+    // Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming
+    if (!_control_status.flags.in_air && _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f) {
+        _control_status.flags.in_air = true;
+    }
+
+    // Transition to on-ground occurs when disarmed.
+    if (_control_status.flags.in_air && !_control_status.flags.armed) {
+        _control_status.flags.in_air = false;
+    }
+}

EKF/ekf.cpp

@@ -43,25 +43,31 @@
 #include <drivers/drv_hrt.h>
 
 Ekf::Ekf():
-	_filter_initialised(false),
+    _control_status{},
+    _filter_initialised(false),
 	_earth_rate_initialised(false),
 	_fuse_height(false),
 	_fuse_pos(false),
 	_fuse_hor_vel(false),
 	_fuse_vert_vel(false),
-	_time_last_fake_gps(0),
-	_vel_pos_innov{},
+    _time_last_fake_gps(0),
+    _time_last_pos_fuse(0),
+    _time_last_vel_fuse(0),
+    _time_last_hgt_fuse(0),
+    _time_last_of_fuse(0),
+    _vel_pos_innov{},
 	_mag_innov{},
 	_heading_innov{},
 	_vel_pos_innov_var{},
 	_mag_innov_var{},
-	_heading_innov_var{}
+    _heading_innov_var{}
 {
 	_earth_rate_NED.setZero();
 	_R_prev = matrix::Dcm<float>();
 	_delta_angle_corr.setZero();
 	_delta_vel_corr.setZero();
 	_vel_corr.setZero();
+    _last_known_posNE.setZero();
 }
 
 
@@ -91,28 +97,41 @@ bool Ekf::update()
 		predictCovariance();
 	}
 
-	// measurement updates
+    // control logic
+    controlFusionModes();
 
+    // measurement updates
+
+    // Fuse magnetometer data using the selected fuson method and only if angular alignment is complete
 	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
-		//fuseHeading();
-                fuseMag();
+        if (_control_status.flags.mag_3D && _control_status.flags.angle_align) {
+            fuseMag();
+            if (_control_status.flags.mag_dec) {
+                // TODO need to fuse synthetic declination measurements if there is no GPS or equivalent aiding
+                // otherwise heading will slowly drift
+            }
+        } else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) {
+            fuseHeading();
+        }
 	}
 
 	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
 		_fuse_height = true;
 	}
 
-	if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
+    // If we are using GPs aiding and data has fallen behind the fusion time horizon then fuse it
+    // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
+    // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
+    if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) {
 		_fuse_pos = true;
 		_fuse_vert_vel = true;
 		_fuse_hor_vel = true;
-
-	} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
-		// if gps does not update then fake position and horizontal veloctiy measurements
-		_fuse_hor_vel = true;	// we only fake horizontal velocity because we still have baro
-		_gps_sample_delayed.vel.setZero();
-	}
-
+    } else if (!_control_status.flags.gps && !_control_status.flags.opt_flow && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
+        _fuse_pos = true;
+        _gps_sample_delayed.pos(0) = _last_known_posNE(0);
+        _gps_sample_delayed.pos(1) = _last_known_posNE(1);
+        _time_last_fake_gps = _time_last_imu;
+    }
 
 	if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
 		fuseVelPosHeight();
@@ -178,6 +197,9 @@ bool Ekf::initialiseFilter(void)
 	_state.quat_nominal = Quaternion(euler_init);
 	_output_new.quat_nominal = _state.quat_nominal;
 
+    // TODO replace this with a conditional test based on fitered angle error states.
+    _control_status.flags.angle_align = true;
+
 	// calculate initial earth magnetic field states
 	matrix::Dcm<float> R_to_earth(euler_init);
 	_state.mag_I = R_to_earth * mag_init.mag;

EKF/ekf.h

@@ -36,6 +36,7 @@
  * Class for core functions for ekf attitude and position estimator.
  *
  * @author Roman Bast <bapstroman@gmail.com>
+ * @author Paul Riseborough <p_riseborough@live.com.au>
  *
  */
 
@@ -78,6 +79,28 @@ public:
 	// get the diagonal elements of the covariance matrix
 	void get_covariances(float *covariances);
 
+    // bitmask containing filter control status
+    union filter_control_status_u {
+        struct {
+            uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete
+            uint8_t gps         : 1; // 1 - true if GPS measurements are being fused
+            uint8_t opt_flow    : 1; // 2 - true if optical flow measurements are being fused
+            uint8_t mag_hdg     : 1; // 3 - true if a simple magnetic heading is being fused
+            uint8_t mag_3D      : 1; // 4 - true if 3-axis magnetometer measurement are being fused
+            uint8_t mag_dec     : 1; // 5 - true if synthetic magnetic declination measurements are being fused
+            uint8_t in_air      : 1; // 6 - true when the vehicle is airborne
+            uint8_t armed       : 1; // 7 - true when the vehicle motors are armed
+        } flags;
+        uint16_t value;
+    };
+    filter_control_status_u _control_status={};
+
+    // get the ekf WGS-84 origin positoin and height and the system time it was last set
+    void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
+
+    // set vehicle arm status data
+    void set_arm_status(bool data);
+
 private:
 
 	static const uint8_t _k_num_states = 24;
@@ -91,7 +114,14 @@ private:
 	bool _fuse_hor_vel;		// gps horizontal velocity measurement should be fused
 	bool _fuse_vert_vel;	// gps vertical velocity measurement should be fused
 
-	uint64_t _time_last_fake_gps;
+    uint64_t _time_last_fake_gps;
+
+    uint64_t _time_last_pos_fuse;   // time the last fusion of horizotal position measurements was performed (usec)
+    uint64_t _time_last_vel_fuse;   // time the last fusion of velocity measurements was performed (usec)
+    uint64_t _time_last_hgt_fuse;   // time the last fusion of height measurements was performed (usec)
+    uint64_t _time_last_of_fuse;    // time the last fusion of optical flow measurements were performed (usec)
+    Vector2f _last_known_posNE;     // last known local NE position vector (m)
+    float _last_disarmed_posD;      // vertical position recorded at arming (m)
 
 	Vector3f _earth_rate_NED;
 
@@ -112,7 +142,40 @@ private:
 	Vector3f _delta_vel_corr;
 	Vector3f _vel_corr;
 
-	void calculateOutputStates();
+    // variables used for the GPS quality checks
+    float _gpsDriftVelN = 0.0f;     // GPS north position derivative (m/s)
+    float _gpsDriftVelE = 0.0f;     // GPS east position derivative (m/s)
+    float _gps_drift_velD = 0.0f;     // GPS down position derivative (m/s)
+    float _gps_velD_diff_filt = 0.0f;   // GPS filtered Down velocity (m/s)
+    float _gps_velN_filt = 0.0f;  // GPS filtered North velocity (m/s)
+    float _gps_velE_filt = 0.0f;   // GPS filtered East velocity (m/s)
+    uint64_t _last_gps_fail_us = 0;   // last system time in usec that the GPS failed it's checks
+
+    // Variables used to publish the WGS-84 location of the EKF local NED origin
+    uint64_t _last_gps_origin_time_us = 0;              // time the origin was last set (uSec)
+    struct map_projection_reference_s _pos_ref = {};    // Contains WGS-84 position latitude and longitude (radians)
+    float _gps_alt_ref = 0.0f;                          // WGS-84 height (m)
+
+    bool _vehicle_armed = false;     // vehicle arm status used to turn off funtionality used on the ground
+
+    // publish the status of various GPS quality checks
+    union gps_check_fail_status_u {
+        struct {
+            uint16_t nsats  : 1; // 0 - true if number of satellites used is insufficient
+            uint16_t gdop   : 1; // 1 - true if geometric dilution of precision is insufficient
+            uint16_t hacc   : 1; // 2 - true if reported horizontal accuracy is insufficient
+            uint16_t vacc   : 1; // 3 - true if reported vertical accuracy is insufficient
+            uint16_t sacc   : 1; // 4 - true if reported speed accuracy is insufficient
+            uint16_t hdrift : 1; // 5 - true if horizontal drift is excessive (can only be used when stationary on ground)
+            uint16_t vdrift : 1; // 6 - true if vertical drift is excessive (can only be used when stationary on ground)
+            uint16_t hspeed : 1; // 7 - true if horizontal speed is excessive (can only be used when stationary on ground)
+            uint16_t vspeed : 1; // 8 - true if vertical speed error is excessive
+        } flags;
+        uint16_t value;
+    };
+    gps_check_fail_status_u _gps_check_fail_status;
+
+    void calculateOutputStates();
 
 	bool initialiseFilter(void);
 
@@ -156,4 +219,14 @@ private:
 
 	void calcEarthRateNED(Vector3f &omega, double lat_rad) const;
 
+    void initialiseGPS(struct gps_message *gps);
+
+    // return true id the GPS quality is good enough to set an origin and start aiding
+    bool gps_is_good(struct gps_message *gps);
+
+    // Control the filter fusion modes
+    void controlFusionModes();
+
+    // Determine if we are airborne or motors are armed
+    void calculateVehicleStatus();
 };

EKF/ekf_helper.cpp

@@ -254,3 +254,18 @@ void Ekf::get_covariances(float *covariances)
 		covariances[i] = P[i][i];
 	}
 }
+
+// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
+void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt)
+{
+    memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t));
+    memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
+    memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
+}
+
+// set the vehicle arm status
+void Ekf::set_arm_status(bool data)
+{
+    _vehicle_armed = data;
+
+}

EKF/estimator_base.cpp

@@ -165,22 +165,20 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
 		return;
 	}
 
-    if (time_usec - _time_last_gps > 70000) {
+    // Only use GPS data if we have a 3D fix and limit the GPS data rate to a maximum of 14Hz
+    if (time_usec - _time_last_gps > 70000 && gps->fix_type >= 3) {
 		gpsSample gps_sample_new = {};
 		gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
 
-
 		gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
 		_time_last_gps = time_usec;
 
 		gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
 
-
 		memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data));
 
 		_gps_speed_valid = gps->vel_ned_valid;
 
-
 		float lpos_x = 0.0f;
 		float lpos_y = 0.0f;
         map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
@@ -293,7 +291,6 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
 	_gps_speed_valid = false;
 
 	_mag_healthy = false;
-	_in_air = false;			// XXX get this flag from the application
 
 	_time_last_imu = 0;
 	_time_last_gps = 0;
@@ -306,21 +303,6 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
 
 }
 
-void EstimatorBase::initialiseGPS(struct gps_message *gps)
-{
-	//Check if the GPS fix is good enough for us to use
-	if (gps_is_good(gps)) {
-		printf("gps is good\n");
-		// Initialise projection
-		double lat = gps->lat / 1.0e7;
-		double lon = gps->lon / 1.0e7;
-        map_projection_init(&_pos_ref, lat, lon);
-		_gps_alt_ref = gps->alt / 1e3f;
-		_gps_initialised = true;
-        _last_gps_origin_time_us = _time_last_imu;
-	}
-}
-
 bool EstimatorBase::position_is_valid()
 {
 	// return true if the position estimate is valid

EKF/estimator_base.h

@@ -159,6 +159,13 @@ public:
 	// return a address to the parameters struct
 	// in order to give access to the application
 	parameters *getParamHandle() {return &_params;}
+
+    // get the ekf WGS-84 origin positoin and height and the system time it was last set
+    virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
+
+    // set vehicle arm status data
+    void set_arm_status(bool data);
+
 protected:
 
 	typedef matrix::Vector<float, 2> Vector2f;
@@ -267,8 +274,6 @@ protected:
 
     float _vel_pos_test_ratio[6];   // velocity and position innovation consistency check ratios
 
-    bool _in_air = true;			// indicates if the vehicle is in the air
-
 	RingBuffer<imuSample> _imu_buffer;
 	RingBuffer<gpsSample> _gps_buffer;
 	RingBuffer<magSample> _mag_buffer;
@@ -296,19 +301,6 @@ protected:
 
 	void initialiseVariables(uint64_t timestamp);
 
-	void initialiseGPS(struct gps_message *gps);
-
-	bool gps_is_good(struct gps_message *gps);
-
-    // variables used for the GPS quality checks
-    float _gpsDriftVelN = 0.0f;     // GPS north position derivative (m/s)
-    float _gpsDriftVelE = 0.0f;     // GPS east position derivative (m/s)
-    float _gps_drift_velD = 0.0f;     // GPS down position derivative (m/s)
-    float _gps_velD_diff_filt = 0.0f;   // GPS filtered Down velocity (m/s)
-    float _gps_velN_filt = 0.0f;  // GPS filtered North velocity (m/s)
-    float _gps_velE_filt = 0.0f;   // GPS filtered East velocity (m/s)
-    uint64_t _last_gps_fail_us = 0;   // last system time in usec that the GPS failed it's checks
-
 public:
 	void printIMU(struct imuSample *data);
 	void printStoredIMU();
@@ -345,29 +337,6 @@ public:
 	{
 		*time_us = _imu_time_last;
 	}
-
-    // Variables used to publish the WGS-84 location of the EKF local NED origin
-    uint64_t _last_gps_origin_time_us = 0;              // time the origin was last set (uSec)
-    struct map_projection_reference_s _pos_ref = {};     // Contains WGS-84 position latitude and longitude (radians)
-    float _gps_alt_ref = 0.0f;                          // WGS-84 height (m)
-
     bool _vehicle_armed = false;     // vehicle arm status used to turn off funtionality used on the ground
 
-    // publish the status of various GPS quality checks
-    union gps_check_fail_status_u {
-        struct {
-            uint16_t nsats  : 1; // 0 - true if number of satellites used is insufficient
-            uint16_t gdop   : 1; // 1 - true if geometric dilution of precision is insufficient
-            uint16_t hacc   : 1; // 2 - true if reported horizontal accuracy is insufficient
-            uint16_t vacc   : 1; // 3 - true if reported vertical accuracy is insufficient
-            uint16_t sacc   : 1; // 4 - true if reported speed accuracy is insufficient
-            uint16_t hdrift : 1; // 5 - true if horizontal drift is excessive (can only be used when stationary on ground)
-            uint16_t vdrift : 1; // 6 - true if vertical drift is excessive (can only be used when stationary on ground)
-            uint16_t hspeed : 1; // 7 - true if horizontal speed is excessive (can only be used when stationary on ground)
-            uint16_t vspeed : 1; // 8 - true if vertical speed error is excessive
-        } flags;
-        uint16_t value;
-    };
-    gps_check_fail_status_u _gps_check_fail_status;
-
 };

EKF/gps_checks.cpp

@@ -52,6 +52,21 @@
 #define MASK_GPS_HSPD   (1<<7)
 #define MASK_GPS_VSPD   (1<<8)
 
+void Ekf::initialiseGPS(struct gps_message *gps)
+{
+    //Check if the GPS fix is good enough for us to use
+    if (gps_is_good(gps)) {
+        printf("gps is good\n");
+        // Initialise projection
+        double lat = gps->lat / 1.0e7;
+        double lon = gps->lon / 1.0e7;
+        map_projection_init(&_pos_ref, lat, lon);
+        _gps_alt_ref = gps->alt / 1e3f;
+        _gps_initialised = true;
+        _last_gps_origin_time_us = _time_last_imu;
+    }
+}
+
 /*
  * Return true if the GPS solution quality is adequate to set an origin for the EKF
  * and start GPS aiding.
@@ -59,7 +74,7 @@
  * Checks are activated using the EKF2_GPS_CHECKS bitmask parameter
  * Checks are adjusted using the EKF2_REQ_* parameters
 */
-bool EstimatorBase::gps_is_good(struct gps_message *gps)
+bool Ekf::gps_is_good(struct gps_message *gps)
 {
     // Check the number of satellites
     _gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats);
@@ -104,7 +119,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
 
     // Calculate the horizontal drift speed and fail if too high
     // This check can only be used if the vehicle is stationary during alignment
-    if(_vehicle_armed) {
+    if(!_control_status.flags.armed) {
         float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
         _gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift);
     } else {
@@ -127,7 +142,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
 
     // Fail if the vertical drift speed is too high
     // This check can only be used if the vehicle is stationary during alignment
-    if(_vehicle_armed) {
+    if(!_control_status.flags.armed) {
         _gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift);
     } else {
         _gps_check_fail_status.flags.vdrift = false;
@@ -135,7 +150,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
 
     // Check the magnitude of the filtered horizontal GPS velocity
     // This check can only be used if the vehicle is stationary during alignment
-    if (_vehicle_armed) {
+    if (!_control_status.flags.armed) {
         vel_limit = 10.0f * _params.req_hdrift;
         float velN = fminf(fmaxf(gps->vel_ned[0],-vel_limit),vel_limit);
         float velE = fminf(fmaxf(gps->vel_ned[1],-vel_limit),vel_limit);

EKF/mag_fusion.cpp

@@ -561,7 +561,7 @@ void Ekf::fuseHeading()
 		// if we are in air we don't want to fuse the measurement
 		// we allow to use it when on the ground because the large innovation could be caused
 		// by interference or a large initial gyro bias
-		if (_in_air) {
+        if (_control_status.flags.in_air) {
 			return;
 		}
 

EKF/vel_pos_fusion.cpp

@@ -49,7 +49,7 @@ void Ekf::fuseVelPosHeight()
     float gate_size[6] = {};
 	float Kfusion[24] = {};
 
-	// calculate innovations
+    // calculate innovations and gate sizes
 	if (_fuse_hor_vel) {
 		fuse_map[0] = fuse_map[1] = true;
 		_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
@@ -97,10 +97,29 @@ void Ekf::fuseVelPosHeight()
 
     // check position, velocity and height innovations
     // treat 3D velocity, 2D position and height as separate sensors
-    innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f);
-    innov_check_pass_map[4] = innov_check_pass_map[3] = (_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f);
+    // always pass position checks if using synthetic position measurements
+    bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f);
+    innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
+    bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow;
+    bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || using_synthetic_measurements;
+    innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
     innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f);
 
+    // record the successful velocity fusion time
+    if (vel_check_pass && _fuse_hor_vel) {
+        _time_last_vel_fuse = _time_last_imu;
+    }
+
+    // record the successful position fusion time
+    if (pos_check_pass && _fuse_pos) {
+        _time_last_pos_fuse = _time_last_imu;
+    }
+
+    // record the successful height fusion time
+    if (innov_check_pass_map[5] && _fuse_height) {
+        _time_last_hgt_fuse = _time_last_imu;
+    }
+
     for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
         // skip fusion if not requested or checks have failed
         if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) {