feat: finish notch remap

src/config.rs

@@ -112,6 +112,8 @@ pub struct StickConfig {
     #[packed_field(size_bits = "8")]
     pub y_snapback: i8, // not used for CStick
     #[packed_field(size_bits = "8")]
+    pub cardinal_snapping: i8, // not used for CStick
+    #[packed_field(size_bits = "8")]
     pub x_smoothing: u8,
     #[packed_field(size_bits = "8")]
     pub y_smoothing: u8,
@@ -132,7 +134,8 @@ impl Default for StickConfig {
             y_snapback: 0,
             x_smoothing: 0,
             y_smoothing: 0,
-            analog_scaler: 0,
+            cardinal_snapping: 0,
+            analog_scaler: 100,
             temp_cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
             temp_cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
             angles: *DEFAULT_ANGLES.to_packed_float_array(),
@@ -147,9 +150,9 @@ pub struct ControllerConfig {
     pub config_revision: u8,
     #[packed_field(size_bits = "8")]
     pub config_version: u8,
-    #[packed_field(size_bytes = "327")]
+    #[packed_field(size_bytes = "328")]
     pub astick_config: StickConfig,
-    #[packed_field(size_bytes = "327")]
+    #[packed_field(size_bytes = "328")]
     pub cstick_config: StickConfig,
 }
 
@@ -168,7 +171,7 @@ impl ControllerConfig {
     pub fn from_flash_memory(
         mut flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
     ) -> Result<Self, embassy_rp::flash::Error> {
-        let mut controller_config_packed: <ControllerConfig as packed_struct::PackedStruct>::ByteArray = [0u8; 656]; // ControllerConfig byte size
+        let mut controller_config_packed: <ControllerConfig as packed_struct::PackedStruct>::ByteArray = [0u8; 658]; // ControllerConfig byte size
         flash.blocking_read(ADDR_OFFSET, &mut controller_config_packed)?;
 
         match ControllerConfig::unpack(&controller_config_packed).unwrap() {

src/input.rs

@@ -1,4 +1,4 @@
-use defmt::{debug, info};
+use defmt::{debug, info, trace, Format};
 use embassy_futures::{join::join, yield_now};
 use embassy_rp::{
     flash::{Async, Flash},
@@ -45,7 +45,7 @@ struct StickPositions {
     cy: f32,
 }
 
-#[derive(Clone, Debug, Default)]
+#[derive(Clone, Debug, Default, Format)]
 struct RawStickValues {
     a_linearized: XyValuePair<f32>,
     c_linearized: XyValuePair<f32>,
@@ -182,8 +182,7 @@ async fn update_stick_states<
         y: (cy_sum as f32) / (adc_count as f32) / 4096.0f32,
     };
 
-    debug!("Raw Control Stick: {:?}", raw_controlstick);
+    trace!("Raw Control Stick: {}", raw_controlstick);
-    debug!("Raw C Stick: {:?}", raw_cstick);
 
     raw_stick_values.a_raw = raw_controlstick;
     raw_stick_values.c_raw = raw_cstick;
@@ -199,107 +198,145 @@ async fn update_stick_states<
     raw_stick_values.c_linearized.x = pos_cx;
     raw_stick_values.c_linearized.y = pos_cy;
 
+    trace!("Raw Stick Values 001: {:?}", raw_stick_values);
+
     let (x_pos_filt, y_pos_filt) =
         kalman_state.run_kalman(x_z, y_z, &controller_config.astick_config, &filter_gains);
 
-    let (shaped_x, shaped_y) = run_waveshaping(
+    let shaped_astick = match run_waveshaping(
         x_pos_filt,
         y_pos_filt,
         controller_config.astick_config.x_waveshaping,
         controller_config.astick_config.y_waveshaping,
         controlstick_waveshaping_values,
         &filter_gains,
-    );
+    ) {
+        (x, y) => XyValuePair { x, y },
+    };
 
-    let pos_x: f32 =
+    trace!("Shaped Controlstick: {}", shaped_astick);
-        filter_gains.smoothing.x * shaped_x + (1.0 - filter_gains.smoothing.x) * old_stick_pos.x;
+
-    let pos_y =
+    let pos_x: f32 = filter_gains.smoothing.x * shaped_astick.x
-        filter_gains.smoothing.y * shaped_y + (1.0 - filter_gains.smoothing.y) * old_stick_pos.y;
+        + (1.0 - filter_gains.smoothing.x) * old_stick_pos.x;
+    let pos_y = filter_gains.smoothing.y * shaped_astick.y
+        + (1.0 - filter_gains.smoothing.y) * old_stick_pos.y;
     old_stick_pos.x = pos_x;
     old_stick_pos.y = pos_y;
 
-    let (shaped_cx, shaped_cy) = run_waveshaping(
+    let shaped_cstick = match run_waveshaping(
         pos_cx,
         pos_cy,
         controller_config.cstick_config.x_waveshaping,
         controller_config.cstick_config.y_waveshaping,
         cstick_waveshaping_values,
         &filter_gains,
-    );
+    ) {
+        (x, y) => XyValuePair { x, y },
+    };
 
-    let old_cx_pos = old_stick_pos.cx;
+    let old_c_pos = XyValuePair {
-    let old_cy_pos = old_stick_pos.cy;
+        x: old_stick_pos.cx,
-    old_stick_pos.cx = shaped_cx;
+        y: old_stick_pos.cy,
-    old_stick_pos.cy = shaped_cy;
+    };
+    old_stick_pos.cx = shaped_cstick.x;
+    old_stick_pos.cy = shaped_cstick.y;
 
     let x_weight_1 = filter_gains.c_smoothing.x;
     let x_weight_2 = 1.0 - x_weight_1;
     let y_weight_1 = filter_gains.c_smoothing.y;
     let y_weight_2 = 1.0 - y_weight_1;
 
-    let pos_cx_filt = x_weight_1 * shaped_cx + x_weight_2 * old_cx_pos;
+    let pos_cx_filt = x_weight_1 * shaped_cstick.x + x_weight_2 * old_c_pos.x;
-    let pos_cy_filt = y_weight_1 * shaped_cy + y_weight_2 * old_cy_pos;
+    let pos_cy_filt = y_weight_1 * shaped_cstick.y + y_weight_2 * old_c_pos.y;
 
     // phob optionally runs a median filter here, but we leave it for now
 
-    let (mut remapped_x, mut remapped_y) = notch_remap(
+    trace!("Controlstick position: {}, {}", pos_x, pos_y);
+
+    let mut remapped = match notch_remap(
         pos_x,
         pos_y,
         controlstick_params,
         controller_config,
         Stick::ControlStick,
-    );
+        false,
-    let (mut remapped_cx, mut remapped_cy) = notch_remap(
+    ) {
+        (x, y) => XyValuePair { x, y },
+    };
+    let mut remapped_c = match notch_remap(
         pos_cx_filt,
         pos_cy_filt,
         cstick_params,
         controller_config,
         Stick::CStick,
-    );
+        false,
-    let (remapped_x_unfiltered, remapped_y_unfiltered) = notch_remap(
+    ) {
+        (x, y) => XyValuePair { x, y },
+    };
+    let remapped_unfiltered = match notch_remap(
         raw_stick_values.a_linearized.x,
         raw_stick_values.a_linearized.y,
         controlstick_params,
         controller_config,
         Stick::ControlStick,
-    );
+        false,
-    let (remapped_cx_unfiltered, remapped_cy_unfiltered) = notch_remap(
+    ) {
+        (x, y) => XyValuePair { x, y },
+    };
+    let remapped_c_unfiltered = match notch_remap(
         raw_stick_values.c_linearized.x,
         raw_stick_values.c_linearized.y,
         cstick_params,
         controller_config,
         Stick::CStick,
-    );
+        false,
+    ) {
+        (x, y) => XyValuePair { x, y },
+    };
 
-    remapped_x = fminf(125., fmaxf(-125., remapped_x));
+    trace!("Remapped Control Stick: {}", remapped);
-    remapped_y = fminf(125., fmaxf(-125., remapped_y));
+
-    remapped_cx = fminf(125., fmaxf(-125., remapped_cx));
+    remapped = XyValuePair {
-    remapped_cy = fminf(125., fmaxf(-125., remapped_cy));
+        x: fminf(125., fmaxf(-125., remapped.x)),
-    raw_stick_values.a_unfiltered.x = fminf(125., fmaxf(-125., remapped_x_unfiltered));
+        y: fminf(125., fmaxf(-125., remapped.y)),
-    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));
+    remapped_c = XyValuePair {
-    raw_stick_values.c_unfiltered.y = fminf(125., fmaxf(-125., remapped_cy_unfiltered));
+        x: fminf(125., fmaxf(-125., remapped_c.x)),
+        y: fminf(125., fmaxf(-125., remapped_c.y)),
+    };
+    raw_stick_values.a_unfiltered.x = fminf(125., fmaxf(-125., remapped_unfiltered.x));
+    raw_stick_values.a_unfiltered.y = fminf(125., fmaxf(-125., remapped_unfiltered.y));
+    raw_stick_values.c_unfiltered.x = fminf(125., fmaxf(-125., remapped_c_unfiltered.x));
+    raw_stick_values.c_unfiltered.y = fminf(125., fmaxf(-125., remapped_c_unfiltered.y));
 
     let mut out_stick_state = current_stick_state.clone();
 
-    let diff_x = (remapped_x + FLOAT_ORIGIN) - current_stick_state.ax as f32;
+    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;
+        out_stick_state.ax = (remapped.x + FLOAT_ORIGIN) as u8;
     }
-    let diff_y = (remapped_y + FLOAT_ORIGIN) - current_stick_state.ay as f32;
+    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;
+        out_stick_state.ay = (remapped.y + FLOAT_ORIGIN) as u8;
     }
 
-    let diff_cx = (remapped_cx + FLOAT_ORIGIN) - current_stick_state.cx as f32;
+    let diff_cx = (remapped_c.x + 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;
+        out_stick_state.cx = (remapped_c.x + FLOAT_ORIGIN) as u8;
     }
-    let diff_cy = (remapped_cy + FLOAT_ORIGIN) - current_stick_state.cy as f32;
+    let diff_cy = (remapped_c.y + 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.cy = (remapped_c.y + FLOAT_ORIGIN) as u8;
     }
 
+    trace!(
+        "Control stick: {}, {}, C-stick: {}, {}",
+        out_stick_state.ax,
+        out_stick_state.ay,
+        out_stick_state.cx,
+        out_stick_state.cy
+    );
+
     out_stick_state
 }
 
@@ -403,7 +440,7 @@ pub async fn input_loop(
         let mut last_loop_time = Instant::now();
 
         loop {
-            let timer = Timer::after_millis(1);
+            let timer = Timer::after_micros(1000);
 
             current_stick_state = update_stick_states(
                 &mut spi,
@@ -424,10 +461,12 @@ pub async fn input_loop(
 
             timer.await;
 
-            debug!(
+            match (Instant::now() - last_loop_time).as_micros() {
-                "Loop took {} us",
+                a if a > 1100 => {
-                (Instant::now() - last_loop_time).as_micros()
+                    debug!("Loop took {} us", a);
-            );
+                }
+                _ => {}
+            };
 
             last_loop_time = Instant::now();
 

src/stick.rs

@@ -3,7 +3,7 @@
 use core::f32::consts::PI;
 
 use defmt::{debug, Format};
-use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf};
+use libm::{atan2f, cosf, fabs, fabsf, roundf, sinf, sqrtf};
 
 use crate::{
     config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS},
@@ -583,6 +583,7 @@ pub fn notch_remap(
     stick_params: &StickParams,
     controller_config: &ControllerConfig,
     which_stick: Stick,
+    is_calibrating: bool,
 ) -> (f32, f32) {
     //determine the angle between the x unit vector and the current position vector
     let angle = match atan2f(y_in, x_in) {
@@ -608,14 +609,40 @@ pub fn notch_remap(
         Stick::CStick => controller_config.cstick_config.analog_scaler as f32 / 100.,
     };
 
-    let x_out = stick_scale
+    let mut 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
+    let mut 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
+    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.;
+            }
+            if fabsf(y_out) < stick_config.cardinal_snapping as f32 + 0.5 && fabsf(x_out) >= 79.5 {
+                y_out = 0.;
+            }
+        } else if stick_config.cardinal_snapping == -1 {
+            if fabsf(x_out) < 6.5 && fabsf(y_out) >= 79.5 {
+                x_out = 0.;
+            }
+            if fabsf(y_out) < 6.5 && fabsf(x_out) >= 79.5 {
+                y_out = 0.;
+            }
+        }
+
+        if fabsf(x_out) < 3. && fabsf(y_out) < 3. {
+            x_out = 0.;
+            y_out = 0.;
+        }
+    }
 
     (x_out, y_out)
 }