From fabac94952995d42c5e72f94d83f29d55a5846b6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Nov 2020 20:11:58 +1100 Subject: [PATCH] AP_DAL: avoid logging unused IMU data --- libraries/AP_DAL/AP_DAL_InertialSensor.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/libraries/AP_DAL/AP_DAL_InertialSensor.cpp b/libraries/AP_DAL/AP_DAL_InertialSensor.cpp index 26f2f8b778..926c9f8bcb 100644 --- a/libraries/AP_DAL/AP_DAL_InertialSensor.cpp +++ b/libraries/AP_DAL/AP_DAL_InertialSensor.cpp @@ -29,15 +29,19 @@ void AP_DAL_InertialSensor::start_frame() log_RISI &RISI = _RISI[i]; const log_RISI old_RISI = RISI; - // accel stuff + // accel data RISI.use_accel = ins.use_accel(i); - RISI.get_delta_velocity_ret = ins.get_delta_velocity(i, RISI.delta_velocity); - RISI.delta_velocity_dt = ins.get_delta_velocity_dt(i); + if (RISI.use_accel) { + RISI.get_delta_velocity_ret = ins.get_delta_velocity(i, RISI.delta_velocity); + RISI.delta_velocity_dt = ins.get_delta_velocity_dt(i); + } - // gryo stuff + // gryo data RISI.use_gyro = ins.use_gyro(i); - RISI.delta_angle_dt = ins.get_delta_angle_dt(i); - RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle); + if (RISI.use_gyro) { + RISI.delta_angle_dt = ins.get_delta_angle_dt(i); + RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle); + } update_filtered(i);