chore(project): implement more xy value pairs

src/config.rs

@@ -18,7 +18,7 @@ use crate::{
 
 /// 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,

src/input.rs

@@ -1,4 +1,4 @@
-use defmt::info;
+use defmt::{debug, info};
 use embassy_futures::{join::join, yield_now};
 use embassy_rp::{
     flash::{Async, Flash},
@@ -18,6 +18,7 @@ use crate::{
     config::ControllerConfig,
     filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS},
     gcc_hid::GcReport,
+    helpers::XyValuePair,
     stick::{linearize, notch_remap, StickParams},
     FLASH_SIZE,
 };
@@ -46,18 +47,12 @@ struct StickPositions {
 
 #[derive(Clone, Debug, Default)]
 struct RawStickValues {
-    ax_linearized: f32,
-    ay_linearized: f32,
-    cx_linearized: f32,
-    cy_linearized: f32,
-    ax_raw: f32,
-    ay_raw: f32,
-    cx_raw: f32,
-    cy_raw: f32,
-    ax_unfiltered: f32,
-    ay_unfiltered: f32,
-    cx_unfiltered: f32,
-    cy_unfiltered: f32,
+    a_linearized: XyValuePair<f32>,
+    c_linearized: XyValuePair<f32>,
+    a_raw: XyValuePair<f32>,
+    c_raw: XyValuePair<f32>,
+    a_unfiltered: XyValuePair<f32>,
+    c_unfiltered: XyValuePair<f32>,
 }
 
 #[derive(PartialEq, Eq)]
@@ -85,7 +80,7 @@ fn read_ext_adc<'a, Acs: Pin, Ccs: Pin, I: embassy_rp::spi::Instance, M: embassy
         buf = [0b11110000; 3];
     }
 
-    if which_stick == Stick::CStick {
+    if which_stick == Stick::ControlStick {
         spi_acs.set_low();
     } else {
         spi_ccs.set_low();
@@ -135,9 +130,7 @@ async fn update_stick_states<
 
     // TODO: lower interval possible?
 
-    let end_time = Instant::now() + embassy_time::Duration::from_millis(1);
-    let timer = Timer::at(end_time);
-
+    let end_time = Instant::now() + embassy_time::Duration::from_micros(500);
     let mut loop_time = Duration::from_millis(0);
 
     while Instant::now() < end_time - loop_time {
@@ -173,36 +166,38 @@ async fn update_stick_states<
             &mut spi_ccs,
         ) as u32;
 
-        loop_time = Instant::now() - loop_start;
-
         // with this, we can poll the sticks at 1000Hz (ish), while updating
         // the rest of the controller (the buttons) much faster, to ensure
         // better input integrity for button inputs.
         yield_now().await;
+        loop_time = Instant::now() - loop_start;
     }
 
-    timer.await;
+    let raw_controlstick = XyValuePair {
+        x: (ax_sum as f32) / (adc_count as f32) / 4096.0f32,
+        y: (ay_sum as f32) / (adc_count as f32) / 4096.0f32,
+    };
+    let raw_cstick = XyValuePair {
+        x: (cx_sum as f32) / (adc_count as f32) / 4096.0f32,
+        y: (cy_sum as f32) / (adc_count as f32) / 4096.0f32,
+    };
 
-    let raw_controlstick_x = (ax_sum as f32) / (adc_count as f32) / 4096.0f32;
-    let raw_controlstick_y = (ay_sum as f32) / (adc_count as f32) / 4096.0f32;
-    let raw_cstick_x = (cx_sum as f32) / (adc_count as f32) / 4096.0f32;
-    let raw_cstick_y = (cy_sum as f32) / (adc_count as f32) / 4096.0f32;
+    debug!("Raw Control Stick: {:?}", raw_controlstick);
+    debug!("Raw C Stick: {:?}", raw_cstick);
 
-    raw_stick_values.ax_raw = raw_controlstick_x;
-    raw_stick_values.ay_raw = raw_controlstick_y;
-    raw_stick_values.cx_raw = raw_cstick_x;
-    raw_stick_values.cy_raw = raw_cstick_y;
+    raw_stick_values.a_raw = raw_controlstick;
+    raw_stick_values.c_raw = raw_cstick;
 
-    let x_z = linearize(raw_controlstick_x, &controlstick_params.fit_coeffs_x);
-    let y_z = linearize(raw_controlstick_y, &controlstick_params.fit_coeffs_y);
+    let x_z = linearize(raw_controlstick.x, &controlstick_params.fit_coeffs.x);
+    let y_z = linearize(raw_controlstick.y, &controlstick_params.fit_coeffs.y);
 
-    let pos_cx = linearize(raw_cstick_x, &cstick_params.fit_coeffs_x);
-    let pos_cy = linearize(raw_cstick_y, &cstick_params.fit_coeffs_y);
+    let pos_cx = linearize(raw_cstick.x, &cstick_params.fit_coeffs.x);
+    let pos_cy = linearize(raw_cstick.y, &cstick_params.fit_coeffs.y);
 
-    raw_stick_values.ax_linearized = x_z;
-    raw_stick_values.ay_linearized = y_z;
-    raw_stick_values.cx_linearized = pos_cx;
-    raw_stick_values.cy_linearized = pos_cy;
+    raw_stick_values.a_linearized.x = x_z;
+    raw_stick_values.a_linearized.y = y_z;
+    raw_stick_values.c_linearized.x = pos_cx;
+    raw_stick_values.c_linearized.y = pos_cy;
 
     let (x_pos_filt, y_pos_filt) =
         kalman_state.run_kalman(x_z, y_z, &controller_config.astick_config, &filter_gains);
@@ -262,15 +257,15 @@ async fn update_stick_states<
         Stick::CStick,
     );
     let (remapped_x_unfiltered, remapped_y_unfiltered) = notch_remap(
-        raw_stick_values.ax_linearized,
-        raw_stick_values.ay_linearized,
+        raw_stick_values.a_linearized.x,
+        raw_stick_values.a_linearized.y,
         controlstick_params,
         controller_config,
         Stick::ControlStick,
     );
     let (remapped_cx_unfiltered, remapped_cy_unfiltered) = notch_remap(
-        raw_stick_values.cx_linearized,
-        raw_stick_values.cy_linearized,
+        raw_stick_values.c_linearized.x,
+        raw_stick_values.c_linearized.y,
         cstick_params,
         controller_config,
         Stick::CStick,
@@ -280,10 +275,10 @@ async fn update_stick_states<
     remapped_y = fminf(125., fmaxf(-125., remapped_y));
     remapped_cx = fminf(125., fmaxf(-125., remapped_cx));
     remapped_cy = fminf(125., fmaxf(-125., remapped_cy));
-    raw_stick_values.ax_unfiltered = fminf(125., fmaxf(-125., remapped_x_unfiltered));
-    raw_stick_values.ay_unfiltered = fminf(125., fmaxf(-125., remapped_y_unfiltered));
-    raw_stick_values.cx_unfiltered = fminf(125., fmaxf(-125., remapped_cx_unfiltered));
-    raw_stick_values.cy_unfiltered = fminf(125., fmaxf(-125., remapped_cy_unfiltered));
+    raw_stick_values.a_unfiltered.x = fminf(125., fmaxf(-125., remapped_x_unfiltered));
+    raw_stick_values.a_unfiltered.y = fminf(125., fmaxf(-125., remapped_y_unfiltered));
+    raw_stick_values.c_unfiltered.x = fminf(125., fmaxf(-125., remapped_cx_unfiltered));
+    raw_stick_values.c_unfiltered.y = fminf(125., fmaxf(-125., remapped_cy_unfiltered));
 
     let mut out_stick_state = current_stick_state.clone();
 
@@ -405,7 +400,11 @@ pub async fn input_loop(
         let mut controlstick_waveshaping_values = WaveshapingValues::default();
         let mut kalman_state = KalmanState::default();
 
+        let mut last_loop_time = Instant::now();
+
         loop {
+            let timer = Timer::after_millis(1);
+
             current_stick_state = update_stick_states(
                 &mut spi,
                 &mut spi_acs,
@@ -423,6 +422,15 @@ pub async fn input_loop(
             )
             .await;
 
+            timer.await;
+
+            debug!(
+                "Loop took {} us",
+                (Instant::now() - last_loop_time).as_micros()
+            );
+
+            last_loop_time = Instant::now();
+
             STICK_SIGNAL.signal(current_stick_state.clone());
         }
     };

src/stick.rs

@@ -7,7 +7,7 @@ use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf};
 
 use crate::{
     config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS},
-    helpers::ToRegularArray,
+    helpers::{ToRegularArray, XyValuePair},
     input::Stick,
 };
 
@@ -34,8 +34,7 @@ const NOTCH_ADJUSTMENT_ORDER: [usize; NO_OF_ADJ_NOTCHES] = [2,           6,
 #[derive(Clone, Debug, Default, Format)]
 pub struct StickParams {
     // these are the linearization coefficients
-    pub fit_coeffs_x: [f32; NUM_COEFFS],
-    pub fit_coeffs_y: [f32; NUM_COEFFS],
+    pub fit_coeffs: XyValuePair<[f32; NUM_COEFFS]>,
 
     // these are the notch remap parameters
     pub affine_coeffs: [[f32; 4]; 16], // affine transformation coefficients for all regions of the stick
@@ -59,8 +58,10 @@ impl StickParams {
         );
 
         Self {
-            fit_coeffs_x: linearized_cal.fit_coeffs_x.map(|e| e as f32),
-            fit_coeffs_y: linearized_cal.fit_coeffs_y.map(|e| e as f32),
+            fit_coeffs: XyValuePair {
+                x: linearized_cal.fit_coeffs.x.map(|e| e as f32),
+                y: linearized_cal.fit_coeffs.y.map(|e| e as f32),
+            },
             affine_coeffs: notch_cal.affine_coeffs,
             boundary_angles: notch_cal.boundary_angles,
         }
@@ -77,20 +78,22 @@ pub enum NotchStatus {
 
 #[derive(Clone, Debug)]
 struct CleanedCalibrationPoints {
-    pub cleaned_points_x: [f32; NO_OF_NOTCHES + 1],
-    pub cleaned_points_y: [f32; NO_OF_NOTCHES + 1],
-    pub notch_points_x: [f32; NO_OF_NOTCHES + 1],
-    pub notch_points_y: [f32; NO_OF_NOTCHES + 1],
+    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],
 }
 
 impl Default for CleanedCalibrationPoints {
     fn default() -> Self {
         Self {
-            cleaned_points_x: [0f32; NO_OF_NOTCHES + 1],
-            cleaned_points_y: [0f32; NO_OF_NOTCHES + 1],
-            notch_points_x: [0f32; NO_OF_NOTCHES + 1],
-            notch_points_y: [0f32; NO_OF_NOTCHES + 1],
+            cleaned_points: XyValuePair {
+                x: [0f32; NO_OF_NOTCHES + 1],
+                y: [0f32; NO_OF_NOTCHES + 1],
+            },
+            notch_points: XyValuePair {
+                x: [0f32; NO_OF_NOTCHES + 1],
+                y: [0f32; NO_OF_NOTCHES + 1],
+            },
             notch_status: DEFAULT_NOTCH_STATUS,
         }
     }
@@ -113,14 +116,14 @@ impl CleanedCalibrationPoints {
 
         for i in 0..NO_OF_NOTCHES {
             // add the origin values to the first x,y point
-            out.cleaned_points_x[0] += cal_points_x[i * 2];
-            out.cleaned_points_y[0] += cal_points_y[i * 2];
+            out.cleaned_points.x[0] += cal_points_x[i * 2];
+            out.cleaned_points.y[0] += cal_points_y[i * 2];
 
             // copy the cal point into the cleaned list
-            out.cleaned_points_x[i + 1] = cal_points_x[i * 2 + 1];
-            out.cleaned_points_y[i + 1] = cal_points_y[i * 2 + 1];
+            out.cleaned_points.x[i + 1] = cal_points_x[i * 2 + 1];
+            out.cleaned_points.y[i + 1] = cal_points_y[i * 2 + 1];
 
-            (out.notch_points_x[i + 1], out.notch_points_y[i + 1]) =
+            (out.notch_points.x[i + 1], out.notch_points.y[i + 1]) =
                 match calc_stick_values(notch_angles[i]) {
                     (a, b) => (roundf(a), roundf(b)),
                 };
@@ -157,22 +160,22 @@ impl CleanedCalibrationPoints {
         let largest_y = y_by_size[y_by_size.len() - 1].0;
 
         // TODO: make this whole thing a function? it looks very ugly
-        out.cleaned_points_x[0] -= cal_points_x[smallest_x];
-        out.cleaned_points_x[0] -= cal_points_x[small_x];
-        out.cleaned_points_x[0] -= cal_points_x[large_x];
-        out.cleaned_points_x[0] -= cal_points_x[largest_x];
+        out.cleaned_points.x[0] -= cal_points_x[smallest_x];
+        out.cleaned_points.x[0] -= cal_points_x[small_x];
+        out.cleaned_points.x[0] -= cal_points_x[large_x];
+        out.cleaned_points.x[0] -= cal_points_x[largest_x];
 
-        out.cleaned_points_y[0] -= cal_points_y[smallest_y];
-        out.cleaned_points_y[0] -= cal_points_y[small_y];
-        out.cleaned_points_y[0] -= cal_points_y[large_y];
-        out.cleaned_points_y[0] -= cal_points_y[largest_y];
+        out.cleaned_points.y[0] -= cal_points_y[smallest_y];
+        out.cleaned_points.y[0] -= cal_points_y[small_y];
+        out.cleaned_points.y[0] -= cal_points_y[large_y];
+        out.cleaned_points.y[0] -= cal_points_y[largest_y];
 
-        out.cleaned_points_x[0] /= (NO_OF_NOTCHES - 4) as f32;
-        out.cleaned_points_y[0] /= (NO_OF_NOTCHES - 4) as f32;
+        out.cleaned_points.x[0] /= (NO_OF_NOTCHES - 4) as f32;
+        out.cleaned_points.y[0] /= (NO_OF_NOTCHES - 4) as f32;
 
         for i in 0..NO_OF_NOTCHES {
-            let delta_x = out.cleaned_points_x[i + 1] - out.cleaned_points_x[0];
-            let delta_y = out.cleaned_points_y[i + 1] - out.cleaned_points_y[0];
+            let delta_x = out.cleaned_points.x[i + 1] - out.cleaned_points.x[0];
+            let delta_y = out.cleaned_points.y[i + 1] - out.cleaned_points.y[0];
             let mag = sqrtf(delta_x * delta_x + delta_y * delta_y);
 
             // if the cleaned point was at the center and would be a firefox notch
@@ -181,15 +184,15 @@ impl CleanedCalibrationPoints {
                 let prev_index = ((i + NO_OF_NOTCHES - 1) % NO_OF_NOTCHES) + 1;
                 let next_index = ((i + 1) % NO_OF_NOTCHES) + 1;
 
-                out.cleaned_points_x[i + 1] =
-                    (out.cleaned_points_x[prev_index] + out.cleaned_points_x[next_index]) / 2.0;
-                out.cleaned_points_y[i + 1] =
-                    (out.cleaned_points_y[prev_index] + out.cleaned_points_y[next_index]) / 2.0;
+                out.cleaned_points.x[i + 1] =
+                    (out.cleaned_points.x[prev_index] + out.cleaned_points.x[next_index]) / 2.0;
+                out.cleaned_points.y[i + 1] =
+                    (out.cleaned_points.y[prev_index] + out.cleaned_points.y[next_index]) / 2.0;
 
-                out.notch_points_x[i + 1] =
-                    (out.notch_points_x[prev_index] + out.notch_points_x[next_index]) / 2.0;
-                out.notch_points_y[i + 1] =
-                    (out.notch_points_y[prev_index] + out.notch_points_y[next_index]) / 2.0;
+                out.notch_points.x[i + 1] =
+                    (out.notch_points.x[prev_index] + out.notch_points.x[next_index]) / 2.0;
+                out.notch_points.y[i + 1] =
+                    (out.notch_points.y[prev_index] + out.notch_points.y[next_index]) / 2.0;
 
                 debug!("Skipping notch {}", i + 1);
 
@@ -204,10 +207,10 @@ impl CleanedCalibrationPoints {
         for i in 0..=NO_OF_NOTCHES {
             debug!(
                 "Cleaned: ({}, {}), Notch: ({}, {})",
-                out.cleaned_points_x[i],
-                out.cleaned_points_y[i],
-                out.notch_points_x[i],
-                out.notch_points_y[i],
+                out.cleaned_points.x[i],
+                out.cleaned_points.y[i],
+                out.notch_points.x[i],
+                out.notch_points.y[i],
             );
         }
 
@@ -219,11 +222,9 @@ impl CleanedCalibrationPoints {
 
 #[derive(Clone, Debug, Default)]
 struct LinearizedCalibration {
-    pub fit_coeffs_x: [f64; NUM_COEFFS],
-    pub fit_coeffs_y: [f64; NUM_COEFFS],
+    pub fit_coeffs: XyValuePair<[f64; NUM_COEFFS]>,
 
-    pub linearized_points_x: [f32; NO_OF_NOTCHES + 1],
-    pub linearized_points_y: [f32; NO_OF_NOTCHES + 1],
+    pub linearized_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>,
 }
 
 impl LinearizedCalibration {
@@ -240,10 +241,12 @@ impl LinearizedCalibration {
         let mut fit_points_y = [0f64; 5];
 
         let in_x = cleaned_calibration_points
-            .cleaned_points_x
+            .cleaned_points
+            .x
             .map(|e| e as f64);
         let in_y = cleaned_calibration_points
-            .cleaned_points_y
+            .cleaned_points
+            .y
             .map(|e| e as f64);
 
         fit_points_x[0] = in_x[8 + 1];
@@ -281,10 +284,14 @@ impl LinearizedCalibration {
         }
 
         Self {
-            fit_coeffs_x,
-            fit_coeffs_y,
-            linearized_points_x,
-            linearized_points_y,
+            fit_coeffs: XyValuePair {
+                x: fit_coeffs_x,
+                y: fit_coeffs_y,
+            },
+            linearized_points: XyValuePair {
+                x: linearized_points_x,
+                y: linearized_points_y,
+            },
         }
     }
 }
@@ -307,40 +314,40 @@ impl NotchCalibration {
             let mut points_out = [[0f32; 3]; 3];
 
             if i == NO_OF_NOTCHES {
-                points_in[0][0] = linearized_calibration.linearized_points_x[0];
-                points_in[0][1] = linearized_calibration.linearized_points_x[i];
-                points_in[0][2] = linearized_calibration.linearized_points_x[1];
-                points_in[1][0] = linearized_calibration.linearized_points_y[0];
-                points_in[1][1] = linearized_calibration.linearized_points_y[i];
-                points_in[1][2] = linearized_calibration.linearized_points_y[1];
+                points_in[0][0] = linearized_calibration.linearized_points.x[0];
+                points_in[0][1] = linearized_calibration.linearized_points.x[i];
+                points_in[0][2] = linearized_calibration.linearized_points.x[1];
+                points_in[1][0] = linearized_calibration.linearized_points.y[0];
+                points_in[1][1] = linearized_calibration.linearized_points.y[i];
+                points_in[1][2] = linearized_calibration.linearized_points.y[1];
                 points_in[2][0] = 1.;
                 points_in[2][1] = 1.;
                 points_in[2][2] = 1.;
-                points_out[0][0] = cleaned_calibration_points.notch_points_x[0];
-                points_out[0][1] = cleaned_calibration_points.notch_points_x[i];
-                points_out[0][2] = cleaned_calibration_points.notch_points_x[1];
-                points_out[1][0] = cleaned_calibration_points.notch_points_y[0];
-                points_out[1][1] = cleaned_calibration_points.notch_points_y[i];
-                points_out[1][2] = cleaned_calibration_points.notch_points_y[1];
+                points_out[0][0] = cleaned_calibration_points.notch_points.x[0];
+                points_out[0][1] = cleaned_calibration_points.notch_points.x[i];
+                points_out[0][2] = cleaned_calibration_points.notch_points.x[1];
+                points_out[1][0] = cleaned_calibration_points.notch_points.y[0];
+                points_out[1][1] = cleaned_calibration_points.notch_points.y[i];
+                points_out[1][2] = cleaned_calibration_points.notch_points.y[1];
                 points_out[2][0] = 1.;
                 points_out[2][1] = 1.;
                 points_out[2][2] = 1.;
             } else {
-                points_in[0][0] = linearized_calibration.linearized_points_x[0];
-                points_in[0][1] = linearized_calibration.linearized_points_x[i];
-                points_in[0][2] = linearized_calibration.linearized_points_x[i + 1];
-                points_in[1][0] = linearized_calibration.linearized_points_y[0];
-                points_in[1][1] = linearized_calibration.linearized_points_y[i];
-                points_in[1][2] = linearized_calibration.linearized_points_y[i + 1];
+                points_in[0][0] = linearized_calibration.linearized_points.x[0];
+                points_in[0][1] = linearized_calibration.linearized_points.x[i];
+                points_in[0][2] = linearized_calibration.linearized_points.x[i + 1];
+                points_in[1][0] = linearized_calibration.linearized_points.y[0];
+                points_in[1][1] = linearized_calibration.linearized_points.y[i];
+                points_in[1][2] = linearized_calibration.linearized_points.y[i + 1];
                 points_in[2][0] = 1.;
                 points_in[2][1] = 1.;
                 points_in[2][2] = 1.;
-                points_out[0][0] = cleaned_calibration_points.notch_points_x[0];
-                points_out[0][1] = cleaned_calibration_points.notch_points_x[i];
-                points_out[0][2] = cleaned_calibration_points.notch_points_x[i + 1];
-                points_out[1][0] = cleaned_calibration_points.notch_points_y[0];
-                points_out[1][1] = cleaned_calibration_points.notch_points_y[i];
-                points_out[1][2] = cleaned_calibration_points.notch_points_y[i + 1];
+                points_out[0][0] = cleaned_calibration_points.notch_points.x[0];
+                points_out[0][1] = cleaned_calibration_points.notch_points.x[i];
+                points_out[0][2] = cleaned_calibration_points.notch_points.x[i + 1];
+                points_out[1][0] = cleaned_calibration_points.notch_points.y[0];
+                points_out[1][1] = cleaned_calibration_points.notch_points.y[i];
+                points_out[1][2] = cleaned_calibration_points.notch_points.y[i + 1];
                 points_out[2][0] = 1.;
                 points_out[2][1] = 1.;
                 points_out[2][2] = 1.;
@@ -366,10 +373,10 @@ impl NotchCalibration {
             );
 
             out.boundary_angles[i - 1] = match atan2f(
-                linearized_calibration.linearized_points_y[i]
-                    - linearized_calibration.linearized_points_y[0],
-                linearized_calibration.linearized_points_x[i]
-                    - linearized_calibration.linearized_points_x[0],
+                linearized_calibration.linearized_points.y[i]
+                    - linearized_calibration.linearized_points.y[0],
+                linearized_calibration.linearized_points.x[i]
+                    - linearized_calibration.linearized_points.x[0],
             ) {
                 a if a < out.boundary_angles[0] => a + 2. * PI,
                 a => a,