From 439a20bfcb65ccf7e534ba56adb851c5de062e6c Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 21 Jan 2020 14:38:31 +0100 Subject: [PATCH] Update EKF mag interface --- src/modules/ekf2/ekf2_main.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 38b3a9ff7e..0018f00d44 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -893,12 +893,13 @@ void Ekf2::Run() PX4_INFO("Mag sensor ID changed to %i", _param_ekf2_magbias_id.get()); } - float magnetometer_ga [3]; - magnetometer_ga[0] = magnetometer.magnetometer_ga[0] - _param_ekf2_magbias_x.get(); - magnetometer_ga[1] = magnetometer.magnetometer_ga[1] - _param_ekf2_magbias_y.get(); - magnetometer_ga[2] = magnetometer.magnetometer_ga[2] - _param_ekf2_magbias_z.get(); + magSample mag_sample; + mag_sample.mag(0) = magnetometer.magnetometer_ga[0] - _param_ekf2_magbias_x.get(); + mag_sample.mag(1) = magnetometer.magnetometer_ga[1] - _param_ekf2_magbias_y.get(); + mag_sample.mag(2) = magnetometer.magnetometer_ga[2] - _param_ekf2_magbias_z.get(); + mag_sample.time_us = magnetometer.timestamp; - _ekf.setMagData(magnetometer.timestamp, magnetometer_ga); + _ekf.setMagData(mag_sample); ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); }