feat(calibration): implement calibration step

src/config.rs

@@ -2,7 +2,7 @@
  *  Storage for controller configuration, including helper functions & types, as well as sane defaults.
  *  Also includes necessary logic for configuring the controller & calibrating the sticks.
  */
-use core::f32::consts::PI;
+use core::{cmp::min, f32::consts::PI};
 
 use defmt::{error, info, warn, Format};
 use embassy_rp::{
@@ -14,7 +14,10 @@ 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},
-    stick::{NotchStatus, NO_OF_ADJ_NOTCHES, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES},
+    stick::{
+        legalize_notches, AppliedCalibration, NotchStatus, NOTCH_ADJUSTMENT_ORDER,
+        NO_OF_ADJ_NOTCHES, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES,
+    },
     ADDR_OFFSET, FLASH_SIZE,
 };
 
@@ -384,6 +387,7 @@ struct StickCalibrationProcess<'a> {
     calibration_step: u8,
     gcc_config: &'a mut ControllerConfig,
     cal_points: [XyValuePair<f32>; NO_OF_CALIBRATION_POINTS],
+    applied_calibration: AppliedCalibration,
 }
 
 impl<'a> StickCalibrationProcess<'a> {
@@ -393,10 +397,15 @@ impl<'a> StickCalibrationProcess<'a> {
             calibration_step: 0,
             gcc_config,
             cal_points: [XyValuePair::default(); NO_OF_CALIBRATION_POINTS],
+            applied_calibration: AppliedCalibration::default(),
         }
     }
 
     async fn calibration_advance(&mut self) {
+        let stick_config = match self.which_stick {
+            Stick::ControlStick => &mut self.gcc_config.astick_config,
+            Stick::CStick => &mut self.gcc_config.cstick_config,
+        };
         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;
@@ -427,18 +436,52 @@ impl<'a> StickCalibrationProcess<'a> {
         // TODO: phob does something related to undo here
 
         if self.calibration_step == NO_OF_CALIBRATION_POINTS as u8 {
-            // TODO
+            stick_config.angles = *legalize_notches(
+                self.calibration_step as usize,
+                &self.applied_calibration.measured_notch_angles,
+                &self.applied_calibration.notch_angles,
+            )
+            .to_packed_float_array();
+
+            self.applied_calibration = AppliedCalibration::from_points(
+                &self.cal_points.map(|e| e.x),
+                &self.cal_points.map(|e| e.y),
+                &stick_config,
+                self.which_stick,
+            );
+        }
+
+        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(
+                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 {
-            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());
         }
+
+        info!("Finished calibrating stick {}", self.which_stick);
     }
 
     pub async fn calibrate_stick(&mut self) {

src/stick.rs

@@ -3,7 +3,7 @@
 use core::f32::consts::PI;
 
 use defmt::{debug, Format};
-use libm::{atan2f, cosf, fabs, fabsf, roundf, sinf, sqrtf};
+use libm::{atan2f, cosf, fabs, fabsf, fmaxf, fminf, roundf, sinf, sqrtf};
 
 use crate::{
     config::{ControllerConfig, StickConfig, DEFAULT_ANGLES, DEFAULT_NOTCH_STATUS},
@@ -29,7 +29,7 @@ const CALIBRATION_ORDER: [usize; NO_OF_CALIBRATION_POINTS] = [ 0, 1,        8, 9
 
 #[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
-const NOTCH_ADJUSTMENT_ORDER: [usize; NO_OF_ADJ_NOTCHES] = [2,           6,           10,          14,          1,           3,           5,           7,           9,           11,          13,          15];
+pub const NOTCH_ADJUSTMENT_ORDER: [usize; NO_OF_ADJ_NOTCHES] = [2,           6,           10,          14,          1,           3,           5,           7,           9,           11,          13,          15];
 
 #[derive(Clone, Debug, Default, Format)]
 pub struct StickParams {
@@ -76,8 +76,8 @@ pub enum NotchStatus {
     Cardinal,
 }
 
-#[derive(Clone, Debug)]
+#[derive(Clone, Debug, Format)]
-struct CleanedCalibrationPoints {
+pub struct CleanedCalibrationPoints {
     pub cleaned_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>,
     pub notch_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>,
     pub notch_status: [NotchStatus; NO_OF_NOTCHES],
@@ -387,11 +387,12 @@ impl NotchCalibration {
     }
 }
 
-struct AppliedCalibration {
+#[derive(Debug, Clone, Format, Default)]
-    stick_params: StickParams,
+pub struct AppliedCalibration {
-    cleaned_calibration: CleanedCalibrationPoints,
+    pub stick_params: StickParams,
-    notch_angles: [f32; NO_OF_NOTCHES],
+    pub cleaned_calibration: CleanedCalibrationPoints,
-    measured_notch_angles: [f32; NO_OF_NOTCHES],
+    pub notch_angles: [f32; NO_OF_NOTCHES],
+    pub measured_notch_angles: [f32; NO_OF_NOTCHES],
 }
 
 impl AppliedCalibration {
@@ -488,6 +489,96 @@ impl AppliedCalibration {
     }
 }
 
+pub fn legalize_notches(
+    current_step: usize,
+    measured_notch_angles: &[f32; NO_OF_NOTCHES],
+    notch_angles: &[f32; NO_OF_NOTCHES],
+) -> [f32; NO_OF_NOTCHES] {
+    let mut out = *notch_angles;
+
+    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
+}
+
+fn legalize_notch(
+    idx: usize,
+    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
+    } else {
+        (idx - 1 + NO_OF_NOTCHES) % NO_OF_NOTCHES
+    };
+    let next_idx = if is_diagonal {
+        (idx + 2) % NO_OF_NOTCHES
+    } else {
+        (idx + 1) % NO_OF_NOTCHES
+    };
+
+    let prev_angle = notch_angles[prev_idx];
+    let next_angle = match notch_angles[next_idx] {
+        a if a < prev_angle => a + 2. * PI,
+        a => a,
+    };
+
+    let prev_meas_angle = measured_notch_angles[prev_idx];
+    let this_meas_angle = measured_notch_angles[idx];
+    let next_meas_angle = match measured_notch_angles[next_idx] {
+        a if a < prev_meas_angle => a + 2. * PI,
+        a => a,
+    };
+
+    let (cmp_amt, str_amt) = match is_diagonal {
+        true => (0.769, 1.3),
+        false => (0.666, 1.5),
+    };
+
+    let min_threshold = 0.15 / 0.975;
+    let deadzone_limit = 0.2875 / 0.95;
+    let deadzone_plus = 0.325 / 0.9375;
+
+    let lower_compress_limit = prev_angle + cmp_amt * (this_meas_angle - prev_meas_angle);
+    let upper_compress_limit = next_angle - cmp_amt * (next_meas_angle - this_meas_angle);
+
+    let lower_strech_limit = if prev_idx % 4 == 0
+        && !is_diagonal
+        && (next_meas_angle - this_meas_angle) > min_threshold
+        && (next_meas_angle - this_meas_angle) < deadzone_limit
+    {
+        next_angle - fmaxf(str_amt * (next_meas_angle - this_meas_angle), deadzone_plus)
+    } else {
+        next_angle - str_amt * (next_meas_angle - this_meas_angle)
+    };
+
+    let upper_strech_limit = if prev_idx % 4 == 0
+        && !is_diagonal
+        && (this_meas_angle - prev_meas_angle) > min_threshold
+        && (this_meas_angle - prev_meas_angle) < deadzone_limit
+    {
+        prev_angle + fmaxf(str_amt * (this_meas_angle - prev_meas_angle), deadzone_plus)
+    } else {
+        prev_angle + str_amt * (this_meas_angle - prev_meas_angle)
+    };
+
+    let lower_distort_limit = fmaxf(lower_compress_limit, lower_strech_limit);
+    let upper_distort_limit = match fminf(upper_compress_limit, upper_strech_limit) {
+        a if a < lower_distort_limit => a + 2. * PI,
+        a => a,
+    };
+
+    fminf(
+        upper_distort_limit,
+        fmaxf(notch_angles[idx], lower_distort_limit),
+    )
+}
+
 /// Sets notches to measured values if absent.
 fn clean_notches(
     measured_notch_angles: &[f32; NO_OF_NOTCHES],