feat(stick): get calibration to a working state

README.md

@@ -44,4 +44,4 @@ This is how it is when you're playing with a PhobGCC (the best GCC currently ava
 
 ### The Solution
 
-Since the NaxGCC connects directly to the console and eliminates the joybus protocol entirely, there is no second polling rate. The scan rate of the NaxGCC's sticks is 1ms, and the buttons are scanned as quickly as the MCU allows (I've measured ~200us -ish). While not quite reaching the ~8.66ms window length, the sticks have a ~7.66ms window of guaranteed input integrity, and the buttons are getting fairly close to ~8.3ms (half a frame).
+Since the NaxGCC connects directly to the console and eliminates the joybus protocol entirely, there is no second polling rate. The scan rate of the NaxGCC's sticks is 1ms, and the buttons are scanned as quickly as the MCU allows (I've measured ~50s on average, worst outliers being ~100us). While not quite reaching the ~8.66ms window length, the sticks have a ~7.66ms window of guaranteed input integrity, and the buttons are getting fairly close to ~8.56ms at worst (more than half a frame).

src/config.rs

@@ -25,7 +25,7 @@ use crate::{
 };
 
 use embassy_sync::{
-    blocking_mutex::raw::{CriticalSectionRawMutex, RawMutex},
+    blocking_mutex::raw::{CriticalSectionRawMutex, RawMutex, ThreadModeRawMutex},
     pubsub::{PubSubBehavior, Subscriber},
     signal::Signal,
 };
@@ -35,7 +35,7 @@ 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();
+pub static SIGNAL_IS_CALIBRATING: Signal<ThreadModeRawMutex, 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<
@@ -625,6 +625,8 @@ pub async fn config_task(
 ) {
     let mut gcc_subscriber = CHANNEL_GCC_STATE.subscriber().unwrap();
 
+    info!("Config task is running.");
+
     loop {
         gcc_subscriber
             .wait_for_simultaneous_button_presses(&CONFIG_MODE_ENTRY_COMBO)

src/input.rs

@@ -38,9 +38,6 @@ pub static CHANNEL_GCC_STATE: PubSubChannel<CriticalSectionRawMutex, GcReport, 1
 /// Used to send the stick state from the stick task to the main input task
 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>>> =
@@ -151,7 +148,7 @@ async fn update_stick_states(
     let mut cx_sum = 0u32;
     let mut cy_sum = 0u32;
 
-    let end_time = Instant::now() + Duration::from_micros(300); // this seems kinda magic, and it is, but
+    let end_time = Instant::now() + Duration::from_micros(250); // 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;
@@ -487,6 +484,7 @@ pub async fn update_stick_states_task(
     spi_ccs: Output<'static, AnyPin>,
     controller_config: ControllerConfig,
 ) {
+    Timer::after_secs(1).await;
     *SPI_SHARED.lock().await = Some(spi);
     *SPI_ACS_SHARED.lock().await = Some(spi_acs);
     *SPI_CCS_SHARED.lock().await = Some(spi_ccs);
@@ -495,6 +493,8 @@ pub async fn update_stick_states_task(
     let cstick_params = StickParams::from_stick_config(&controller_config.cstick_config);
     let filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config);
 
+    info!("Controlstick params: {:?}", controlstick_params);
+
     let mut current_stick_state = StickState {
         ax: 127,
         ay: 127,
@@ -538,7 +538,7 @@ pub async fn update_stick_states_task(
         match Instant::now() {
             n => {
                 match (n - last_loop_time).as_micros() {
-                    a if a > 1 => debug!("Loop took {} us", a),
+                    a if a > 19999 => debug!("Loop took {} us", a),
                     _ => {}
                 };
                 last_loop_time = n;
@@ -548,5 +548,14 @@ pub async fn update_stick_states_task(
         SIGNAL_STICK_STATE.signal(current_stick_state.clone());
 
         ticker.next().await;
+
+        #[cfg(debug_assertions)]
+        {
+            // give other tasks a chance to do something
+            // in debug, this loop runs noticeably slower, so this is necessary
+            // prefer running with `cargo run --release` for this reason, when
+            // developing stick related features
+            yield_now().await;
+        }
     }
 }

src/main.rs

@@ -15,7 +15,10 @@ use config::config_task;
 use config::ControllerConfig;
 use defmt::{debug, info};
 use embassy_executor::Executor;
+use embassy_executor::InterruptExecutor;
 use embassy_futures::join::join;
+use embassy_rp::interrupt;
+use embassy_rp::interrupt::InterruptExt;
 use embassy_rp::{
     bind_interrupts,
     flash::{Async, Flash},
@@ -103,8 +106,20 @@ fn main() -> ! {
         });
     });
 
-    let executor0 = EXECUTOR0.init(Executor::new());
+    // Stick loop has to run on core0 because it makes use of SPI0.
-    info!("Initialized.");
+    // 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.
+    // Also, it needs to run on a higher prio executor to ensure consistent polling.
+    // interrupt::SWI_IRQ_1.set_priority(interrupt::Priority::P0);
+    // let spawner_high = EXECUTOR_HIGH.start(interrupt::SWI_IRQ_1);
+    // spawner_high
+    //     .spawn(update_stick_states_task(
+    //         spi,
+    //         spi_acs,
+    //         spi_ccs,
+    //         controller_config.clone(),
+    //     ))
+    //     .unwrap();
 
     let mut pwm_config: embassy_rp::pwm::Config = Default::default();
     pwm_config.top = 255;
@@ -117,15 +132,15 @@ fn main() -> ! {
     // pwm_rumble.set_counter(0);
     // pwm_brake.set_counter(255);
 
+    let executor0 = EXECUTOR0.init(Executor::new());
+    info!("Initialized.");
+
     executor0.run(|spawner| {
         // 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,
@@ -133,6 +148,6 @@ fn main() -> ! {
                 spi_ccs,
                 controller_config,
             ))
-            .unwrap()
+            .unwrap();
     });
 }

src/stick.rs

@@ -2,7 +2,7 @@
 
 use core::f32::consts::PI;
 
-use defmt::{debug, Format};
+use defmt::{debug, info, trace, Format};
 use libm::{atan2f, cosf, fabs, fabsf, fmaxf, fminf, roundf, sinf, sqrtf};
 
 use crate::{
@@ -107,10 +107,11 @@ impl CleanedCalibrationPoints {
     ) -> Self {
         let mut out = Self::default();
 
-        debug!("Raw calibration points:");
+        trace!(
-        for i in 0..NO_OF_CALIBRATION_POINTS {
+            "Raw calibration points x {} and y {}:",
-            debug!("({}, {})", cal_points_x[i], cal_points_y[i])
+            cal_points_x,
-        }
+            cal_points_y
+        );
 
         debug!("Notch angles: {}", notch_angles);
 
@@ -132,32 +133,45 @@ impl CleanedCalibrationPoints {
         // TODO: put the below in a macro to clean it up a bit, once it's confirmed to work
         // remove the largest and smallest two origin values to remove outliers
         // first, find their indices
-        let mut i = 0;
+        let mut smallest_x = 0;
-        let x_by_size = &mut cal_points_x.map(|e| {
+        let mut small_x = 0;
-            i += 1;
+        let mut large_x = 0;
-            (i - 1, e)
+        let mut largest_x = 0;
-        });
 
-        tiny_sort::unstable::sort_by(x_by_size, |a, b| a.1.partial_cmp(&b.1).unwrap());
+        let mut smallest_y = 0;
+        let mut small_y = 0;
+        let mut large_y = 0;
+        let mut largest_y = 0;
 
-        let smallest_x = x_by_size[0].0;
+        for i in 0..NO_OF_NOTCHES {
-        let small_x = x_by_size[1].0;
+            if cal_points_x[i * 2] < cal_points_x[smallest_x] {
-        let large_x = x_by_size[x_by_size.len() - 2].0;
+                small_x = smallest_x;
-        let largest_x = x_by_size[x_by_size.len() - 1].0;
+                smallest_x = i * 2;
+            } else if cal_points_x[i * 2] < cal_points_x[small_x] {
+                small_x = i * 2;
+            }
 
-        // do the same for y
+            if cal_points_x[i * 2] > cal_points_x[largest_x] {
-        let mut i = 0;
+                large_x = largest_x;
-        let y_by_size = &mut cal_points_y.map(|e| {
+                largest_x = i * 2;
-            i += 1;
+            } else if cal_points_x[i * 2] > cal_points_x[large_x] {
-            (i - 1, e)
+                large_x = i * 2;
-        });
+            }
 
-        tiny_sort::unstable::sort_by(y_by_size, |a, b| a.1.partial_cmp(&b.1).unwrap());
+            if cal_points_y[i * 2] < cal_points_y[smallest_y] {
+                small_y = smallest_y;
+                smallest_y = i * 2;
+            } else if cal_points_y[i * 2] < cal_points_y[small_y] {
+                small_y = i * 2;
+            }
 
-        let smallest_y = y_by_size[0].0;
+            if cal_points_y[i * 2] > cal_points_y[largest_y] {
-        let small_y = y_by_size[1].0;
+                large_y = largest_y;
-        let large_y = y_by_size[y_by_size.len() - 2].0;
+                largest_y = i * 2;
-        let largest_y = y_by_size[y_by_size.len() - 1].0;
+            } else if cal_points_y[i * 2] > cal_points_y[large_y] {
+                large_y = i * 2;
+            }
+        }
 
         // TODO: make this whole thing a function? it looks very ugly
         out.cleaned_points.x[0] -= cal_points_x[smallest_x];
@@ -180,8 +194,8 @@ impl CleanedCalibrationPoints {
 
             // if the cleaned point was at the center and would be a firefox notch
             // average the previous and next points (cardinal & diagonal) for some sanity
-            if mag < 0.02 && (i % 2 == 0) {
+            if mag < 0.02 && (i % 2 != 0) {
-                let prev_index = ((i + NO_OF_NOTCHES - 1) % NO_OF_NOTCHES) + 1;
+                let prev_index = ((i - 1 + NO_OF_NOTCHES) % NO_OF_NOTCHES) + 1;
                 let next_index = ((i + 1) % NO_OF_NOTCHES) + 1;
 
                 out.cleaned_points.x[i + 1] =
@@ -194,7 +208,7 @@ impl CleanedCalibrationPoints {
                 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);
+                trace!("Skipping notch {}", i + 1);
 
                 // Mark that notch adjustment should be skipped for this
                 out.notch_status[i] = NotchStatus::TertInactive;
@@ -203,18 +217,15 @@ impl CleanedCalibrationPoints {
             }
         }
 
-        debug!("Final points:");
+        trace!(
-        for i in 0..=NO_OF_NOTCHES {
+            "Final points clean_x: {:?}, clean_y: {:?}, notch_x: {:?}, notch_y: {:?}",
-            debug!(
+            out.cleaned_points.x,
-                "Cleaned: ({}, {}), Notch: ({}, {})",
+            out.cleaned_points.y,
-                out.cleaned_points.x[i],
+            out.notch_points.x,
-                out.cleaned_points.y[i],
+            out.notch_points.y
-                out.notch_points.x[i],
+        );
-                out.notch_points.y[i],
-            );
-        }
 
-        debug!("The notch statuses are: {:?}", out.notch_status);
+        trace!("The notch statuses are: {:?}", out.notch_status);
 
         out
     }
@@ -283,6 +294,11 @@ impl LinearizedCalibration {
             linearized_points_y[i] = linearize(in_y[i] as f32, &fit_coeffs_y.map(|e| e as f32));
         }
 
+        debug!(
+            "Linearized points x: {:?}, y: {:?}",
+            linearized_points_x, linearized_points_y
+        );
+
         Self {
             fit_coeffs: XyValuePair {
                 x: fit_coeffs_x,
@@ -352,14 +368,14 @@ impl NotchCalibration {
                 points_out[2][1] = 1.;
                 points_out[2][2] = 1.;
             }
-            debug!("In points: {:?}", points_in);
+            trace!("In points: {:?}", points_in);
-            debug!("Out points: {:?}", points_out);
+            trace!("Out points: {:?}", points_out);
 
             let temp = inverse(&points_in);
 
             let a = matrix_mult(&points_out, &temp);
 
-            debug!("The transform matrix is: {:?}", a);
+            trace!("The transform matrix is: {:?}", a);
 
             for j in 0..2 {
                 for k in 0..2 {
@@ -367,7 +383,7 @@ impl NotchCalibration {
                 }
             }
 
-            debug!(
+            trace!(
                 "Transform coefficients for this region are: {:?}",
                 out.affine_coeffs[i - 1]
             );
@@ -387,7 +403,7 @@ impl NotchCalibration {
     }
 }
 
-#[derive(Debug, Clone, Format, Default)]
+#[derive(Debug, Clone, Format)]
 pub struct AppliedCalibration {
     pub stick_params: StickParams,
     pub cleaned_calibration: CleanedCalibrationPoints,
@@ -395,6 +411,17 @@ pub struct AppliedCalibration {
     pub measured_notch_angles: [f32; NO_OF_NOTCHES],
 }
 
+impl Default for AppliedCalibration {
+    fn default() -> Self {
+        Self {
+            stick_params: StickParams::default(),
+            cleaned_calibration: CleanedCalibrationPoints::default(),
+            notch_angles: DEFAULT_ANGLES,
+            measured_notch_angles: [0f32; NO_OF_NOTCHES],
+        }
+    }
+}
+
 impl AppliedCalibration {
     pub fn from_points(
         cal_points_x: &[f32; NO_OF_CALIBRATION_POINTS],
@@ -404,8 +431,6 @@ impl AppliedCalibration {
     ) -> 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);
 
@@ -444,9 +469,16 @@ impl AppliedCalibration {
             stick_config,
         );
 
+        info!(
+            "Transformed calibration points x: {:?}, y: {:?}",
+            transformed_cal_points_x, transformed_cal_points_y
+        );
+
         let measured_notch_angles =
             compute_stick_angles(&transformed_cal_points_x, &transformed_cal_points_y);
 
+        info!("Measured notch angles: {:?}", measured_notch_angles);
+
         let cleaned_with_measured_notch_angles =
             CleanedCalibrationPoints::from_temp_calibration_points(
                 cal_points_x,
@@ -535,9 +567,10 @@ fn legalize_notch(
         a => a,
     };
 
-    let (cmp_amt, str_amt) = match is_diagonal {
+    let (cmp_amt, str_amt) = if is_diagonal {
-        true => (0.769, 1.3),
+        (0.769, 1.3)
-        false => (0.666, 1.5),
+    } else {
+        (0.666, 1.5)
     };
 
     let min_threshold = 0.15 / 0.975;
@@ -547,7 +580,7 @@ fn legalize_notch(
     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
+    let lower_strech_limit = if next_idx % 4 == 0
         && !is_diagonal
         && (next_meas_angle - this_meas_angle) > min_threshold
         && (next_meas_angle - this_meas_angle) < deadzone_limit
@@ -614,7 +647,13 @@ fn compute_stick_angles(
         if i % 2 == 0 {
             angles[i] = DEFAULT_ANGLES[i];
         } else {
-            angles[i] = angle_on_sphere(x_in[i], y_in[i]);
+            angles[i] = angle_on_sphere(x_in[i + 1], y_in[i + 1]);
+            debug!(
+                "Computed angle for x,y: ({}, {}) is: {}",
+                x_in[i + 1],
+                y_in[i + 1],
+                angles[i]
+            );
         }
     }
 
@@ -665,19 +704,21 @@ fn strip_cal_points(
 fn inverse(in_mat: &[[f32; 3]; 3]) -> [[f32; 3]; 3] {
     let mut out_mat = [[0f32; 3]; 3];
 
-    let det = in_mat[0][0] * (in_mat[1][1] * in_mat[2][2] - in_mat[1][2] * in_mat[2][1])
+    let det = in_mat[0][0] * (in_mat[1][1] * in_mat[2][2] - in_mat[2][1] * in_mat[1][2])
         - in_mat[0][1] * (in_mat[1][0] * in_mat[2][2] - in_mat[1][2] * in_mat[2][0])
         + in_mat[0][2] * (in_mat[1][0] * in_mat[2][1] - in_mat[1][1] * in_mat[2][0]);
 
-    out_mat[0][0] = (in_mat[1][1] * in_mat[2][2] - in_mat[1][2] * in_mat[2][1]) / det;
+    let invdet = 1. / det;
-    out_mat[0][1] = (in_mat[0][2] * in_mat[2][1] - in_mat[0][1] * in_mat[2][2]) / det;
+
-    out_mat[0][2] = (in_mat[0][1] * in_mat[1][2] - in_mat[0][2] * in_mat[1][1]) / det;
+    out_mat[0][0] = (in_mat[1][1] * in_mat[2][2] - in_mat[2][1] * in_mat[1][2]) * invdet;
-    out_mat[1][0] = (in_mat[1][2] * in_mat[2][0] - in_mat[1][0] * in_mat[2][2]) / det;
+    out_mat[0][1] = (in_mat[0][2] * in_mat[2][1] - in_mat[0][1] * in_mat[2][2]) * invdet;
-    out_mat[1][1] = (in_mat[0][0] * in_mat[2][2] - in_mat[0][2] * in_mat[2][0]) / det;
+    out_mat[0][2] = (in_mat[0][1] * in_mat[1][2] - in_mat[0][2] * in_mat[1][1]) * invdet;
-    out_mat[1][2] = (in_mat[0][2] * in_mat[1][0] - in_mat[0][0] * in_mat[1][2]) / det;
+    out_mat[1][0] = (in_mat[1][2] * in_mat[2][0] - in_mat[1][0] * in_mat[2][2]) * invdet;
-    out_mat[2][0] = (in_mat[1][0] * in_mat[2][1] - in_mat[1][1] * in_mat[2][0]) / det;
+    out_mat[1][1] = (in_mat[0][0] * in_mat[2][2] - in_mat[0][2] * in_mat[2][0]) * invdet;
-    out_mat[2][1] = (in_mat[0][1] * in_mat[2][0] - in_mat[0][0] * in_mat[2][1]) / det;
+    out_mat[1][2] = (in_mat[1][0] * in_mat[0][2] - in_mat[0][0] * in_mat[1][2]) * invdet;
-    out_mat[2][2] = (in_mat[0][0] * in_mat[1][1] - in_mat[0][1] * in_mat[1][0]) / det;
+    out_mat[2][0] = (in_mat[1][0] * in_mat[2][1] - in_mat[2][0] * in_mat[1][1]) * invdet;
+    out_mat[2][1] = (in_mat[2][0] * in_mat[0][1] - in_mat[0][0] * in_mat[2][1]) * invdet;
+    out_mat[2][2] = (in_mat[0][0] * in_mat[1][1] - in_mat[1][0] * in_mat[0][1]) * invdet;
 
     out_mat
 }
@@ -729,7 +770,8 @@ fn sub_col<const N: usize>(
 
 /// Calculate the determinant of a matrix
 fn det<const N: usize>(matrix: &[[f64; N]; N]) -> f64 {
-    let sign = trianglize(matrix);
+    let mut matrix = *matrix;
+    let sign = trianglize(&mut matrix);
 
     if sign == 0 {
         return 0.;
@@ -745,7 +787,7 @@ fn det<const N: usize>(matrix: &[[f64; N]; N]) -> f64 {
 }
 
 /// Trianglize a matrix
-fn trianglize<const N: usize>(matrix: &[[f64; N]; N]) -> i32 {
+fn trianglize<const N: usize>(matrix: &mut [[f64; N]; N]) -> i32 {
     let mut sign = 1;
     let mut matrix = *matrix;