refactor orientation_filter and sensor_data_receiver roll_pitch filter

Author: schluis

crates/control/src/orientation_filter.rs

@@ -62,12 +62,11 @@ impl OrientationFilter {
             .sensor_data
             .inertial_measurement_unit
             .linear_acceleration;
+        let angular_velocity_sum = measured_angular_velocity.abs().inner.sum();
 
         match &mut self.state {
             State::WaitingForSteady => {
-                if measured_angular_velocity.abs().inner.sum()
-                    < *context.calibration_steady_threshold
-                {
+                if angular_velocity_sum < *context.calibration_steady_threshold {
                     self.state = State::CalibratingGravity {
                         filtered_gravity: LowPassFilter::with_smoothing_factor(
                             measured_acceleration,
@@ -81,9 +80,9 @@ impl OrientationFilter {
                 filtered_gravity,
                 remaining_cycles,
             } => {
-                if measured_angular_velocity.abs().inner.sum()
-                    < *context.calibration_steady_threshold
-                {
+                if angular_velocity_sum > *context.calibration_steady_threshold {
+                    self.state = State::WaitingForSteady;
+                } else {
                     filtered_gravity.update(measured_acceleration);
                     *remaining_cycles -= 1;
                     if *remaining_cycles == 0 {
@@ -92,8 +91,6 @@ impl OrientationFilter {
                         let orientation = UnitQuaternion::look_at_rh(&gravity, &up);
                         self.state = State::Filtering { state: orientation };
                     }
-                } else {
-                    self.state = State::WaitingForSteady;
                 }
             }
             State::Filtering { state } => {

crates/control/src/sensor_data_receiver.rs

@@ -80,12 +80,11 @@ impl SensorDataReceiver {
         let measured_angular_velocity = sensor_data.inertial_measurement_unit.angular_velocity;
         let measured_acceleration = sensor_data.inertial_measurement_unit.linear_acceleration;
         let measured_roll_pitch = sensor_data.inertial_measurement_unit.roll_pitch;
+        let angular_velocity_sum = measured_angular_velocity.abs().inner.sum();
 
         match &mut self.calibration_state {
             State::WaitingForSteady => {
-                if measured_angular_velocity.abs().inner.sum()
-                    < *context.calibration_steady_threshold
-                {
+                if angular_velocity_sum < *context.calibration_steady_threshold {
                     self.calibration_state = State::CalibratingGravity {
                         filtered_gravity: LowPassFilter::with_smoothing_factor(
                             measured_acceleration,
@@ -104,9 +103,9 @@ impl SensorDataReceiver {
                 filtered_roll_pitch,
                 remaining_cycles,
             } => {
-                if measured_angular_velocity.abs().inner.sum()
-                    < *context.calibration_steady_threshold
-                {
+                if angular_velocity_sum < *context.calibration_steady_threshold {
+                    self.calibration_state = State::WaitingForSteady;
+                } else {
                     filtered_gravity.update(measured_acceleration);
                     filtered_roll_pitch.update(measured_roll_pitch);
                     *remaining_cycles -= 1;
@@ -129,27 +128,23 @@ impl SensorDataReceiver {
                             calibration: roll_pitch_calibration,
                         };
                     }
-                } else {
-                    self.calibration_state = State::WaitingForSteady;
                 }
             }
-            State::Calibrated { .. } => {}
-        }
-
-        if let State::Calibrated { calibration } = self.calibration_state {
-            let mut roll_pitch_orientation = Orientation3::<Robot>::from_euler_angles(
-                -sensor_data.inertial_measurement_unit.roll_pitch.x(),
-                -sensor_data.inertial_measurement_unit.roll_pitch.y(),
-                0.0,
-            )
-            .mirror();
+            State::Calibrated { calibration } => {
+                let mut roll_pitch_orientation = Orientation3::<Robot>::from_euler_angles(
+                    -sensor_data.inertial_measurement_unit.roll_pitch.x(),
+                    -sensor_data.inertial_measurement_unit.roll_pitch.y(),
+                    0.0,
+                )
+                .mirror();
 
-            roll_pitch_orientation.inner = calibration * roll_pitch_orientation.inner;
+                roll_pitch_orientation.inner = *calibration * roll_pitch_orientation.inner;
 
-            let (roll, pitch, _) = roll_pitch_orientation.euler_angles();
+                let (roll, pitch, _) = roll_pitch_orientation.euler_angles();
 
-            sensor_data.inertial_measurement_unit.roll_pitch.inner.x = roll;
-            sensor_data.inertial_measurement_unit.roll_pitch.inner.y = pitch;
+                sensor_data.inertial_measurement_unit.roll_pitch.inner.x = roll;
+                sensor_data.inertial_measurement_unit.roll_pitch.inner.y = pitch;
+            }
         }
 
         sensor_data.positions = sensor_data.positions - (*context.joint_calibration_offsets);