From 251996d3870e7e3b158d67225c3a6d299edc571b Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Fri, 29 Jan 2016 17:15:18 -0800 Subject: [PATCH] ekf: move gps checks to Ekf library from estimator_base --- EKF/ekf.h | 12 ++++-------- EKF/ekf_helper.cpp | 7 ------- EKF/estimator_base.cpp | 5 +---- EKF/estimator_base.h | 33 +++++++++++++++++++++++++-------- EKF/gps_checks.cpp | 10 ++++++++++ 5 files changed, 40 insertions(+), 27 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 7e69916025..1123a44cea 100644 --- a/EKF/ekf.h +++ b/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(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 68b9d565d5..7f0c6251f1 100644 --- a/EKF/ekf_helper.cpp +++ b/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; - -} diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index 0fa2fc01c4..d1cb5350ff 100644 --- a/EKF/estimator_base.cpp +++ b/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; } diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index 5b7ddb7a11..7b36e57b4a 100644 --- a/EKF/estimator_base.h +++ b/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 diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index c167a6b7c2..6c78abc957 100644 --- a/EKF/gps_checks.cpp +++ b/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