diff --git a/EKF/common.h b/EKF/common.h index 8c4b2d7de9..a9cc104aa0 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -80,15 +80,6 @@ struct flow_message { uint32_t dt; ///< integration time of flow samples (microseconds) }; -struct ext_vision_message { - Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis - Vector3f vel; ///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis - Quatf quat; ///< quaternion defining rotation from body to earth frame - Vector3f posVar; ///< XYZ position variances (m**2) - Vector3f velVar; ///< XYZ velocity variances ((m/sec)**2) - float angVar; ///< angular heading variance (rad**2) -}; - struct outputSample { Quatf quat_nominal; ///< nominal quaternion describing vehicle attitude Vector3f vel; ///< NED velocity estimate in earth frame (m/sec) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 5d5e89c44a..502762c9a0 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -403,7 +403,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl } // set attitude and position data derived from an external vision system -void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message *evdata) +void EstimatorInterface::setExtVisionData(const extVisionSample& evdata) { if (!_initialised || _ev_buffer_fail) { return; @@ -421,19 +421,13 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message } // limit data rate to prevent data being lost - if ((time_usec - _time_last_ext_vision) > _min_obs_interval_us) { - extVisionSample ev_sample_new; + if ((evdata.time_us - _time_last_ext_vision) > _min_obs_interval_us) { + _time_last_ext_vision = evdata.time_us; + + extVisionSample ev_sample_new = evdata; // calculate the system time-stamp for the mid point of the integration period - ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000; - - ev_sample_new.angVar = evdata->angVar; - ev_sample_new.posVar = evdata->posVar; - ev_sample_new.velVar = evdata->velVar; - ev_sample_new.quat = evdata->quat; - ev_sample_new.pos = evdata->pos; - ev_sample_new.vel = evdata->vel; - - _time_last_ext_vision = time_usec; + ev_sample_new.time_us -= _params.ev_delay_ms * 1000; + ev_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; _ext_vision_buffer.push(ev_sample_new); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index c520e2516b..c45d725e26 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -189,7 +189,7 @@ public: void setOpticalFlowData(uint64_t time_usec, flow_message *flow); // set external vision position and attitude data - void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata); + void setExtVisionData(const extVisionSample& evdata); void setAuxVelData(const auxVelSample& auxvel_sample); diff --git a/test/sensor_simulator/vio.cpp b/test/sensor_simulator/vio.cpp index 8051c6b844..c32bca540c 100644 --- a/test/sensor_simulator/vio.cpp +++ b/test/sensor_simulator/vio.cpp @@ -15,10 +15,11 @@ Vio::~Vio() void Vio::send(uint64_t time) { - _ekf->setExtVisionData(time, &_vio_data); + _vio_data.time_us = time; + _ekf->setExtVisionData(_vio_data); } -void Vio::setData(const ext_vision_message& vio_data) +void Vio::setData(const extVisionSample& vio_data) { _vio_data = vio_data; } @@ -53,9 +54,9 @@ void Vio::setOrientation(const Quatf& quat) _vio_data.quat = quat; } -ext_vision_message Vio::dataAtRest() +extVisionSample Vio::dataAtRest() { - ext_vision_message vio_data; + extVisionSample vio_data; vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};; vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};; vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f}; diff --git a/test/sensor_simulator/vio.h b/test/sensor_simulator/vio.h index 731dda9ffd..5b873b508a 100644 --- a/test/sensor_simulator/vio.h +++ b/test/sensor_simulator/vio.h @@ -50,7 +50,7 @@ public: Vio(std::shared_ptr ekf); ~Vio(); - void setData(const ext_vision_message& vio_data); + void setData(const extVisionSample& vio_data); void setVelocityVariance(const Vector3f& velVar); void setPositionVariance(const Vector3f& posVar); void setAngularVariance(float angVar); @@ -58,10 +58,10 @@ public: void setPosition(const Vector3f& pos); void setOrientation(const Quatf& quat); - ext_vision_message dataAtRest(); + extVisionSample dataAtRest(); private: - ext_vision_message _vio_data; + extVisionSample _vio_data; void send(uint64_t time) override;