feat(calibration): implement notch adjustment

src/config.rs

@@ -5,6 +5,7 @@
 use core::{cmp::min, f32::consts::PI};
 
 use defmt::{debug, error, info, warn, Format};
+use embassy_futures::yield_now;
 use embassy_rp::{
     flash::{Async, Flash, ERASE_SIZE},
     peripherals::FLASH,
@@ -12,7 +13,7 @@ use embassy_rp::{
 use packed_struct::{derive::PackedStruct, PackedStruct};
 
 use crate::{
-    helpers::{PackedFloat, ToPackedFloatArray, XyValuePair},
+    helpers::{PackedFloat, ToPackedFloatArray, ToRegularArray, XyValuePair},
     input::{
         read_ext_adc, Stick, StickAxis, StickState, FLOAT_ORIGIN, SPI_ACS_SHARED, SPI_CCS_SHARED,
         SPI_SHARED,
@@ -38,6 +39,9 @@ use crate::{gcc_hid::GcReport, input::CHANNEL_GCC_STATE};
 /// Initial status is assumed to be false.
 pub static SIGNAL_IS_CALIBRATING: Signal<ThreadModeRawMutex, bool> = Signal::new();
 
+/// Config change signalled to the stick task.
+pub static SIGNAL_CONFIG_CHANGE: Signal<ThreadModeRawMutex, ControllerConfig> = Signal::new();
+
 /// Signal used to override the stick state in order to display desired stick positions during calibration.
 pub static SIGNAL_OVERRIDE_STICK_STATE: Signal<
     CriticalSectionRawMutex,
@@ -74,7 +78,7 @@ const BUTTON_POLL_INTERVAL_MILLIS: u64 = 20;
 
 /// This needs to be incremented for ANY change to ControllerConfig
 /// else we risk loading uninitialized memory.
-pub const CONTROLLER_CONFIG_REVISION: u8 = 2;
+pub const CONTROLLER_CONFIG_REVISION: u8 = 1;
 
 pub const DEFAULT_NOTCH_STATUS: [NotchStatus; NO_OF_NOTCHES] = [
     NotchStatus::Cardinal,
@@ -458,30 +462,32 @@ impl<'a> StickCalibrationProcess<'a> {
         }
     }
 
-    fn adjust_notch(&mut self, notch_adjustment_type: NotchAdjustmentType) -> Option<(f32, f32)> {
+    fn adjust_notch(&mut self, notch_adjustment_type: NotchAdjustmentType) {
+        let stick_config = match self.which_stick {
+            Stick::ControlStick => &mut self.gcc_config.astick_config,
+            Stick::CStick => &mut self.gcc_config.cstick_config,
+        };
+
         let notch_idx =
             NOTCH_ADJUSTMENT_ORDER[self.calibration_step as usize - NO_OF_CALIBRATION_POINTS];
 
         if self.applied_calibration.cleaned_calibration.notch_status[notch_idx]
             == NotchStatus::TertInactive
-        {
-            return None;
-        }
+        {}
 
         // assumes a tick rate of 1ms
         match notch_adjustment_type {
             NotchAdjustmentType::Clockwise => {
-                self.applied_calibration.notch_angles[notch_idx] -= 0.000075;
+                stick_config.angles[notch_idx] -= 0.0075;
             }
             NotchAdjustmentType::CounterClockwise => {
-                self.applied_calibration.notch_angles[notch_idx] += 0.000075;
+                stick_config.angles[notch_idx] += 0.0075;
             }
             NotchAdjustmentType::Reset => {
-                self.applied_calibration.notch_angles[notch_idx] = DEFAULT_ANGLES[notch_idx];
-            }
-            NotchAdjustmentType::None => {
-                return None;
+                stick_config.angles[notch_idx] =
+                    PackedFloat(self.applied_calibration.measured_notch_angles[notch_idx]);
             }
+            NotchAdjustmentType::None => {}
         }
 
         match notch_adjustment_type {
@@ -492,7 +498,7 @@ impl<'a> StickCalibrationProcess<'a> {
                     CleanedCalibrationPoints::from_temp_calibration_points(
                         &self.cal_points.map(|e| e.x),
                         &self.cal_points.map(|e| e.y),
-                        &self.applied_calibration.notch_angles,
+                        &self.applied_calibration.measured_notch_angles,
                     );
 
                 let linearized_calibration =
@@ -512,15 +518,18 @@ impl<'a> StickCalibrationProcess<'a> {
                     notch_calibration.affine_coeffs;
                 self.applied_calibration.stick_params.boundary_angles =
                     notch_calibration.boundary_angles;
-            }
-            NotchAdjustmentType::None => return None,
-        }
 
-        Some(
-            match calc_stick_values(self.applied_calibration.measured_notch_angles[notch_idx]) {
-                (x, y) => (x + FLOAT_ORIGIN, y + FLOAT_ORIGIN),
-            },
-        )
+                stick_config.angles = *legalize_notches(
+                    self.calibration_step as usize,
+                    &self.applied_calibration.measured_notch_angles,
+                    &stick_config.angles.to_regular_array(),
+                )
+                .to_packed_float_array();
+
+                SIGNAL_CONFIG_CHANGE.signal(self.gcc_config.clone());
+            }
+            NotchAdjustmentType::None => {}
+        }
     }
 
     async fn calibration_advance(&mut self) -> bool {
@@ -593,7 +602,7 @@ impl<'a> StickCalibrationProcess<'a> {
                 stick_config.angles = *legalize_notches(
                     self.calibration_step as usize,
                     &self.applied_calibration.measured_notch_angles,
-                    &stick_config.angles.map(|e| *e),
+                    &stick_config.angles.to_regular_array(),
                 )
                 .to_packed_float_array();
 
@@ -610,6 +619,8 @@ impl<'a> StickCalibrationProcess<'a> {
             stick_config.cal_points_x = self.cal_points.map(|p| p.x.into());
             stick_config.cal_points_y = self.cal_points.map(|p| p.y.into());
 
+            SIGNAL_CONFIG_CHANGE.signal(self.gcc_config.clone());
+
             info!("Finished calibrating stick {}", self.which_stick);
 
             return true;
@@ -687,7 +698,7 @@ impl<'a> StickCalibrationProcess<'a> {
                         AwaitableButtons::Y,
                     ]);
 
-                    let override_result = match btn_result {
+                    match btn_result {
                         Some(btn) => match btn {
                             AwaitableButtons::A => {
                                 debug!("Btn A release pressed");
@@ -705,18 +716,8 @@ impl<'a> StickCalibrationProcess<'a> {
                         None => self.adjust_notch(NotchAdjustmentType::None),
                     };
 
-                    if let Some((x, y)) = override_result {
-                        SIGNAL_OVERRIDE_STICK_STATE.signal(Some(OverrideStickState {
-                            x: x as u8,
-                            y: y as u8,
-                            which_stick: match self.which_stick {
-                                Stick::ControlStick => Stick::CStick,
-                                Stick::CStick => Stick::ControlStick,
-                            },
-                        }));
-                    }
-
                     ticker.next().await;
+                    yield_now().await;
                 }
             };
 

src/gcc_hid.rs

@@ -360,7 +360,13 @@ pub async fn usb_transfer_task(
                     trace!("Report Written: {:08b}", report);
                     let currtime = Instant::now();
                     let polltime = currtime.duration_since(lasttime);
-                    trace!("Report written in {}us", polltime.as_micros());
+                    let micros = polltime.as_micros();
+                    debug!("Report written in {}us", micros);
+                    // If we're sending reports too fast, reset the ticker.
+                    // This might happen right after plug-in, or after suspend.
+                    if micros < 8200 {
+                        ticker.reset();
+                    }
                     lasttime = currtime;
                 }
                 Err(e) => warn!("Failed to send report: {:?}", e),

src/helpers.rs

@@ -1,11 +1,67 @@
-use core::ops::Deref;
+use core::ops::{Add, AddAssign, Deref, Sub, SubAssign};
 use defmt::Format;
 use packed_struct::PackedStruct;
 
 /// wrapper type because packed_struct doesn't implement float
 /// packing by default
 #[derive(Debug, Format, Clone, Default, Copy)]
-pub struct PackedFloat(f32);
+pub struct PackedFloat(pub f32);
+
+impl Add for PackedFloat {
+    type Output = Self;
+
+    fn add(self, rhs: Self) -> Self::Output {
+        Self(self.0 + rhs.0)
+    }
+}
+
+impl Add<f32> for PackedFloat {
+    type Output = Self;
+
+    fn add(self, rhs: f32) -> Self::Output {
+        Self(self.0 + rhs)
+    }
+}
+
+impl Sub for PackedFloat {
+    type Output = Self;
+
+    fn sub(self, rhs: Self) -> Self::Output {
+        Self(self.0 - rhs.0)
+    }
+}
+
+impl Sub<f32> for PackedFloat {
+    type Output = Self;
+
+    fn sub(self, rhs: f32) -> Self::Output {
+        Self(self.0 - rhs)
+    }
+}
+
+impl SubAssign for PackedFloat {
+    fn sub_assign(&mut self, rhs: Self) {
+        self.0 -= rhs.0;
+    }
+}
+
+impl SubAssign<f32> for PackedFloat {
+    fn sub_assign(&mut self, rhs: f32) {
+        self.0 -= rhs;
+    }
+}
+
+impl AddAssign for PackedFloat {
+    fn add_assign(&mut self, rhs: Self) {
+        self.0 += rhs.0;
+    }
+}
+
+impl AddAssign<f32> for PackedFloat {
+    fn add_assign(&mut self, rhs: f32) {
+        self.0 += rhs;
+    }
+}
 
 pub trait ToRegularArray<const T: usize> {
     fn to_regular_array(&self) -> &[f32; T];

src/input.rs

@@ -21,8 +21,8 @@ use libm::{fmaxf, fminf};
 
 use crate::{
     config::{
-        ControllerConfig, OverrideGcReportInstruction, OverrideStickState, SIGNAL_IS_CALIBRATING,
-        SIGNAL_OVERRIDE_GCC_STATE, SIGNAL_OVERRIDE_STICK_STATE,
+        ControllerConfig, OverrideGcReportInstruction, OverrideStickState, SIGNAL_CONFIG_CHANGE,
+        SIGNAL_IS_CALIBRATING, SIGNAL_OVERRIDE_GCC_STATE, SIGNAL_OVERRIDE_STICK_STATE,
     },
     filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS},
     gcc_hid::GcReport,
@@ -506,9 +506,10 @@ pub async fn update_stick_states_task(
     *SPI_ACS_SHARED.lock().await = Some(spi_acs);
     *SPI_CCS_SHARED.lock().await = Some(spi_ccs);
 
-    let controlstick_params = StickParams::from_stick_config(&controller_config.astick_config);
-    let cstick_params = StickParams::from_stick_config(&controller_config.cstick_config);
-    let filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config);
+    let mut controller_config = controller_config;
+    let mut controlstick_params = StickParams::from_stick_config(&controller_config.astick_config);
+    let mut cstick_params = StickParams::from_stick_config(&controller_config.cstick_config);
+    let mut filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config);
 
     info!("Controlstick params: {:?}", controlstick_params);
 
@@ -572,6 +573,13 @@ pub async fn update_stick_states_task(
             yield_now().await;
         }
 
+        if let Some(new_config) = SIGNAL_CONFIG_CHANGE.try_take() {
+            controller_config = new_config;
+            controlstick_params = StickParams::from_stick_config(&controller_config.astick_config);
+            cstick_params = StickParams::from_stick_config(&controller_config.cstick_config);
+            filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config);
+        }
+
         #[cfg(debug_assertions)]
         {
             // give other tasks a chance to do something

src/main.rs

@@ -95,7 +95,7 @@ fn main() -> ! {
                     controller_config.input_consistency_mode,
                 ))
                 .unwrap();
-            // spawner.spawn(input_integrity_benchmark()).unwrap();
+            spawner.spawn(input_integrity_benchmark()).unwrap();
             spawner
                 .spawn(update_button_state_task(
                     Input::new(AnyPin::from(p.PIN_20), gpio::Pull::Up),