finish up the stick read function (minus calibration steps)

src/input.rs

@@ -14,18 +14,21 @@ use embassy_rp::{
 };
 use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
 use embassy_time::{Instant, Timer};
+use libm::{fmaxf, fmin, fminf};
 use packed_struct::derive::PackedStruct;
 
 use crate::{
     filter::{run_waveshaping, WaveshapingValues},
     gcc_hid::GcReport,
-    stick::{linearize, run_kalman, FilterGains, StickParams},
+    stick::{linearize, notch_remap, run_kalman, FilterGains, StickParams},
     PackedFloat, ADDR_OFFSET, FLASH_SIZE,
 };
 
 pub static GCC_SIGNAL: Signal<CriticalSectionRawMutex, GcReport> = Signal::new();
 
 static STICK_SIGNAL: Signal<CriticalSectionRawMutex, StickState> = Signal::new();
+static STICK_HYST_VAL: f32 = 0.3;
+static FLOAT_ORIGIN: f32 = 127.5;
 
 #[derive(Debug, Clone, Default, Format, PackedStruct)]
 #[packed_struct(endian = "msb")]
@@ -40,8 +43,13 @@ pub struct ControllerConfig {
     pub cx_waveshaping: u8,
     #[packed_field(size_bits = "8")]
     pub cy_waveshaping: u8,
+    #[packed_field(size_bits = "8")]
+    pub astick_analog_scaler: u8,
+    #[packed_field(size_bits = "8")]
+    pub cstick_analog_scaler: u8,
 }
 
+#[derive(Clone, Debug, Default)]
 struct StickState {
     ax: u8,
     ay: u8,
@@ -127,6 +135,7 @@ async fn update_stick_states<
     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,
     controller_config: &ControllerConfig,
@@ -135,7 +144,7 @@ async fn update_stick_states<
     cstick_waveshaping_values: &mut WaveshapingValues,
     old_stick_pos: &mut StickPositions,
     raw_stick_values: &mut RawStickValues,
-) {
+) -> StickState {
     let mut adc_count = 0u32;
     let mut ax_sum = 0u32;
     let mut ay_sum = 0u32;
@@ -216,7 +225,7 @@ async fn update_stick_states<
         filter_gains,
     );
 
-    let pos_x =
+    let pos_x: f32 =
         filter_gains.x_smoothing * shaped_x + (1.0 - filter_gains.x_smoothing) * old_stick_pos.x;
     let pos_y =
         filter_gains.y_smoothing * shaped_y + (1.0 - filter_gains.y_smoothing) * old_stick_pos.y;
@@ -247,12 +256,65 @@ async fn update_stick_states<
 
     // phob optionally runs a median filter here, but we leave it for now
 
-    STICK_SIGNAL.signal(StickState {
+    let (mut remapped_x, mut remapped_y) = notch_remap(
-        ax: 127,
+        pos_x,
-        ay: 127,
+        pos_y,
-        cx: 127,
+        controlstick_params,
-        cy: 127,
+        controller_config,
-    })
+        Stick::ControlStick,
+    );
+    let (mut remapped_cx, mut remapped_cy) = notch_remap(
+        pos_cx_filt,
+        pos_cy_filt,
+        cstick_params,
+        controller_config,
+        Stick::CStick,
+    );
+    let (remapped_x_unfiltered, remapped_y_unfiltered) = notch_remap(
+        raw_stick_values.ax_linearized,
+        raw_stick_values.ay_linearized,
+        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,
+        cstick_params,
+        controller_config,
+        Stick::CStick,
+    );
+
+    remapped_x = fminf(125., fmaxf(-125., remapped_x));
+    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));
+
+    let mut out_stick_state = current_stick_state.clone();
+
+    let diff_x = (remapped_x + FLOAT_ORIGIN) - current_stick_state.ax as f32;
+    if (diff_x > (1.0 + STICK_HYST_VAL)) || (diff_x < -STICK_HYST_VAL) {
+        out_stick_state.ax = (remapped_x + FLOAT_ORIGIN) as u8;
+    }
+    let diff_y = (remapped_y + FLOAT_ORIGIN) - current_stick_state.ay as f32;
+    if (diff_y > (1.0 + STICK_HYST_VAL)) || (diff_y < -STICK_HYST_VAL) {
+        out_stick_state.ay = (remapped_y + FLOAT_ORIGIN) as u8;
+    }
+
+    let diff_cx = (remapped_cx + FLOAT_ORIGIN) - current_stick_state.cx as f32;
+    if (diff_cx > (1.0 + STICK_HYST_VAL)) || (diff_cx < -STICK_HYST_VAL) {
+        out_stick_state.cx = (remapped_cx + FLOAT_ORIGIN) as u8;
+    }
+    let diff_cy = (remapped_cy + FLOAT_ORIGIN) - current_stick_state.cy as f32;
+    if (diff_cy > (1.0 + STICK_HYST_VAL)) || (diff_cy < -STICK_HYST_VAL) {
+        out_stick_state.cy = (remapped_cy + FLOAT_ORIGIN) as u8;
+    }
+
+    out_stick_state
 }
 
 fn update_button_states<
@@ -335,7 +397,8 @@ pub async fn input_loop(
 
     let stick_state_fut = async {
         loop {
-            // update_stick_states(&mut spi, &mut spi_acs, &mut spi_ccs, 1.0).await;
+            // current_stick_state = update_stick_states(&mut spi, &mut spi_acs, &mut spi_ccs, 1.0).await;
+            // STICK_SIGNAL.signal(current_stick_state.clone());
         }
     };
 

src/stick.rs

@@ -1,9 +1,14 @@
 // vast majority of this is taken from Phob firmware
 
-use defmt::Format;
+use core::f32::consts::PI;
-use libm::fabs;
 
-use crate::input::ControllerConfig;
+use defmt::Format;
+use libm::{atan2f, fabs};
+
+use crate::{
+    input::{ControllerConfig, Stick},
+    stick,
+};
 
 /// fit order for the linearization
 const FIT_ORDER: usize = 3;
@@ -18,8 +23,8 @@ pub struct StickParams {
     pub fit_coeffs_y: [f32; NUM_COEFFS],
 
     // these are the notch remap parameters
-    pub affine_coeffs_x: [[f32; 16]; 4], // affine transformation coefficients for all regions of the stick
+    pub affine_coeffs: [[f32; 16]; 4], // affine transformation coefficients for all regions of the stick
-    pub boundary_angles_x: [f32; 4], // angles at the boundaries between regions of the stick (in the plane)
+    pub boundary_angles: [f32; 4], // angles at the boundaries between regions of the stick (in the plane)
 }
 
 #[derive(Clone, Debug, Default, Format)]
@@ -272,3 +277,46 @@ pub fn linearize_calibration(in_x: &[f64; 17], in_y: &[f64; 17]) -> LinearizeCal
         out_y,
     }
 }
+
+pub fn notch_remap(
+    x_in: f32,
+    y_in: f32,
+    stick_params: &StickParams,
+    controller_config: &ControllerConfig,
+    which_stick: Stick,
+) -> (f32, f32) {
+    //determine the angle between the x unit vector and the current position vector
+    let angle = match atan2f(y_in, x_in) {
+        //unwrap the angle based on the first region boundary
+        a if a < stick_params.boundary_angles[0] => a + PI * 2.0,
+        a => a,
+    };
+
+    //go through the region boundaries from lowest angle to highest, checking if the current position vector is in that region
+    //if the region is not found then it must be between the first and the last boundary, ie the last region
+    //we check GATE_REGIONS*2 because each notch has its own very small region we use to make notch values more consistent
+    let region = 'a: {
+        for i in 1..NO_OF_NOTCHES {
+            if angle < stick_params.boundary_angles[i] {
+                break 'a i - 1;
+            }
+        }
+        NO_OF_NOTCHES - 1
+    };
+
+    let stick_scale = match which_stick {
+        Stick::ControlStick => controller_config.astick_analog_scaler as f32 / 100.,
+        Stick::CStick => controller_config.cstick_analog_scaler as f32 / 100.,
+    };
+
+    let x_out = stick_scale
+        * (stick_params.affine_coeffs[region][0] * x_in
+            + stick_params.affine_coeffs[region][1] * y_in);
+    let y_out = stick_scale
+        * (stick_params.affine_coeffs[region][2] * x_in
+            + stick_params.affine_coeffs[region][3] * y_in);
+
+    // TODO: here, add calibration step shenanigans
+
+    (x_out, y_out)
+}