From 2bbdef205e163181cb8c02b08aeceb4c69101b36 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 10 May 2020 14:53:52 -0400 Subject: [PATCH] sensors/vehicle_angular_velocity: don't remove bias twice --- .../vehicle_angular_velocity/VehicleAngularVelocity.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 32fc701b9d..946b706c67 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -257,10 +257,7 @@ void VehicleAngularVelocity::Run() const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; // correct for in-run bias errors - Vector3f angular_velocity_raw = _corrections.Correct(val) - _bias; - - // correct for in-run bias errors - angular_velocity_raw -= _bias; + const Vector3f angular_velocity_raw = _corrections.Correct(val) - _bias; // Differentiate angular velocity (after notch filter) const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)};