begin implementing stick filter algorithms

src/filter.rs

@@ -0,0 +1,82 @@
+use libm::{fmin, fminf};
+
+use crate::{
+    input::{ControllerConfig, Stick},
+    stick::FilterGains,
+};
+
+pub struct WaveshapingValues {
+    pub old_x_pos: f32,
+    pub old_y_pos: f32,
+    pub old_x_vel: f32,
+    pub old_y_vel: f32,
+    pub old_x_out: f32,
+    pub old_y_out: f32,
+}
+
+fn calc_waveshaping_mult(setting: u8) -> f32 {
+    if setting > 0 && setting <= 5 {
+        1. / (440. - 40. * setting as f32)
+    } else if setting > 5 && setting <= 15 {
+        1. / (340. - 20. * setting as f32)
+    } else {
+        0.
+    }
+}
+
+/// This simulates an idealized sort of pode:
+///
+///  if the stick is moving fast, it responds poorly, while
+///  if the stick is moving slowly, it follows closely.
+///
+/// It's not suitable to be the sole filter, but when put after
+///  the smart snapback filter, it should be able to hold the
+///  output at the rim longer when released.
+///
+/// Output is a tuple of the x and y positions.
+pub fn run_waveshaping(
+    x_pos: f32,
+    y_pos: f32,
+    which_stick: Stick,
+    waveshaping_values: &mut WaveshapingValues,
+    controller_config: &ControllerConfig,
+    filter_gains: &FilterGains,
+) -> (f32, f32) {
+    let x_vel = x_pos - waveshaping_values.old_x_pos;
+    let y_vel = y_pos - waveshaping_values.old_y_pos;
+
+    let x_vel_smooth = 0.5 * (x_vel + waveshaping_values.old_x_vel);
+    let y_vel_smooth = 0.5 * (y_vel + waveshaping_values.old_y_vel);
+
+    let x_factor = calc_waveshaping_mult(match which_stick {
+        Stick::ControlStick => controller_config.ax_waveshaping,
+        Stick::CStick => controller_config.cx_waveshaping,
+    });
+    let y_factor = calc_waveshaping_mult(match which_stick {
+        Stick::ControlStick => controller_config.ay_waveshaping,
+        Stick::CStick => controller_config.cy_waveshaping,
+    });
+
+    let old_x_pos_weight = fminf(
+        1.,
+        x_vel_smooth * x_vel_smooth * filter_gains.vel_thresh * x_factor,
+    );
+    let new_x_pos_weight = 1. - old_x_pos_weight;
+    let old_y_pos_weight = fminf(
+        1.,
+        y_vel_smooth * y_vel_smooth * filter_gains.vel_thresh * y_factor,
+    );
+    let new_y_pos_weight = 1. - old_y_pos_weight;
+
+    let x_out = x_pos * new_x_pos_weight + waveshaping_values.old_x_out * old_x_pos_weight;
+    let y_out = y_pos * new_y_pos_weight + waveshaping_values.old_y_out * old_y_pos_weight;
+
+    waveshaping_values.old_x_pos = x_pos;
+    waveshaping_values.old_y_pos = y_pos;
+    waveshaping_values.old_x_vel = x_vel_smooth;
+    waveshaping_values.old_y_vel = y_vel_smooth;
+    waveshaping_values.old_x_out = x_out;
+    waveshaping_values.old_y_out = y_out;
+
+    (x_out, y_out)
+}

src/input.rs

@@ -17,6 +17,7 @@ use embassy_time::{Instant, Timer};
 use packed_struct::derive::PackedStruct;
 
 use crate::{
+    filter::{run_waveshaping, WaveshapingValues},
     gcc_hid::GcReport,
     stick::{linearize, run_kalman, FilterGains, StickParams},
     PackedFloat, ADDR_OFFSET, FLASH_SIZE,
@@ -31,14 +32,14 @@ static STICK_SIGNAL: Signal<CriticalSectionRawMutex, StickState> = Signal::new()
 pub struct ControllerConfig {
     #[packed_field(size_bits = "8")]
     pub config_version: u8,
-    #[packed_field(size_bits = "32")]
+    #[packed_field(size_bits = "8")]
-    pub ax_waveshaping: PackedFloat,
+    pub ax_waveshaping: u8,
-    #[packed_field(size_bits = "32")]
+    #[packed_field(size_bits = "8")]
-    pub ay_waveshaping: PackedFloat,
+    pub ay_waveshaping: u8,
-    #[packed_field(size_bits = "32")]
+    #[packed_field(size_bits = "8")]
-    pub cx_waveshaping: PackedFloat,
+    pub cx_waveshaping: u8,
-    #[packed_field(size_bits = "32")]
+    #[packed_field(size_bits = "8")]
-    pub cy_waveshaping: PackedFloat,
+    pub cy_waveshaping: u8,
 }
 
 struct StickState {
@@ -48,14 +49,36 @@ struct StickState {
     cy: u8,
 }
 
+struct StickPositions {
+    x: f32,
+    y: f32,
+    cx: f32,
+    cy: f32,
+}
+
+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,
+}
+
 #[derive(PartialEq, Eq)]
-enum Stick {
+pub enum Stick {
     ControlStick,
     CStick,
 }
 
 #[derive(PartialEq, Eq)]
-enum StickAxis {
+pub enum StickAxis {
     XAxis,
     YAxis,
 }
@@ -104,11 +127,14 @@ 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>,
-    adc_scale: f32,
     controlstick_params: &StickParams,
     cstick_params: &StickParams,
     controller_config: &ControllerConfig,
     filter_gains: &FilterGains,
+    controlstick_waveshaping_values: &mut WaveshapingValues,
+    cstick_waveshaping_values: &mut WaveshapingValues,
+    old_stick_pos: &mut StickPositions,
+    raw_stick_values: &mut RawStickValues,
 ) {
     let mut adc_count = 0u32;
     let mut ax_sum = 0u32;
@@ -158,10 +184,15 @@ async fn update_stick_states<
 
     timer.await;
 
-    let raw_controlstick_x = (ax_sum as f32) / (adc_count as f32) / 4096.0f32 * adc_scale;
+    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 * adc_scale;
+    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 * adc_scale;
+    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 * adc_scale;
+    let raw_cstick_y = (cy_sum as f32) / (adc_count as f32) / 4096.0f32;
+
+    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;
 
     let x_z = linearize(raw_controlstick_x, &controlstick_params.fit_coeffs_x);
     let y_z = linearize(raw_controlstick_y, &controlstick_params.fit_coeffs_y);
@@ -169,8 +200,53 @@ async fn update_stick_states<
     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;
+
     let (x_pos_filt, y_pos_filt) = run_kalman(x_z, y_z, controller_config, filter_gains);
 
+    let (shaped_x, shaped_y) = run_waveshaping(
+        x_pos_filt,
+        y_pos_filt,
+        Stick::ControlStick,
+        controlstick_waveshaping_values,
+        controller_config,
+        filter_gains,
+    );
+
+    let pos_x =
+        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;
+    old_stick_pos.x = pos_x;
+    old_stick_pos.y = pos_y;
+
+    let (shaped_cx, shaped_cy) = run_waveshaping(
+        pos_cx,
+        pos_cy,
+        Stick::CStick,
+        cstick_waveshaping_values,
+        controller_config,
+        filter_gains,
+    );
+
+    let old_cx_pos = old_stick_pos.cx;
+    let old_cy_pos = old_stick_pos.cy;
+    old_stick_pos.cx = shaped_cx;
+    old_stick_pos.cy = shaped_cy;
+
+    let x_weight_1 = filter_gains.c_xsmoothing;
+    let x_weight_2 = 1.0 - x_weight_1;
+    let y_weight_1 = filter_gains.c_ysmoothing;
+    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_cy_filt = y_weight_1 * shaped_cy + y_weight_2 * old_cy_pos;
+
+    // phob optionally runs a median filter here, but we leave it for now
+
     STICK_SIGNAL.signal(StickState {
         ax: 127,
         ay: 127,

src/main.rs

@@ -4,6 +4,7 @@
 
 #![no_std]
 #![no_main]
+mod filter;
 mod gcc_hid;
 mod input;
 mod stick;

src/stick.rs

@@ -7,15 +7,15 @@ use crate::input::ControllerConfig;
 
 /// fit order for the linearization
 const FIT_ORDER: usize = 3;
-const N_COEFFS: usize = FIT_ORDER + 1;
+const NUM_COEFFS: usize = FIT_ORDER + 1;
 const NO_OF_NOTCHES: usize = 16;
 const MAX_ORDER: usize = 20;
 
 #[derive(Clone, Debug, Default, Format)]
 pub struct StickParams {
     // these are the linearization coefficients
-    pub fit_coeffs_x: [f32; N_COEFFS],
+    pub fit_coeffs_x: [f32; NUM_COEFFS],
-    pub fit_coeffs_y: [f32; N_COEFFS],
+    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
@@ -60,8 +60,8 @@ pub struct FilterGains {
 
 #[derive(Clone, Debug, Default)]
 struct LinearizeCalibrationOutput {
-    pub fit_coeffs_x: [f64; N_COEFFS],
+    pub fit_coeffs_x: [f64; NUM_COEFFS],
-    pub fit_coeffs_y: [f64; N_COEFFS],
+    pub fit_coeffs_y: [f64; NUM_COEFFS],
 
     pub out_x: [f32; NO_OF_NOTCHES],
     pub out_y: [f32; NO_OF_NOTCHES],
@@ -76,12 +76,14 @@ pub fn run_kalman(
     todo!()
 }
 
+/// Calculate the power of a number
 fn curve_fit_power(base: f64, exponent: u32) -> f64 {
     if exponent == 0 {
         return 1.0;
     }
 
     let mut val = base;
+
     for _ in 1..exponent {
         val *= base;
     }
@@ -89,6 +91,7 @@ fn curve_fit_power(base: f64, exponent: u32) -> f64 {
     val
 }
 
+/// Substitutes a column in a matrix with a vector
 fn sub_col<const N: usize>(
     matrix: &[[f64; N]; N],
     t: &[f64; MAX_ORDER],
@@ -96,24 +99,32 @@ fn sub_col<const N: usize>(
     n: usize,
 ) -> [[f64; N]; N] {
     let mut m = *matrix;
+
     for i in 0..n {
         m[i][col] = t[i];
     }
+
     m
 }
 
+/// Calculate the determinant of a matrix
 fn det<const N: usize>(matrix: &[[f64; N]; N]) -> f64 {
     let sign = trianglize(matrix);
+
     if sign == 0 {
         return 0.;
     }
+
     let mut p = 1f64;
+
     for i in 0..N {
         p *= matrix[i][i];
     }
+
     p * (sign as f64)
 }
 
+/// Trianglize a matrix
 fn trianglize<const N: usize>(matrix: &[[f64; N]; N]) -> i32 {
     let mut sign = 1;
     let mut matrix = *matrix;
@@ -148,22 +159,22 @@ fn trianglize<const N: usize>(matrix: &[[f64; N]; N]) -> i32 {
     sign
 }
 
-fn fit_curve<const N: usize, const NCoeffs: usize>(
+fn fit_curve<const N: usize, const NCOEFFS: usize>(
     order: i32,
     px: &[f64; N],
     py: &[f64; N],
-) -> [f64; NCoeffs] {
+) -> [f64; NCOEFFS] {
-    let mut coeffs = [0f64; NCoeffs];
+    let mut coeffs = [0f64; NCOEFFS];
 
-    if NCoeffs != (order + 1) as usize {
+    if NCOEFFS != (order + 1) as usize {
         panic!(
             "Invalid coefficients length, expected {}, but got {}",
             order + 1,
-            NCoeffs
+            NCOEFFS
         );
     }
 
-    if NCoeffs > MAX_ORDER || NCoeffs < 2 {
+    if NCOEFFS > MAX_ORDER || NCOEFFS < 2 {
         panic!("Matrix size out of bounds");
     }
 
@@ -177,27 +188,27 @@ fn fit_curve<const N: usize, const NCoeffs: usize>(
     for i in 0..N {
         let x = px[i];
         let y = py[i];
-        for j in 0..NCoeffs * 2 - 1 {
+        for j in 0..NCOEFFS * 2 - 1 {
             s[j] += curve_fit_power(x, j as u32);
         }
-        for j in 0..NCoeffs {
+        for j in 0..NCOEFFS {
             t[j] += y * curve_fit_power(x, j as u32);
         }
     }
 
     //Master matrix LHS of linear equation
-    let mut matrix = [[0f64; NCoeffs]; NCoeffs];
+    let mut matrix = [[0f64; NCOEFFS]; NCOEFFS];
 
-    for i in 0..NCoeffs {
+    for i in 0..NCOEFFS {
-        for j in 0..NCoeffs {
+        for j in 0..NCOEFFS {
             matrix[i][j] = s[i + j];
         }
     }
 
     let denom = det(&matrix);
 
-    for i in 0..NCoeffs {
+    for i in 0..NCOEFFS {
-        coeffs[NCoeffs - i - 1] = det(&sub_col(&matrix, &t, i, NCoeffs)) / denom;
+        coeffs[NCOEFFS - i - 1] = det(&sub_col(&matrix, &t, i, NCOEFFS)) / denom;
     }
 
     coeffs
@@ -218,27 +229,27 @@ pub fn linearize(point: f32, coefficients: &[f32; 4]) -> f32 {
 ///
 ///	Outputs:
 ///		linearization fit coefficients for X and Y
-pub fn linearize_calibration(in_x: [f32; 17], in_y: [f32; 17]) -> LinearizeCalibrationOutput {
+pub fn linearize_calibration(in_x: &[f64; 17], in_y: &[f64; 17]) -> LinearizeCalibrationOutput {
     let mut fit_points_x = [0f64; 5];
     let mut fit_points_y = [0f64; 5];
 
-    fit_points_x[0] = in_x[8 + 1] as f64;
+    fit_points_x[0] = in_x[8 + 1];
-    fit_points_x[1] = (in_x[6 + 1] as f64 + in_x[10 + 1] as f64) / 2.0f64;
+    fit_points_x[1] = (in_x[6 + 1] + in_x[10 + 1]) / 2.0f64;
-    fit_points_x[2] = in_x[0] as f64;
+    fit_points_x[2] = in_x[0];
-    fit_points_x[3] = (in_x[2 + 1] as f64 + in_x[14 + 1] as f64) / 2.0f64;
+    fit_points_x[3] = (in_x[2 + 1] + in_x[14 + 1]) / 2.0f64;
-    fit_points_x[4] = in_x[0 + 1] as f64;
+    fit_points_x[4] = in_x[0 + 1];
 
-    fit_points_y[0] = in_y[12 + 1] as f64;
+    fit_points_y[0] = in_y[12 + 1];
-    fit_points_y[1] = (in_y[10 + 1] as f64 + in_y[14 + 1] as f64) / 2.0f64;
+    fit_points_y[1] = (in_y[10 + 1] + in_y[14 + 1]) / 2.0f64;
-    fit_points_y[2] = in_y[0] as f64;
+    fit_points_y[2] = in_y[0];
-    fit_points_y[3] = (in_y[6 + 1] as f64 + in_y[2 + 1] as f64) / 2.0f64;
+    fit_points_y[3] = (in_y[6 + 1] + in_y[2 + 1]) / 2.0f64;
-    fit_points_y[4] = in_y[4 + 1] as f64;
+    fit_points_y[4] = in_y[4 + 1];
 
     let x_output: [f64; 5] = [27.5, 53.2537879754, 127.5, 201.7462120246, 227.5];
     let y_output: [f64; 5] = [27.5, 53.2537879754, 127.5, 201.7462120246, 227.5];
 
-    let mut fit_coeffs_x = fit_curve::<5, N_COEFFS>(FIT_ORDER as i32, &fit_points_x, &x_output);
+    let mut fit_coeffs_x = fit_curve::<5, NUM_COEFFS>(FIT_ORDER as i32, &fit_points_x, &x_output);
-    let mut fit_coeffs_y = fit_curve::<5, N_COEFFS>(FIT_ORDER as i32, &fit_points_y, &y_output);
+    let mut fit_coeffs_y = fit_curve::<5, NUM_COEFFS>(FIT_ORDER as i32, &fit_points_y, &y_output);
 
     let x_zero_error = linearize(fit_points_x[2] as f32, &fit_coeffs_x.map(|e| e as f32));
     let y_zero_error = linearize(fit_points_y[2] as f32, &fit_coeffs_y.map(|e| e as f32));
@@ -250,8 +261,8 @@ pub fn linearize_calibration(in_x: [f32; 17], in_y: [f32; 17]) -> LinearizeCalib
     let mut out_y = [0f32; NO_OF_NOTCHES];
 
     for i in 0..=NO_OF_NOTCHES {
-        out_x[i] = linearize(in_x[i], &fit_coeffs_x.map(|e| e as f32));
+        out_x[i] = linearize(in_x[i] as f32, &fit_coeffs_x.map(|e| e as f32));
-        out_y[i] = linearize(in_y[i], &fit_coeffs_y.map(|e| e as f32));
+        out_y[i] = linearize(in_y[i] as f32, &fit_coeffs_y.map(|e| e as f32));
     }
 
     LinearizeCalibrationOutput {