ekf: move gps checks to Ekf library from estimator_base

EKF/ekf.h

@@ -79,6 +79,9 @@ public:
 	// get the diagonal elements of the covariance matrix
 	void get_covariances(float *covariances);
 
+    // ask estimator for sensor data collection decision, returns true if not defined
+    bool collect_gps(uint64_t time_usec, struct gps_message *gps);
+
     // bitmask containing filter control status
     union filter_control_status_u {
         struct {
@@ -98,9 +101,6 @@ public:
     // 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;
@@ -153,11 +153,8 @@ private:
 
     // 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 {
@@ -172,8 +169,7 @@ private:
             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;
+    }_gps_check_fail_status;
 
     void calculateOutputStates();
 

EKF/ekf_helper.cpp

@@ -262,10 +262,3 @@ void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig
     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

@@ -141,7 +141,6 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
 void EstimatorBase::setMagData(uint64_t time_usec, float *data)
 {
 
-
 	if (time_usec - _time_last_mag > 70000) {
 
 		magSample mag_sample_new = {};
@@ -159,9 +158,7 @@ void EstimatorBase::setMagData(uint64_t time_usec, float *data)
 
 void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
 {
-
-	if (!_gps_initialised) {
-		initialiseGPS(gps);
+	if(!collect_gps(time_usec, gps)) {
 		return;
 	}
 

EKF/estimator_base.h

@@ -135,6 +135,24 @@ public:
 
 	virtual void get_covariances(float *covariances) = 0;
 
+    // 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;
+
+    // ask estimator for sensor data collection decision, returns true if not defined
+    virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; };
+
+    virtual bool collect_imu(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel) { return true; };
+
+    virtual bool collect_mag(uint64_t time_usec, float *data) { return true; };
+
+    virtual bool collect_baro(uint64_t time_usec, float *data) { return true; };
+
+    virtual bool collect_airspeed(uint64_t time_usec, float *data) { return true; };
+
+    virtual bool collect_range(uint64_t time_usec, float *data) { return true; };
+
+    virtual bool collect_opticalflow(uint64_t time_usec, float *data) { return true; };
+
 	// set delta angle imu data
 	void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel);
 
@@ -160,11 +178,9 @@ public:
 	// 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);
+    void set_arm_status(bool data){ _vehicle_armed = data; }
+
 
 protected:
 
@@ -245,8 +261,7 @@ protected:
 
 	imuSample _imu_sample_delayed;
 	imuSample _imu_down_sampled;
-	Quaternion
-	_q_down_sampled;
+	Quaternion _q_down_sampled;
 
 	magSample _mag_sample_delayed;
 	baroSample _baro_sample_delayed;
@@ -265,8 +280,10 @@ protected:
 	bool _imu_updated = false;
 	bool _start_predict_enabled = false;
 	bool _initialised = false;
-	bool _gps_initialised = false;
-	bool _gps_speed_valid = false;
+
+    bool _gps_initialised = false;
+    bool _gps_speed_valid = false;
+    struct map_projection_reference_s _pos_ref = {};    // Contains WGS-84 position latitude and longitude (radians)
 
 	bool _mag_healthy = false;		// computed by mag innovation test
     float _yaw_test_ratio;          // yaw innovation consistency check ratio

EKF/gps_checks.cpp

@@ -52,6 +52,16 @@
 #define MASK_GPS_HSPD   (1<<7)
 #define MASK_GPS_VSPD   (1<<8)
 
+bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
+{
+    if(_gps_initialised) {
+        return true;
+    } else {
+        initialiseGPS(gps);
+    }
+    return false;
+}
+
 void Ekf::initialiseGPS(struct gps_message *gps)
 {
     //Check if the GPS fix is good enough for us to use