feat(config, stick): implement more calibration logic

Cargo.toml

@@ -62,6 +62,7 @@ debug = 2
 debug-assertions = true
 incremental = false
 opt-level = 3
+lto = "fat"
 overflow-checks = true
 
 # cargo build/run --release

src/config.rs

@@ -4,7 +4,7 @@
  */
 use core::f32::consts::PI;
 
-use defmt::{info, warn, Format};
+use defmt::{error, info, warn, Format};
 use embassy_rp::{
     flash::{Async, Flash, ERASE_SIZE},
     peripherals::FLASH,
@@ -12,16 +12,38 @@ use embassy_rp::{
 use packed_struct::{derive::PackedStruct, PackedStruct};
 
 use crate::{
-    helpers::{PackedFloat, ToPackedFloatArray},
+    helpers::{PackedFloat, ToPackedFloatArray, XyValuePair},
-    stick::{NotchStatus, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES},
+    input::{read_ext_adc, Stick, StickAxis, SPI_ACS_SHARED, SPI_CCS_SHARED, SPI_SHARED},
+    stick::{NotchStatus, NO_OF_ADJ_NOTCHES, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES},
     ADDR_OFFSET, FLASH_SIZE,
 };
 
-use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pubsub::Subscriber};
+use embassy_sync::{
+    blocking_mutex::raw::{CriticalSectionRawMutex, RawMutex},
+    pubsub::Subscriber,
+    signal::Signal,
+};
 use embassy_time::Timer;
 
 use crate::{gcc_hid::GcReport, input::CHANNEL_GCC_STATE};
 
+/// Whether we are currently calibrating the sticks. Updates are dispatched when the status changes.
+/// Initial status is assumed to be false.
+pub static SIGNAL_IS_CALIBRATING: Signal<CriticalSectionRawMutex, bool> = 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();
+
+/// Struct used for overriding the GCC state for a given amount of
+/// time, useful for providing feedback to the user e.g. if we just entered
+/// a certain mode.
+#[derive(Default, Debug, Clone, Format)]
+pub struct OverrideGcReportInstruction {
+    pub report: GcReport,
+    pub duration_ms: u64,
+}
+
 const CONFIG_MODE_ENTRY_COMBO: [AwaitableButtons; 4] = [
     AwaitableButtons::Start,
     AwaitableButtons::A,
@@ -29,6 +51,20 @@ const CONFIG_MODE_ENTRY_COMBO: [AwaitableButtons; 4] = [
     AwaitableButtons::Y,
 ];
 
+const LSTICK_CALIBRATION_COMBO: [AwaitableButtons; 4] = [
+    AwaitableButtons::A,
+    AwaitableButtons::X,
+    AwaitableButtons::Y,
+    AwaitableButtons::L,
+];
+
+const RSTICK_CALIBRATION_COMBO: [AwaitableButtons; 4] = [
+    AwaitableButtons::A,
+    AwaitableButtons::X,
+    AwaitableButtons::Y,
+    AwaitableButtons::R,
+];
+
 /// This doesn't need to be super fast, since it's only used
 /// in config mode.
 const BUTTON_POLL_INTERVAL_MILLIS: u64 = 20;
@@ -96,7 +132,7 @@ const DEFAULT_CAL_POINTS_Y: [f32; NO_OF_CALIBRATION_POINTS] = [
 	0.3000802760,0.3008482317
 ];
 
-const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [
+pub const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [
     0.,
     PI / 8.0,
     PI * 2. / 8.,
@@ -150,9 +186,9 @@ pub struct StickConfig {
     #[packed_field(size_bits = "8")]
     pub y_smoothing: u8,
     #[packed_field(element_size_bytes = "4")]
-    pub temp_cal_points_x: [PackedFloat; 32],
+    pub cal_points_x: [PackedFloat; 32],
     #[packed_field(element_size_bytes = "4")]
-    pub temp_cal_points_y: [PackedFloat; 32],
+    pub cal_points_y: [PackedFloat; 32],
     #[packed_field(element_size_bytes = "4")]
     pub angles: [PackedFloat; 16],
 }
@@ -168,8 +204,8 @@ impl Default for StickConfig {
             y_smoothing: 0,
             cardinal_snapping: 0,
             analog_scaler: 100,
-            temp_cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
+            cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
-            temp_cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
+            cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
             angles: *DEFAULT_ANGLES.to_packed_float_array(),
         }
     }
@@ -255,16 +291,17 @@ trait WaitForButtonPress {
     ) -> usize;
 }
 
-impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress
+impl<'a, T: RawMutex, const I: usize, const J: usize, const K: usize> WaitForButtonPress
-    for Subscriber<'a, CriticalSectionRawMutex, GcReport, I, J, K>
+    for Subscriber<'a, T, GcReport, I, J, K>
 {
     async fn wait_for_button_press(&mut self, button_to_wait_for: &AwaitableButtons) {
         loop {
-            if match self.next_message_pure().await {
+            let report = self.next_message_pure().await;
-                report => is_awaitable_button_pressed(&report, button_to_wait_for),
+
-            } {
+            if is_awaitable_button_pressed(&report, button_to_wait_for) {
                 break;
             }
+
             Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
         }
     }
@@ -274,13 +311,15 @@ impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress
         buttons_to_wait_for: &[AwaitableButtons; N],
     ) {
         loop {
-            if match self.next_message_pure().await {
+            let report = self.next_message_pure().await;
-                report => buttons_to_wait_for
+
-                    .iter()
+            if buttons_to_wait_for
-                    .all(|button| is_awaitable_button_pressed(&report, button)),
+                .iter()
-            } {
+                .all(|button| is_awaitable_button_pressed(&report, button))
+            {
                 break;
             }
+
             Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
         }
     }
@@ -290,15 +329,14 @@ impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress
         buttons_to_wait_for: &[AwaitableButtons; N],
     ) -> AwaitableButtons {
         loop {
-            match self.next_message_pure().await {
+            let report = self.next_message_pure().await;
-                report => {
+
-                    for button in buttons_to_wait_for {
+            for button in buttons_to_wait_for {
-                        if is_awaitable_button_pressed(&report, button) {
+                if is_awaitable_button_pressed(&report, button) {
-                            return *button;
+                    return *button;
-                        }
-                    }
                 }
             }
+
             Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
         }
     }
@@ -308,18 +346,17 @@ impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress
         buttons_to_wait_for: &[[AwaitableButtons; N]; M],
     ) -> usize {
         loop {
-            match self.next_message_pure().await {
+            let report = self.next_message_pure().await;
-                report => {
+
-                    for (i, buttons) in buttons_to_wait_for.iter().enumerate() {
+            for (i, buttons) in buttons_to_wait_for.iter().enumerate() {
-                        if buttons
+                if buttons
-                            .iter()
+                    .iter()
-                            .all(|button| is_awaitable_button_pressed(&report, button))
+                    .all(|button| is_awaitable_button_pressed(&report, button))
-                        {
+                {
-                            return i;
+                    return i;
-                        }
-                    }
                 }
             }
+
             Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
         }
     }
@@ -336,18 +373,156 @@ fn is_awaitable_button_pressed(report: &GcReport, button_to_wait_for: &Awaitable
         AwaitableButtons::Left => report.buttons_1.dpad_left,
         AwaitableButtons::Right => report.buttons_1.dpad_right,
         AwaitableButtons::Start => report.buttons_2.button_start,
-        AwaitableButtons::L => report.buttons_2.button_l,
+        AwaitableButtons::L => report.buttons_2.button_l || report.trigger_l > 10,
-        AwaitableButtons::R => report.buttons_2.button_r,
+        AwaitableButtons::R => report.buttons_2.button_r || report.trigger_r > 10,
+    }
+}
+
+#[derive(Debug, Format)]
+struct StickCalibrationProcess<'a> {
+    which_stick: Stick,
+    calibration_step: u8,
+    gcc_config: &'a mut ControllerConfig,
+    cal_points: [XyValuePair<f32>; NO_OF_CALIBRATION_POINTS],
+}
+
+impl<'a> StickCalibrationProcess<'a> {
+    pub fn new(gcc_config: &'a mut ControllerConfig, which_stick: Stick) -> Self {
+        Self {
+            which_stick,
+            calibration_step: 0,
+            gcc_config,
+            cal_points: [XyValuePair::default(); NO_OF_CALIBRATION_POINTS],
+        }
+    }
+
+    async fn calibration_advance(&mut self) {
+        let mut spi_unlocked = SPI_SHARED.lock().await;
+        let mut spi_acs_unlocked = SPI_ACS_SHARED.lock().await;
+        let mut spi_ccs_unlocked = SPI_CCS_SHARED.lock().await;
+
+        let spi = spi_unlocked.as_mut().unwrap();
+        let spi_acs = spi_acs_unlocked.as_mut().unwrap();
+        let spi_ccs = spi_ccs_unlocked.as_mut().unwrap();
+
+        if self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 {
+            let mut x: f32 = 0.;
+            let mut y: f32 = 0.;
+
+            for _ in 0..128 {
+                x += read_ext_adc(self.which_stick, StickAxis::XAxis, spi, spi_acs, spi_ccs) as f32
+                    / 4096.0;
+                y += read_ext_adc(self.which_stick, StickAxis::YAxis, spi, spi_acs, spi_ccs) as f32
+                    / 4096.0;
+            }
+
+            x /= 128.;
+            y /= 128.;
+
+            self.cal_points[self.calibration_step as usize] = XyValuePair { x, y };
+        }
+
+        self.calibration_step += 1;
+
+        // TODO: phob does something related to undo here
+
+        if self.calibration_step == NO_OF_CALIBRATION_POINTS as u8 {
+            // TODO
+        }
+
+        if self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8 {
+            let stick_config = match self.which_stick {
+                Stick::ControlStick => &mut self.gcc_config.astick_config,
+                Stick::CStick => &mut self.gcc_config.cstick_config,
+            };
+
+            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());
+        }
+    }
+
+    pub async fn calibrate_stick(&mut self) {
+        todo!()
+    }
+}
+
+async fn configuration_main_loop<
+    'a,
+    M: RawMutex,
+    const C: usize,
+    const S: usize,
+    const P: usize,
+>(
+    current_config: &ControllerConfig,
+    mut flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
+    gcc_subscriber: &mut Subscriber<'a, M, GcReport, C, S, P>,
+) {
+    let mut final_config = current_config.clone();
+    let config_options = [LSTICK_CALIBRATION_COMBO, RSTICK_CALIBRATION_COMBO];
+
+    'main: loop {
+        match gcc_subscriber
+            .wait_and_filter_simultaneous_button_presses(&config_options)
+            .await
+        {
+            selection => match selection {
+                1 => {
+                    StickCalibrationProcess::new(&mut final_config, Stick::ControlStick)
+                        .calibrate_stick()
+                        .await
+                }
+                2 => {
+                    StickCalibrationProcess::new(&mut final_config, Stick::CStick)
+                        .calibrate_stick()
+                        .await
+                }
+                _ => {
+                    error!("Invalid selection: {}", selection);
+                    break 'main;
+                }
+            },
+        };
+
+        final_config.write_to_flash(&mut flash).unwrap();
     }
 }
 
 #[embassy_executor::task]
-pub async fn config_task() {
+pub async fn config_task(
+    current_config: ControllerConfig,
+    mut flash: Flash<'static, FLASH, Async, FLASH_SIZE>,
+) {
     let mut gcc_subscriber = CHANNEL_GCC_STATE.subscriber().unwrap();
 
     loop {
         gcc_subscriber
             .wait_for_simultaneous_button_presses(&CONFIG_MODE_ENTRY_COMBO)
             .await;
+
+        info!("Entering config mode.");
+
+        SIGNAL_OVERRIDE_GCC_STATE.signal(OverrideGcReportInstruction {
+            report: match GcReport::default() {
+                mut a => {
+                    a.trigger_r = 255;
+                    a.trigger_l = 255;
+                    a.buttons_2.button_l = true;
+                    a.buttons_2.button_r = true;
+                    a.buttons_1.button_x = true;
+                    a.buttons_1.button_y = true;
+                    a.buttons_1.button_a = true;
+                    a.stick_x = 127;
+                    a.stick_y = 127;
+                    a.cstick_x = 127;
+                    a.cstick_y = 127;
+                    a
+                }
+            },
+            duration_ms: 1000,
+        });
+
+        configuration_main_loop(&current_config, &mut flash, &mut gcc_subscriber).await;
+
+        info!("Exiting config mode.");
     }
 }

src/helpers.rs

@@ -47,6 +47,12 @@ impl Deref for PackedFloat {
     }
 }
 
+impl From<f32> for PackedFloat {
+    fn from(f: f32) -> Self {
+        Self(f)
+    }
+}
+
 #[derive(Debug, Clone, Format, Default, Copy)]
 pub struct XyValuePair<T> {
     pub x: T,

src/input.rs

@@ -5,19 +5,22 @@ use embassy_rp::{
     gpio::{AnyPin, Input, Output, Pin},
     peripherals::{
         FLASH, PIN_10, PIN_11, PIN_16, PIN_17, PIN_18, PIN_19, PIN_20, PIN_21, PIN_22, PIN_23,
-        PIN_24, PIN_5, PIN_8, PIN_9, PWM_CH4, PWM_CH6, SPI0,
+        PIN_24, PIN_5, PIN_8, PIN_9, PWM_CH4, PWM_CH6, SPI0, SPI1,
     },
     pwm::Pwm,
-    spi::Spi,
+    spi::{Blocking, Spi},
 };
 use embassy_sync::{
-    blocking_mutex::raw::CriticalSectionRawMutex, pubsub::PubSubChannel, signal::Signal,
+    blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex, ThreadModeRawMutex},
+    mutex::Mutex,
+    pubsub::PubSubChannel,
+    signal::Signal,
 };
 use embassy_time::{Duration, Instant, Timer};
 use libm::{fmaxf, fminf};
 
 use crate::{
-    config::ControllerConfig,
+    config::{ControllerConfig, SIGNAL_IS_CALIBRATING, SIGNAL_OVERRIDE_GCC_STATE},
     filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS},
     gcc_hid::GcReport,
     helpers::XyValuePair,
@@ -30,7 +33,17 @@ pub static CHANNEL_GCC_STATE: PubSubChannel<CriticalSectionRawMutex, GcReport, 1
     PubSubChannel::new();
 
 /// Used to send the stick state from the stick task to the main input task
-static STICK_SIGNAL: Signal<CriticalSectionRawMutex, StickState> = Signal::new();
+static SIGNAL_STICK_STATE: Signal<CriticalSectionRawMutex, StickState> = Signal::new();
+
+/// Used to send the raw stick values for the calibration task
+static SIGNAL_RAW_STICK_VALUES: Signal<CriticalSectionRawMutex, RawStickValues> = Signal::new();
+
+pub static SPI_SHARED: Mutex<ThreadModeRawMutex, Option<Spi<'static, SPI0, Blocking>>> =
+    Mutex::new(None);
+pub static SPI_ACS_SHARED: Mutex<ThreadModeRawMutex, Option<Output<'static, AnyPin>>> =
+    Mutex::new(None);
+pub static SPI_CCS_SHARED: Mutex<ThreadModeRawMutex, Option<Output<'static, AnyPin>>> =
+    Mutex::new(None);
 
 const STICK_HYST_VAL: f32 = 0.3;
 const FLOAT_ORIGIN: f32 = 127.5;
@@ -61,7 +74,7 @@ struct RawStickValues {
     c_unfiltered: XyValuePair<f32>,
 }
 
-#[derive(PartialEq, Eq)]
+#[derive(PartialEq, Eq, Debug, Clone, Format, Copy)]
 pub enum Stick {
     ControlStick,
     CStick,
@@ -74,7 +87,13 @@ pub enum StickAxis {
 }
 
 #[link_section = ".time_critical.read_ext_adc"]
-fn read_ext_adc<'a, Acs: Pin, Ccs: Pin, I: embassy_rp::spi::Instance, M: embassy_rp::spi::Mode>(
+pub fn read_ext_adc<
+    'a,
+    Acs: Pin,
+    Ccs: Pin,
+    I: embassy_rp::spi::Instance,
+    M: embassy_rp::spi::Mode,
+>(
     which_stick: Stick,
     which_axis: StickAxis,
     spi: &mut Spi<'a, I, M>,
@@ -110,16 +129,7 @@ fn read_ext_adc<'a, Acs: Pin, Ccs: Pin, I: embassy_rp::spi::Instance, M: embassy
 /// Gets the average stick state over a 1ms interval in a non-blocking fashion.
 /// Will wait until end_time is reached before continuing after reading the ADCs.
 #[link_section = ".time_critical.update_stick_states"]
-async fn update_stick_states<
+async fn update_stick_states(
-    'a,
-    Acs: Pin,
-    Ccs: Pin,
-    I: embassy_rp::spi::Instance,
-    M: embassy_rp::spi::Mode,
->(
-    mut spi: &mut Spi<'a, I, M>,
-    mut spi_acs: &mut Output<'a, Acs>,
-    mut spi_ccs: &mut Output<'a, Ccs>,
     current_stick_state: &StickState,
     controlstick_params: &StickParams,
     cstick_params: &StickParams,
@@ -130,6 +140,7 @@ async fn update_stick_states<
     old_stick_pos: &mut StickPositions,
     raw_stick_values: &mut RawStickValues,
     kalman_state: &mut KalmanState,
+    is_calibrating: bool,
 ) -> StickState {
     let mut adc_count = 0u32;
     let mut ax_sum = 0u32;
@@ -139,39 +150,23 @@ async fn update_stick_states<
 
     let end_time = Instant::now() + Duration::from_micros(300); // this seems kinda magic, and it is, but
 
+    let mut spi_unlocked = SPI_SHARED.lock().await;
+    let mut spi_acs_unlocked = SPI_ACS_SHARED.lock().await;
+    let mut spi_ccs_unlocked = SPI_CCS_SHARED.lock().await;
+
+    let spi = spi_unlocked.as_mut().unwrap();
+    let spi_acs = spi_acs_unlocked.as_mut().unwrap();
+    let spi_ccs = spi_ccs_unlocked.as_mut().unwrap();
+
     // "do-while at home"
     while {
         let loop_start = Instant::now();
 
         adc_count += 1;
-        ax_sum += read_ext_adc(
+        ax_sum += read_ext_adc(Stick::ControlStick, StickAxis::XAxis, spi, spi_acs, spi_ccs) as u32;
-            Stick::ControlStick,
+        ay_sum += read_ext_adc(Stick::ControlStick, StickAxis::YAxis, spi, spi_acs, spi_ccs) as u32;
-            StickAxis::XAxis,
+        cx_sum += read_ext_adc(Stick::CStick, StickAxis::XAxis, spi, spi_acs, spi_ccs) as u32;
-            &mut spi,
+        cy_sum += read_ext_adc(Stick::CStick, StickAxis::YAxis, spi, spi_acs, spi_ccs) as u32;
-            &mut spi_acs,
-            &mut spi_ccs,
-        ) as u32;
-        ay_sum += read_ext_adc(
-            Stick::ControlStick,
-            StickAxis::YAxis,
-            &mut spi,
-            &mut spi_acs,
-            &mut spi_ccs,
-        ) as u32;
-        cx_sum += read_ext_adc(
-            Stick::CStick,
-            StickAxis::XAxis,
-            &mut spi,
-            &mut spi_acs,
-            &mut spi_ccs,
-        ) as u32;
-        cy_sum += read_ext_adc(
-            Stick::CStick,
-            StickAxis::YAxis,
-            &mut spi,
-            &mut spi_acs,
-            &mut spi_ccs,
-        ) as u32;
 
         let loop_end = Instant::now();
         loop_end < end_time - (loop_end - loop_start)
@@ -265,7 +260,7 @@ async fn update_stick_states<
         controlstick_params,
         controller_config,
         Stick::ControlStick,
-        false,
+        is_calibrating,
     ) {
         (x, y) => XyValuePair { x, y },
     };
@@ -275,7 +270,7 @@ async fn update_stick_states<
         cstick_params,
         controller_config,
         Stick::CStick,
-        false,
+        is_calibrating,
     ) {
         (x, y) => XyValuePair { x, y },
     };
@@ -285,7 +280,7 @@ async fn update_stick_states<
         controlstick_params,
         controller_config,
         Stick::ControlStick,
-        false,
+        is_calibrating,
     ) {
         (x, y) => XyValuePair { x, y },
     };
@@ -295,7 +290,7 @@ async fn update_stick_states<
         cstick_params,
         controller_config,
         Stick::CStick,
-        false,
+        is_calibrating,
     ) {
         (x, y) => XyValuePair { x, y },
     };
@@ -440,16 +435,19 @@ pub async fn update_button_state_task(
         );
 
         // not every loop pass is going to update the stick state
-        match STICK_SIGNAL.try_take() {
+        if let Some(stick_state) = SIGNAL_STICK_STATE.try_take() {
-            Some(stick_state) => {
+            gcc_state.stick_x = stick_state.ax;
-                gcc_state.stick_x = stick_state.ax;
+            gcc_state.stick_y = stick_state.ay;
-                gcc_state.stick_y = stick_state.ay;
+            gcc_state.cstick_x = stick_state.cx;
-                gcc_state.cstick_x = stick_state.cx;
+            gcc_state.cstick_y = stick_state.cy;
-                gcc_state.cstick_y = stick_state.cy;
-            }
-            None => (),
         }
 
+        // 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);
 
         // give other tasks a chance to do something
@@ -459,6 +457,8 @@ pub async fn update_button_state_task(
 
 /// Task responsible for updating the stick states.
 /// Publishes the result to STICK_SIGNAL.
+///
+/// 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>,
@@ -490,13 +490,12 @@ pub async fn update_stick_states_task(
     // the time at which the current loop iteration should end
     let mut end_time = Instant::now() + Duration::from_micros(1000);
 
+    let mut is_calibrating = false;
+
     loop {
         let timer = Timer::at(end_time);
 
         current_stick_state = update_stick_states(
-            &mut spi,
-            &mut spi_acs,
-            &mut spi_ccs,
             &current_stick_state,
             &controlstick_params,
             &cstick_params,
@@ -507,22 +506,27 @@ pub async fn update_stick_states_task(
             &mut old_stick_pos,
             &mut raw_stick_values,
             &mut kalman_state,
+            is_calibrating,
         )
         .await;
 
         timer.await;
         end_time += Duration::from_micros(1000);
 
+        if let Some(new_calibrating) = SIGNAL_IS_CALIBRATING.try_take() {
+            is_calibrating = new_calibrating;
+        }
+
         match Instant::now() {
             n => {
                 match (n - last_loop_time).as_micros() {
-                    a if a > 1100 => debug!("Loop took {} us", a),
+                    a if a > 1999 => debug!("Loop took {} us", a),
                     _ => {}
                 };
                 last_loop_time = n;
             }
         };
 
-        STICK_SIGNAL.signal(current_stick_state.clone());
+        SIGNAL_STICK_STATE.signal(current_stick_state.clone());
     }
 }

src/main.rs

@@ -118,7 +118,14 @@ fn main() -> ! {
     // pwm_brake.set_counter(255);
 
     executor0.run(|spawner| {
-        spawner.spawn(config_task()).unwrap();
+        // Config task has to run on core0 because it reads and writes to flash.
+        spawner
+            .spawn(config_task(controller_config.clone(), flash))
+            .unwrap();
+
+        // Stick loop has to run on core0 because it makes use of SPI0.
+        // Perhaps in the future we can rewire the board to have it make use of SPI1 instead.
+        // This way it could be the sole task running on core1, and everything else could happen on core0.
         spawner
             .spawn(update_stick_states_task(
                 spi,

src/stick.rs

@@ -6,7 +6,7 @@ use defmt::{debug, Format};
 use libm::{atan2f, cosf, fabs, fabsf, roundf, sinf, sqrtf};
 
 use crate::{
-    config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS},
+    config::{ControllerConfig, StickConfig, DEFAULT_ANGLES, DEFAULT_NOTCH_STATUS},
     helpers::{ToRegularArray, XyValuePair},
     input::Stick,
 };
@@ -15,7 +15,7 @@ use crate::{
 const FIT_ORDER: usize = 3;
 const NUM_COEFFS: usize = FIT_ORDER + 1;
 pub const NO_OF_NOTCHES: usize = 16;
-const NO_OF_ADJ_NOTCHES: usize = 12;
+pub const NO_OF_ADJ_NOTCHES: usize = 12;
 pub const NO_OF_CALIBRATION_POINTS: usize = 32;
 const MAX_ORDER: usize = 20;
 
@@ -45,9 +45,9 @@ impl StickParams {
     /// Generate StickParams structs for the sticks, returned as a tuple of (analog_stick, c_stick)
     pub fn from_stick_config(stick_config: &StickConfig) -> Self {
         let cleaned_cal_points = CleanedCalibrationPoints::from_temp_calibration_points(
-            &stick_config.temp_cal_points_x.to_regular_array(),
+            stick_config.cal_points_x.to_regular_array(),
-            &stick_config.temp_cal_points_y.to_regular_array(),
+            stick_config.cal_points_y.to_regular_array(),
-            &stick_config.angles.to_regular_array(),
+            stick_config.angles.to_regular_array(),
         );
 
         let linearized_cal = LinearizedCalibration::from_calibration_points(&cleaned_cal_points);
@@ -68,7 +68,7 @@ impl StickParams {
     }
 }
 
-#[derive(Clone, Debug, Format, Copy)]
+#[derive(Clone, Debug, Format, Copy, Eq, PartialEq)]
 pub enum NotchStatus {
     TertInactive,
     TertActive,
@@ -387,6 +387,190 @@ impl NotchCalibration {
     }
 }
 
+struct AppliedCalibration {
+    stick_params: StickParams,
+    cleaned_calibration: CleanedCalibrationPoints,
+    notch_angles: [f32; NO_OF_NOTCHES],
+    measured_notch_angles: [f32; NO_OF_NOTCHES],
+}
+
+impl AppliedCalibration {
+    pub fn from_points(
+        cal_points_x: &[f32; NO_OF_CALIBRATION_POINTS],
+        cal_points_y: &[f32; NO_OF_CALIBRATION_POINTS],
+        stick_config: &StickConfig,
+        which_stick: Stick,
+    ) -> Self {
+        let mut stick_params = StickParams::from_stick_config(stick_config);
+
+        let angles = stick_config.angles;
+
+        let (stripped_cal_points_x, stripped_cal_points_y) =
+            strip_cal_points(cal_points_x, cal_points_y);
+
+        let stripped_cleaned_calibration = CleanedCalibrationPoints::from_temp_calibration_points(
+            &stripped_cal_points_x,
+            &stripped_cal_points_y,
+            &DEFAULT_ANGLES,
+        );
+
+        let linearized_calibration =
+            LinearizedCalibration::from_calibration_points(&stripped_cleaned_calibration);
+
+        stick_params.fit_coeffs = XyValuePair {
+            x: linearized_calibration.fit_coeffs.x.map(|e| e as f32),
+            y: linearized_calibration.fit_coeffs.y.map(|e| e as f32),
+        };
+
+        let notch_calibration = NotchCalibration::from_cleaned_and_linearized_calibration(
+            &stripped_cleaned_calibration,
+            &linearized_calibration,
+        );
+
+        stick_params.affine_coeffs = notch_calibration.affine_coeffs;
+        stick_params.boundary_angles = notch_calibration.boundary_angles;
+
+        let original_cleaned_calibration = CleanedCalibrationPoints::from_temp_calibration_points(
+            cal_points_x,
+            cal_points_y,
+            &DEFAULT_ANGLES,
+        );
+
+        let (transformed_cal_points_x, transformed_cal_points_y) = transform_cal_points(
+            &original_cleaned_calibration.cleaned_points.x,
+            &original_cleaned_calibration.cleaned_points.y,
+            &stick_params,
+            stick_config,
+        );
+
+        let measured_notch_angles =
+            compute_stick_angles(&transformed_cal_points_x, &transformed_cal_points_y);
+
+        let cleaned_with_measured_notch_angles =
+            CleanedCalibrationPoints::from_temp_calibration_points(
+                cal_points_x,
+                cal_points_y,
+                &measured_notch_angles,
+            );
+
+        let cleaned_notch_angles = clean_notches(
+            &measured_notch_angles,
+            &cleaned_with_measured_notch_angles.notch_status,
+        );
+
+        let cleaned_full = CleanedCalibrationPoints::from_temp_calibration_points(
+            cal_points_x,
+            cal_points_y,
+            &cleaned_notch_angles,
+        );
+
+        let linearized_full = LinearizedCalibration::from_calibration_points(&cleaned_full);
+
+        stick_params.fit_coeffs = XyValuePair {
+            x: linearized_full.fit_coeffs.x.map(|e| e as f32),
+            y: linearized_full.fit_coeffs.y.map(|e| e as f32),
+        };
+
+        let notch_calibrate_full = NotchCalibration::from_cleaned_and_linearized_calibration(
+            &cleaned_full,
+            &linearized_full,
+        );
+
+        stick_params.affine_coeffs = notch_calibrate_full.affine_coeffs;
+        stick_params.boundary_angles = notch_calibrate_full.boundary_angles;
+
+        Self {
+            stick_params,
+            measured_notch_angles,
+            notch_angles: cleaned_notch_angles,
+            cleaned_calibration: cleaned_full,
+        }
+    }
+}
+
+/// Sets notches to measured values if absent.
+fn clean_notches(
+    measured_notch_angles: &[f32; NO_OF_NOTCHES],
+    notch_status: &[NotchStatus; NO_OF_NOTCHES],
+) -> [f32; NO_OF_NOTCHES] {
+    let mut out = [0f32; NO_OF_NOTCHES];
+
+    for i in 0..NO_OF_NOTCHES {
+        if notch_status[i] == NotchStatus::TertInactive {
+            out[i] = measured_notch_angles[i];
+        }
+    }
+
+    out
+}
+
+fn angle_on_sphere(x: f32, y: f32) -> f32 {
+    let xx = sinf(x * MAX_STICK_ANGLE / 100.) * cosf(y * MAX_STICK_ANGLE / 100.);
+    let yy = cosf(x * MAX_STICK_ANGLE / 100.) * sinf(y * MAX_STICK_ANGLE / 100.);
+    match atan2f(yy, xx) {
+        a if a < 0. => a + 2. * PI,
+        a => a,
+    }
+}
+
+fn compute_stick_angles(
+    x_in: &[f32; NO_OF_NOTCHES + 1],
+    y_in: &[f32; NO_OF_NOTCHES + 1],
+) -> [f32; NO_OF_NOTCHES] {
+    let mut angles = [0f32; NO_OF_NOTCHES];
+
+    for i in 0..NO_OF_NOTCHES {
+        if i % 2 == 0 {
+            angles[i] = DEFAULT_ANGLES[i];
+        } else {
+            angles[i] = angle_on_sphere(x_in[i], y_in[i]);
+        }
+    }
+
+    angles
+}
+
+fn transform_cal_points(
+    cal_points_x: &[f32; NO_OF_NOTCHES + 1],
+    cal_points_y: &[f32; NO_OF_NOTCHES + 1],
+    stick_params: &StickParams,
+    stick_config: &StickConfig,
+) -> ([f32; NO_OF_NOTCHES + 1], [f32; NO_OF_NOTCHES + 1]) {
+    let mut transformed_points_x = [0f32; NO_OF_NOTCHES + 1];
+    let mut transformed_points_y = [0f32; NO_OF_NOTCHES + 1];
+
+    for i in 0..NO_OF_NOTCHES + 1 {
+        let x = linearize(cal_points_x[i], &stick_params.fit_coeffs.x);
+        let y = linearize(cal_points_y[i], &stick_params.fit_coeffs.y);
+        let (out_x, out_y) = notch_remap(x, y, stick_params, stick_config, true);
+        transformed_points_x[i] = out_x;
+        transformed_points_y[i] = out_y;
+    }
+
+    (transformed_points_x, transformed_points_y)
+}
+/// Removes the notches from un-cleaned cal points
+/// so we can get the original values of the notches after the affine transform.
+fn strip_cal_points(
+    cal_points_x: &[f32; NO_OF_CALIBRATION_POINTS],
+    cal_points_y: &[f32; NO_OF_CALIBRATION_POINTS],
+) -> (
+    [f32; NO_OF_CALIBRATION_POINTS],
+    [f32; NO_OF_CALIBRATION_POINTS],
+) {
+    let mut stripped_points_x = [0f32; NO_OF_CALIBRATION_POINTS];
+    let mut stripped_points_y = [0f32; NO_OF_CALIBRATION_POINTS];
+    for i in 0..NO_OF_CALIBRATION_POINTS {
+        (stripped_points_x[i], stripped_points_y[i]) = if (i + 1) % 4 == 0 {
+            (cal_points_x[0], cal_points_y[0])
+        } else {
+            (cal_points_x[i], cal_points_y[i])
+        }
+    }
+
+    (stripped_points_x, stripped_points_y)
+}
+
 fn inverse(in_mat: &[[f32; 3]; 3]) -> [[f32; 3]; 3] {
     let mut out_mat = [[0f32; 3]; 3];
 
@@ -561,7 +745,7 @@ fn fit_curve<const N: usize, const NCOEFFS: usize>(
 
 /// Compute the stick x/y coordinates from a given angle.
 /// The stick moves spherically, so it requires 3D trigonometry.
-fn calc_stick_values(angle: f32) -> (f32, f32) {
+pub fn calc_stick_values(angle: f32) -> (f32, f32) {
     let x =
         100. * atan2f(sinf(MAX_STICK_ANGLE) * cosf(angle), cosf(MAX_STICK_ANGLE)) / MAX_STICK_ANGLE;
     let y =
@@ -571,7 +755,7 @@ fn calc_stick_values(angle: f32) -> (f32, f32) {
 }
 
 #[link_section = ".time_critical.linearize"]
-pub fn linearize(point: f32, coefficients: &[f32; 4]) -> f32 {
+pub fn linearize(point: f32, coefficients: &[f32; NUM_COEFFS]) -> f32 {
     coefficients[0] * (point * point * point)
         + coefficients[1] * (point * point)
         + coefficients[2] * point
@@ -583,8 +767,7 @@ pub fn notch_remap(
     x_in: f32,
     y_in: f32,
     stick_params: &StickParams,
-    controller_config: &ControllerConfig,
+    stick_config: &StickConfig,
-    which_stick: Stick,
     is_calibrating: bool,
 ) -> (f32, f32) {
     //determine the angle between the x unit vector and the current position vector
@@ -606,10 +789,7 @@ pub fn notch_remap(
         NO_OF_NOTCHES - 1
     };
 
-    let stick_scale = match which_stick {
+    let stick_scale = stick_config.analog_scaler as f32 / 100.;
-        Stick::ControlStick => controller_config.astick_config.analog_scaler as f32 / 100.,
-        Stick::CStick => controller_config.cstick_config.analog_scaler as f32 / 100.,
-    };
 
     let mut x_out = stick_scale
         * (stick_params.affine_coeffs[region][0] * x_in
@@ -619,11 +799,6 @@ pub fn notch_remap(
             + stick_params.affine_coeffs[region][3] * y_in);
 
     if !is_calibrating {
-        let stick_config = match which_stick {
-            Stick::ControlStick => &controller_config.astick_config,
-            Stick::CStick => &controller_config.cstick_config,
-        };
-
         if stick_config.cardinal_snapping > 0 {
             if fabsf(x_out) < stick_config.cardinal_snapping as f32 + 0.5 && fabsf(y_out) >= 79.5 {
                 x_out = 0.;