diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.cpp b/libraries/AP_IMU/AP_IMU_Oilpan.cpp index f4de20ac96..5af178e60d 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.cpp +++ b/libraries/AP_IMU/AP_IMU_Oilpan.cpp @@ -278,6 +278,10 @@ AP_IMU_Oilpan::update(void) _accel.y = _accel_scale * _sensor_in(4, tc_temp); _accel.z = _accel_scale * _sensor_in(5, tc_temp); + _accel_filtered.x = _accel_filtered.x * .98 + _accel.x * .02; + _accel_filtered.y = _accel_filtered.y * .98 + _accel.y * .02; + _accel_filtered.z = _accel_filtered.z * .98 + _accel.z * .02; + // always updated return true; } diff --git a/libraries/AP_IMU/IMU.h b/libraries/AP_IMU/IMU.h index bea43fddd0..6e31c394e5 100644 --- a/libraries/AP_IMU/IMU.h +++ b/libraries/AP_IMU/IMU.h @@ -70,6 +70,12 @@ public: /// Vector3f get_accel(void) { return _accel; } + /// Fetch the current accelerometer values + /// + /// @returns vector of current accelerations in m/s/s + /// + Vector3f get_accel_filtered(void) { return _accel_filtered; } + /// A count of bad sensor readings /// /// @todo This should be renamed, as there's no guarantee that sensors @@ -80,6 +86,7 @@ public: protected: /// Most recent accelerometer reading obtained by ::update Vector3f _accel; + Vector3f _accel_filtered; /// Most recent gyro reading obtained by ::update Vector3f _gyro;