From 9f0d8ed59ee6f60d95187dc230f8cb2407feb31f Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 21 Jan 2020 14:49:19 +0100 Subject: [PATCH] Update mag interface --- EKF/estimator_interface.cpp | 11 +++++------ EKF/estimator_interface.h | 2 +- test/sensor_simulator/mag.cpp | 7 ++++--- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index b8380be3d8..7b8d21418f 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -130,7 +130,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u imu_sample_new.delta_ang = Vector3f(delta_ang); imu_sample_new.delta_vel = Vector3f(delta_vel); - // convert time from us to secs +void EstimatorInterface::setMagData(const magSample &mag_sample) imu_sample_new.delta_ang_dt = delta_ang_dt / 1e6f; imu_sample_new.delta_vel_dt = delta_vel_dt / 1e6f; imu_sample_new.time_us = time_usec; @@ -138,7 +138,6 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u setIMUData(imu_sample_new); } -void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3]) { if (!_initialised || _mag_buffer_fail) { return; @@ -158,11 +157,12 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3]) // downsample to highest possible sensor rate // by taking the average of incoming sample _mag_sample_count++; - _mag_data_sum += Vector3f(data); - _mag_timestamp_sum += time_usec / 1000; // Dividing by 1000 to avoid overflow + _mag_data_sum += mag_sample.mag; + _mag_timestamp_sum += mag_sample.time_us / 1000; // Dividing by 1000 to avoid overflow // limit data rate to prevent data being lost - if ((time_usec - _time_last_mag) > _min_obs_interval_us) { + if ((mag_sample.time_us - _time_last_mag) > _min_obs_interval_us) { + _time_last_mag = mag_sample.time_us; magSample mag_sample_new; @@ -175,7 +175,6 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3]) _mag_buffer.push(mag_sample_new); - _time_last_mag = time_usec; _mag_sample_count = 0; _mag_data_sum.setZero(); _mag_timestamp_sum = 0; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 76becbb059..e548b96ccf 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -177,7 +177,7 @@ public: // legacy interface for compatibility (2018-09-14) void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float (&delta_ang)[3], float (&delta_vel)[3]); - void setMagData(uint64_t time_usec, float (&data)[3]); + void setMagData(const magSample &mag_sample); void setGpsData(const gps_message &gps); diff --git a/test/sensor_simulator/mag.cpp b/test/sensor_simulator/mag.cpp index d40493ce44..c5a9744d98 100644 --- a/test/sensor_simulator/mag.cpp +++ b/test/sensor_simulator/mag.cpp @@ -15,9 +15,10 @@ Mag::~Mag() void Mag::send(uint64_t time) { - float mag[3]; - _mag_data.copyTo(mag); - _ekf->setMagData(time,mag); + magSample mag_sample; + mag_sample.mag = _mag_data; + mag_sample.time_us = time; + _ekf->setMagData(mag_sample); } void Mag::setData(const Vector3f& mag)