feat(calibrate): finish stick calibration logic

src/config.rs

@@ -4,7 +4,7 @@
  */
 use core::{cmp::min, f32::consts::PI};
 
-use defmt::{error, info, warn, Format};
+use defmt::{debug, error, info, warn, Format};
 use embassy_rp::{
     flash::{Async, Flash, ERASE_SIZE},
     peripherals::FLASH,
@@ -13,17 +13,20 @@ use packed_struct::{derive::PackedStruct, PackedStruct};
 
 use crate::{
     helpers::{PackedFloat, ToPackedFloatArray, XyValuePair},
-    input::{read_ext_adc, Stick, StickAxis, SPI_ACS_SHARED, SPI_CCS_SHARED, SPI_SHARED},
+    input::{
+        read_ext_adc, Stick, StickAxis, StickState, FLOAT_ORIGIN, SPI_ACS_SHARED, SPI_CCS_SHARED,
+        SPI_SHARED,
+    },
     stick::{
-        legalize_notches, AppliedCalibration, NotchStatus, NOTCH_ADJUSTMENT_ORDER,
-        NO_OF_ADJ_NOTCHES, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES,
+        calc_stick_values, legalize_notches, AppliedCalibration, NotchStatus, CALIBRATION_ORDER,
+        NOTCH_ADJUSTMENT_ORDER, NO_OF_ADJ_NOTCHES, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES,
     },
     ADDR_OFFSET, FLASH_SIZE,
 };
 
 use embassy_sync::{
     blocking_mutex::raw::{CriticalSectionRawMutex, RawMutex},
-    pubsub::Subscriber,
+    pubsub::{PubSubBehavior, Subscriber},
     signal::Signal,
 };
 use embassy_time::Timer;
@@ -34,6 +37,12 @@ use crate::{gcc_hid::GcReport, input::CHANNEL_GCC_STATE};
 /// Initial status is assumed to be false.
 pub static SIGNAL_IS_CALIBRATING: Signal<CriticalSectionRawMutex, bool> = 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,
+    Option<OverrideStickState>,
+> = Signal::new();
+
 /// Dispatched when we want to override the GCC state for a short amount of time.
 pub static SIGNAL_OVERRIDE_GCC_STATE: Signal<CriticalSectionRawMutex, OverrideGcReportInstruction> =
     Signal::new();
@@ -54,19 +63,9 @@ const CONFIG_MODE_ENTRY_COMBO: [AwaitableButtons; 4] = [
     AwaitableButtons::Y,
 ];
 
-const LSTICK_CALIBRATION_COMBO: [AwaitableButtons; 4] = [
-    AwaitableButtons::A,
-    AwaitableButtons::X,
-    AwaitableButtons::Y,
-    AwaitableButtons::L,
-];
+const LSTICK_CALIBRATION_COMBO: [AwaitableButtons; 2] = [AwaitableButtons::A, AwaitableButtons::X];
 
-const RSTICK_CALIBRATION_COMBO: [AwaitableButtons; 4] = [
-    AwaitableButtons::A,
-    AwaitableButtons::X,
-    AwaitableButtons::Y,
-    AwaitableButtons::R,
-];
+const RSTICK_CALIBRATION_COMBO: [AwaitableButtons; 2] = [AwaitableButtons::A, AwaitableButtons::Y];
 
 /// This doesn't need to be super fast, since it's only used
 /// in config mode.
@@ -74,7 +73,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,
@@ -154,6 +153,13 @@ pub const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [
     PI * 15. / 8.,
 ];
 
+#[derive(Clone, Format)]
+pub struct OverrideStickState {
+    pub x: u8,
+    pub y: u8,
+    pub which_stick: Stick,
+}
+
 #[derive(Clone, Copy, Debug, Format)]
 enum AwaitableButtons {
     A,
@@ -201,11 +207,11 @@ impl Default for StickConfig {
         Self {
             x_waveshaping: 0,
             y_waveshaping: 0,
-            x_snapback: 0,
-            y_snapback: 0,
+            x_snapback: 4,
+            y_snapback: 4,
             x_smoothing: 0,
             y_smoothing: 0,
-            cardinal_snapping: 0,
+            cardinal_snapping: 6,
             analog_scaler: 100,
             cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
             cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
@@ -275,6 +281,9 @@ trait WaitForButtonPress {
     /// Wait for a single button press.
     async fn wait_for_button_press(&mut self, button_to_wait_for: &AwaitableButtons);
 
+    /// Wait for a single button release.
+    async fn wait_for_button_release(&mut self, button_to_wait_for: &AwaitableButtons);
+
     /// Wait for multiple buttons to be pressed simultaneously.
     async fn wait_for_simultaneous_button_presses<const N: usize>(
         &mut self,
@@ -309,6 +318,18 @@ impl<'a, T: RawMutex, const I: usize, const J: usize, const K: usize> WaitForBut
         }
     }
 
+    async fn wait_for_button_release(&mut self, button_to_wait_for: &AwaitableButtons) {
+        loop {
+            let report = self.next_message_pure().await;
+
+            if !is_awaitable_button_pressed(&report, button_to_wait_for) {
+                break;
+            }
+
+            Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
+        }
+    }
+
     async fn wait_for_simultaneous_button_presses<const N: usize>(
         &mut self,
         buttons_to_wait_for: &[AwaitableButtons; N],
@@ -401,7 +422,12 @@ impl<'a> StickCalibrationProcess<'a> {
         }
     }
 
-    async fn calibration_advance(&mut self) {
+    async fn calibration_advance(&mut self) -> bool {
+        info!(
+            "Running calibration advance on stick {} at step {}",
+            self.which_stick, self.calibration_step
+        );
+
         let stick_config = match self.which_stick {
             Stick::ControlStick => &mut self.gcc_config.astick_config,
             Stick::CStick => &mut self.gcc_config.cstick_config,
@@ -428,7 +454,9 @@ impl<'a> StickCalibrationProcess<'a> {
             x /= 128.;
             y /= 128.;
 
-            self.cal_points[self.calibration_step as usize] = XyValuePair { x, y };
+            let idx = CALIBRATION_ORDER[self.calibration_step as usize];
+
+            self.cal_points[idx] = XyValuePair { x, y };
         }
 
         self.calibration_step += 1;
@@ -451,41 +479,97 @@ impl<'a> StickCalibrationProcess<'a> {
             );
         }
 
-        let mut notch_idx = NOTCH_ADJUSTMENT_ORDER[min(
-            self.calibration_step - NO_OF_CALIBRATION_POINTS as u8,
-            NO_OF_ADJ_NOTCHES as u8 - 1,
-        ) as usize];
-
-        while self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8
-            && self.applied_calibration.cleaned_calibration.notch_status[notch_idx]
-                == NotchStatus::TertInactive
-            && self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8
-        {
-            stick_config.angles = *legalize_notches(
-                self.calibration_step as usize,
-                &self.applied_calibration.measured_notch_angles,
-                &stick_config.angles.map(|e| *e),
-            )
-            .to_packed_float_array();
-
-            self.calibration_step += 1;
-
-            notch_idx = NOTCH_ADJUSTMENT_ORDER[min(
+        if self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8 {
+            let mut notch_idx = NOTCH_ADJUSTMENT_ORDER[min(
                 self.calibration_step - NO_OF_CALIBRATION_POINTS as u8,
                 NO_OF_ADJ_NOTCHES as u8 - 1,
             ) as usize];
+
+            while self.applied_calibration.cleaned_calibration.notch_status[notch_idx]
+                == NotchStatus::TertInactive
+                && self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8
+            {
+                stick_config.angles = *legalize_notches(
+                    self.calibration_step as usize,
+                    &self.applied_calibration.measured_notch_angles,
+                    &stick_config.angles.map(|e| *e),
+                )
+                .to_packed_float_array();
+
+                self.calibration_step += 1;
+
+                notch_idx = NOTCH_ADJUSTMENT_ORDER[min(
+                    self.calibration_step - NO_OF_CALIBRATION_POINTS as u8,
+                    NO_OF_ADJ_NOTCHES as u8 - 1,
+                ) as usize];
+            }
         }
 
         if self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8 {
             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());
+
+            info!("Finished calibrating stick {}", self.which_stick);
+
+            return true;
         }
 
-        info!("Finished calibrating stick {}", self.which_stick);
+        return false;
     }
 
     pub async fn calibrate_stick(&mut self) {
-        todo!()
+        info!("Beginning stick calibration for {}", self.which_stick);
+
+        let mut gcc_subscriber = CHANNEL_GCC_STATE.subscriber().unwrap();
+        SIGNAL_IS_CALIBRATING.signal(true);
+
+        while {
+            if self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 {
+                let (x, y) = get_stick_display_coords(self.calibration_step as usize);
+                debug!(
+                    "Raw display coords for step {}: {}, {}",
+                    self.calibration_step, x, y
+                );
+                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,
+                    },
+                }));
+            } else {
+                // TODO: phob calls `adjustNotch` here
+            }
+
+            gcc_subscriber
+                .wait_for_button_release(&AwaitableButtons::A)
+                .await;
+
+            // Prevent accidental double presses
+            Timer::after_millis(20).await;
+
+            gcc_subscriber
+                .wait_for_button_press(&AwaitableButtons::A)
+                .await;
+
+            !self.calibration_advance().await
+        } {}
+
+        SIGNAL_IS_CALIBRATING.signal(false);
+        SIGNAL_OVERRIDE_STICK_STATE.signal(None);
+    }
+}
+
+fn get_stick_display_coords(current_step: usize) -> (f32, f32) {
+    let idx = CALIBRATION_ORDER[current_step];
+    if idx % 2 != 0 {
+        let notch_idx = idx / 2;
+        match calc_stick_values(DEFAULT_ANGLES[notch_idx]) {
+            (x, y) => (x + FLOAT_ORIGIN, y + FLOAT_ORIGIN),
+        }
+    } else {
+        (127.5, 127.5)
     }
 }
 
@@ -509,25 +593,29 @@ async fn configuration_main_loop<
             .await
         {
             selection => match selection {
-                1 => {
+                0 => {
                     StickCalibrationProcess::new(&mut final_config, Stick::ControlStick)
                         .calibrate_stick()
-                        .await
+                        .await;
                 }
-                2 => {
+                1 => {
                     StickCalibrationProcess::new(&mut final_config, Stick::CStick)
                         .calibrate_stick()
-                        .await
+                        .await;
                 }
-                _ => {
-                    error!("Invalid selection: {}", selection);
-                    break 'main;
+                s => {
+                    error!("Invalid selection in config loop: {}", s);
+                    continue;
                 }
             },
         };
 
         final_config.write_to_flash(&mut flash).unwrap();
+
+        break 'main;
     }
+
+    info!("Exiting config main loop.");
 }
 
 #[embassy_executor::task]
@@ -564,6 +652,9 @@ pub async fn config_task(
             duration_ms: 1000,
         });
 
+        // Wait for the user to release the buttons
+        Timer::after_millis(500).await;
+
         configuration_main_loop(&current_config, &mut flash, &mut gcc_subscriber).await;
 
         info!("Exiting config mode.");

src/input.rs

@@ -20,7 +20,10 @@ use embassy_time::{Duration, Instant, Timer};
 use libm::{fmaxf, fminf};
 
 use crate::{
-    config::{ControllerConfig, SIGNAL_IS_CALIBRATING, SIGNAL_OVERRIDE_GCC_STATE},
+    config::{
+        ControllerConfig, OverrideStickState, SIGNAL_IS_CALIBRATING, SIGNAL_OVERRIDE_GCC_STATE,
+        SIGNAL_OVERRIDE_STICK_STATE,
+    },
     filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS},
     gcc_hid::GcReport,
     helpers::XyValuePair,
@@ -29,7 +32,7 @@ use crate::{
 };
 
 /// Used to send the button state to the usb task and the calibration task
-pub static CHANNEL_GCC_STATE: PubSubChannel<CriticalSectionRawMutex, GcReport, 1, 2, 1> =
+pub static CHANNEL_GCC_STATE: PubSubChannel<CriticalSectionRawMutex, GcReport, 1, 3, 1> =
     PubSubChannel::new();
 
 /// Used to send the stick state from the stick task to the main input task
@@ -46,14 +49,14 @@ pub static SPI_CCS_SHARED: Mutex<ThreadModeRawMutex, Option<Output<'static, AnyP
     Mutex::new(None);
 
 const STICK_HYST_VAL: f32 = 0.3;
-const FLOAT_ORIGIN: f32 = 127.5;
+pub const FLOAT_ORIGIN: f32 = 127.5;
 
-#[derive(Clone, Debug, Default)]
-struct StickState {
-    ax: u8,
-    ay: u8,
-    cx: u8,
-    cy: u8,
+#[derive(Clone, Debug, Default, Format)]
+pub struct StickState {
+    pub ax: u8,
+    pub ay: u8,
+    pub cx: u8,
+    pub cy: u8,
 }
 
 #[derive(Clone, Debug, Default)]
@@ -413,6 +416,8 @@ pub async fn update_button_state_task(
 
     let gcc_publisher = CHANNEL_GCC_STATE.publisher().unwrap();
 
+    let mut override_stick_state: Option<OverrideStickState> = None;
+
     loop {
         update_button_states(
             &mut gcc_state,
@@ -438,13 +443,33 @@ pub async fn update_button_state_task(
             gcc_state.cstick_y = stick_state.cy;
         }
 
+        if let Some(override_stick_state_opt) = SIGNAL_OVERRIDE_STICK_STATE.try_take() {
+            debug!("Overridden stick state: {:?}", override_stick_state_opt);
+            override_stick_state = override_stick_state_opt;
+        }
+
         // check for a gcc state override (usually coming from the config task)
         if let Some(override_gcc_state) = SIGNAL_OVERRIDE_GCC_STATE.try_take() {
             gcc_publisher.publish_immediate(override_gcc_state.report);
             Timer::after_millis(override_gcc_state.duration_ms).await;
         };
 
-        gcc_publisher.publish_immediate(gcc_state);
+        if let Some(override_state) = &override_stick_state {
+            let mut overriden_gcc_state = gcc_state.clone();
+            match override_state.which_stick {
+                Stick::ControlStick => {
+                    overriden_gcc_state.stick_x = override_state.x;
+                    overriden_gcc_state.stick_y = override_state.y;
+                }
+                Stick::CStick => {
+                    overriden_gcc_state.cstick_x = override_state.x;
+                    overriden_gcc_state.cstick_y = override_state.y;
+                }
+            }
+            gcc_publisher.publish_immediate(overriden_gcc_state);
+        } else {
+            gcc_publisher.publish_immediate(gcc_state);
+        }
 
         // give other tasks a chance to do something
         yield_now().await;
@@ -457,14 +482,17 @@ pub async fn update_button_state_task(
 /// Has to run on core0 because it makes use of SPI0.
 #[embassy_executor::task]
 pub async fn update_stick_states_task(
-    mut spi: Spi<'static, SPI0, embassy_rp::spi::Blocking>,
-    mut spi_acs: Output<'static, AnyPin>,
-    mut spi_ccs: Output<'static, AnyPin>,
+    spi: Spi<'static, SPI0, embassy_rp::spi::Blocking>,
+    spi_acs: Output<'static, AnyPin>,
+    spi_ccs: Output<'static, AnyPin>,
     controller_config: ControllerConfig,
 ) {
+    *SPI_SHARED.lock().await = Some(spi);
+    *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 current_stick_state = StickState {

src/stick.rs

@@ -23,9 +23,9 @@ const MAX_ORDER: usize = 20;
 const MAX_STICK_ANGLE: f32 = 0.4886921906;
 
 #[rustfmt::skip]
-//                                                             right        notch 1      up right     notch 2      up           notch 3      up left      notch 4      left         notch 5      down left    notch 6      down         notch 7      down right   notch 8
-//                                                             0            1            2            3            4            5            6            7            8            9            10           11           12           13           14           15
-const CALIBRATION_ORDER: [usize; NO_OF_CALIBRATION_POINTS] = [ 0, 1,        8, 9,       16, 17,       24, 25,      4, 5,        12, 13,      20, 21,      28, 29,      2, 3,        6, 7,        10, 11,      14, 15,      18, 19,      22, 23,      26, 27,      30, 31 ];
+//                                                                right        notch 1      up right     notch 2      up           notch 3      up left      notch 4      left         notch 5      down left    notch 6      down         notch 7      down right   notch 8
+//                                                                0            1            2            3            4            5            6            7            8            9            10           11           12           13           14           15
+pub const CALIBRATION_ORDER: [usize; NO_OF_CALIBRATION_POINTS] = [ 0, 1,        8, 9,       16, 17,       24, 25,      4, 5,        12, 13,      20, 21,      28, 29,      2, 3,        6, 7,        10, 11,      14, 15,      18, 19,      22, 23,      26, 27,      30, 31 ];
 
 #[rustfmt::skip]
 //                                                              up right     up left      down left    down right   notch 1      notch 2      notch 3      notch 4      notch 5      notch 6      notch 7      notch 8
@@ -498,29 +498,29 @@ pub fn legalize_notches(
 
     for i in current_step..44 {
         let idx = NOTCH_ADJUSTMENT_ORDER[i - NO_OF_CALIBRATION_POINTS];
-        out[idx] = legalize_notch(idx, measured_notch_angles, &out);
+        out[idx] = legalize_notch(idx as isize, measured_notch_angles, &out);
     }
 
     out
 }
 
 fn legalize_notch(
-    idx: usize,
+    idx: isize,
     measured_notch_angles: &[f32; NO_OF_NOTCHES],
     notch_angles: &[f32; NO_OF_NOTCHES],
 ) -> f32 {
     let is_diagonal = (idx - 2) % 4 == 0;
 
     let prev_idx = if is_diagonal {
-        (idx - 2 + NO_OF_NOTCHES) % NO_OF_NOTCHES
+        (idx - 2 + NO_OF_NOTCHES as isize) % NO_OF_NOTCHES as isize
     } else {
-        (idx - 1 + NO_OF_NOTCHES) % NO_OF_NOTCHES
-    };
+        (idx - 1 + NO_OF_NOTCHES as isize) % NO_OF_NOTCHES as isize
+    } as usize;
     let next_idx = if is_diagonal {
-        (idx + 2) % NO_OF_NOTCHES
+        (idx + 2) % NO_OF_NOTCHES as isize
     } else {
-        (idx + 1) % NO_OF_NOTCHES
-    };
+        (idx + 1) % NO_OF_NOTCHES as isize
+    } as usize;
 
     let prev_angle = notch_angles[prev_idx];
     let next_angle = match notch_angles[next_idx] {
@@ -529,7 +529,7 @@ fn legalize_notch(
     };
 
     let prev_meas_angle = measured_notch_angles[prev_idx];
-    let this_meas_angle = measured_notch_angles[idx];
+    let this_meas_angle = measured_notch_angles[idx as usize];
     let next_meas_angle = match measured_notch_angles[next_idx] {
         a if a < prev_meas_angle => a + 2. * PI,
         a => a,
@@ -575,7 +575,7 @@ fn legalize_notch(
 
     fminf(
         upper_distort_limit,
-        fmaxf(notch_angles[idx], lower_distort_limit),
+        fmaxf(notch_angles[idx as usize], lower_distort_limit),
     )
 }
 
@@ -853,6 +853,7 @@ pub fn linearize(point: f32, coefficients: &[f32; NUM_COEFFS]) -> f32 {
         + coefficients[3]
 }
 
+// TODO: currently broken!
 #[link_section = ".time_critical.notch_remap"]
 pub fn notch_remap(
     x_in: f32,