From 74ec80cdc77646e9a200fa441f03f16535a20f2a Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 21 Jan 2020 14:45:31 +0100 Subject: [PATCH] Update gps interface --- EKF/common.h | 2 +- EKF/estimator_interface.cpp | 14 ++++++++------ EKF/estimator_interface.h | 2 +- EKF/gps_checks.cpp | 8 ++++---- test/sensor_simulator/gps.cpp | 10 +++------- 5 files changed, 17 insertions(+), 19 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 718377ba29..161c343674 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -67,7 +67,7 @@ struct gps_message { float epv; ///< GPS vertical position accuracy in m float sacc; ///< GPS speed accuracy in m/s float vel_m_s; ///< GPS ground speed (m/sec) - float vel_ned[3]; ///< GPS ground speed NED - TODO: make Vector3f + Vector3f vel_ned; ///< GPS ground speed NED - TODO: make Vector3f bool vel_ned_valid; ///< GPS ground speed is valid uint8_t nsats; ///< number of satellites used float pdop; ///< position dilution of precision diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 8c0d3db9a0..67a54c28b8 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -182,7 +182,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3]) } } -void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps) +void EstimatorInterface::setGpsData(const gps_message &gps) { if (!_initialised || _gps_buffer_fail) { return; @@ -202,15 +202,17 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps) // limit data rate to prevent data being lost bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS); - if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) { + // TODO: remove checks that are not timing related + if (((gps.time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) { + _time_last_gps = gps.time_usec; + gpsSample gps_sample_new; + gps_sample_new.time_us = gps.time_usec - _params.gps_delay_ms * 1000; - gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_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); - gps_sample_new.vel = Vector3f(gps.vel_ned); + + gps_sample_new.vel = gps.vel_ned; _gps_speed_valid = gps.vel_ned_valid; gps_sample_new.sacc = gps.sacc; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 19a133cfc1..0dbf7bbeff 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -179,7 +179,7 @@ public: void setMagData(uint64_t time_usec, float (&data)[3]); - void setGpsData(uint64_t time_usec, const gps_message &gps); + void setGpsData(const gps_message &gps); void setBaroData(uint64_t time_usec, float data); diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index af3b41833a..613bac59f3 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -175,9 +175,9 @@ bool Ekf::gps_is_good(const gps_message &gps) _gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift); // Check the magnitude of the filtered horizontal GPS velocity - Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned[0], gps.vel_ned[1]), // TODO: when vel_ned vector3f use .xy() - -10.0f * _params.req_hdrift, - 10.0f * _params.req_hdrift); + Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()), + -10.0f * _params.req_hdrift, + 10.0f * _params.req_hdrift); _gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef); _gps_drift_metrics[2] = _gps_velNE_filt.norm(); _gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift); @@ -204,7 +204,7 @@ bool Ekf::gps_is_good(const gps_message &gps) // Check the filtered difference between GPS and EKF vertical velocity float vz_diff_limit = 10.0f * _params.req_vdrift; - float vertVel = fminf(fmaxf((gps.vel_ned[2] - _state.vel(2)), -vz_diff_limit), vz_diff_limit); + float vertVel = fminf(fmaxf((gps.vel_ned(2) - _state.vel(2)), -vz_diff_limit), vz_diff_limit); _gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef); _gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift); diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp index f6931a54e4..bf18a4b4dd 100644 --- a/test/sensor_simulator/gps.cpp +++ b/test/sensor_simulator/gps.cpp @@ -16,7 +16,7 @@ Gps::~Gps() void Gps::send(uint64_t time) { _gps_data.time_usec = time; - _ekf->setGpsData(time, _gps_data); + _ekf->setGpsData(_gps_data); } void Gps::setData(const gps_message& gps) @@ -41,9 +41,7 @@ void Gps::setLongitude(int32_t lon) void Gps::setVelocity(const Vector3f& vel) { - _gps_data.vel_ned[0] = vel(0); - _gps_data.vel_ned[1] = vel(1); - _gps_data.vel_ned[2] = vel(2); + _gps_data.vel_ned = vel; } void Gps::stepHeightByMeters(float hgt_change) @@ -79,9 +77,7 @@ gps_message Gps::getDefaultGpsData() gps_data.epv = 0.8f; gps_data.sacc = 0.2f; gps_data.vel_m_s = 0.0; - gps_data.vel_ned[0] = 0.0f; - gps_data.vel_ned[1] = 0.0f; - gps_data.vel_ned[2] = 0.0f; + gps_data.vel_ned.setZero(); gps_data.vel_ned_valid = 1; gps_data.nsats = 16; gps_data.pdop = 0.0f;