refactor orientation_filter and sensor_data_receiver roll_pitch filter

Author: schluis

crates/control/src/orientation_filter.rs

@@ -63,13 +63,12 @@ impl OrientationFilter {
             .sensor_data
             .inertial_measurement_unit
             .linear_acceleration;
+        let angular_velocity_sum = measured_angular_velocity.abs().inner.sum();
 
         let mut recalibrated_this_cycle = false;
         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,
@@ -83,9 +82,7 @@ impl OrientationFilter {
                 filtered_gravity,
                 number_of_cycles,
             } => 'update: {
-                if measured_angular_velocity.abs().inner.sum()
-                    >= *context.calibration_steady_threshold
-                {
+                if angular_velocity_sum >= *context.calibration_steady_threshold {
                     self.calibration_state = State::WaitingForSteady;
                     break 'update;
                 }

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,7 @@ 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 {
                     filtered_gravity.update(measured_acceleration);
                     filtered_roll_pitch.update(measured_roll_pitch);
                     *remaining_cycles -= 1;