From a0fcd74b528b880c1891e8dabfaa6e6d2586a696 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 12 Feb 2021 08:34:36 +1100 Subject: [PATCH] AP_DAL: remove separate calls to get delta-times for vel and ang --- libraries/AP_DAL/AP_DAL_InertialSensor.cpp | 6 ++---- libraries/AP_DAL/AP_DAL_InertialSensor.h | 10 ++++------ 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/libraries/AP_DAL/AP_DAL_InertialSensor.cpp b/libraries/AP_DAL/AP_DAL_InertialSensor.cpp index 027696b218..e8332cb7ad 100644 --- a/libraries/AP_DAL/AP_DAL_InertialSensor.cpp +++ b/libraries/AP_DAL/AP_DAL_InertialSensor.cpp @@ -32,15 +32,13 @@ void AP_DAL_InertialSensor::start_frame() // accel data RISI.use_accel = ins.use_accel(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); + RISI.get_delta_velocity_ret = ins.get_delta_velocity(i, RISI.delta_velocity, RISI.delta_velocity_dt); } // gryo data RISI.use_gyro = ins.use_gyro(i); 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); + RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle, RISI.delta_angle_dt); } update_filtered(i); diff --git a/libraries/AP_DAL/AP_DAL_InertialSensor.h b/libraries/AP_DAL/AP_DAL_InertialSensor.h index cc2ed13d74..4f482fcf37 100644 --- a/libraries/AP_DAL/AP_DAL_InertialSensor.h +++ b/libraries/AP_DAL/AP_DAL_InertialSensor.h @@ -24,13 +24,11 @@ public: bool use_accel(uint8_t instance) const { return _RISI[instance].use_accel; } const Vector3f &get_accel(uint8_t i) const { return accel_filtered[i]; } - bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const { + bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const { delta_velocity = _RISI[i].delta_velocity; + delta_velocity_dt = _RISI[i].delta_velocity_dt; return _RISI[i].get_delta_velocity_ret; } - float get_delta_velocity_dt(uint8_t i) const { - return _RISI[i].delta_velocity_dt; - } // gyro stuff uint8_t get_gyro_count(void) const { return _RISH.gyro_count; } @@ -39,11 +37,11 @@ public: bool use_gyro(uint8_t instance) const { return _RISI[instance].use_gyro; } const Vector3f &get_gyro(uint8_t i) const { return gyro_filtered[i]; } const Vector3f &get_gyro() const { return get_gyro(_primary_gyro); } - bool get_delta_angle(uint8_t i, Vector3f &delta_angle) const { + bool get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const { delta_angle = _RISI[i].delta_angle; + delta_angle_dt = _RISI[i].delta_angle_dt; return _RISI[i].get_delta_angle_ret; } - float get_delta_angle_dt(uint8_t i) const { return _RISI[i].delta_angle_dt; } // return the main loop delta_t in seconds float get_loop_delta_t(void) const { return _RISH.loop_delta_t; }