From baa1ab123528daf90cd156ef745692af2cc3efff Mon Sep 17 00:00:00 2001 From: Naxdy Date: Fri, 22 Mar 2024 21:38:10 +0100 Subject: [PATCH] begin implementing stick filter algorithms --- src/filter.rs | 82 ++++++++++++++++++++++++++++++++++++++ src/input.rs | 106 +++++++++++++++++++++++++++++++++++++++++++------- src/main.rs | 1 + src/stick.rs | 77 ++++++++++++++++++++---------------- 4 files changed, 218 insertions(+), 48 deletions(-) create mode 100644 src/filter.rs diff --git a/src/filter.rs b/src/filter.rs new file mode 100644 index 0000000..52782af --- /dev/null +++ b/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) +} diff --git a/src/input.rs b/src/input.rs index fc37f58..e2cfcef 100644 --- a/src/input.rs +++ b/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 = Signal::new() pub struct ControllerConfig { #[packed_field(size_bits = "8")] pub config_version: u8, - #[packed_field(size_bits = "32")] - pub ax_waveshaping: PackedFloat, - #[packed_field(size_bits = "32")] - pub ay_waveshaping: PackedFloat, - #[packed_field(size_bits = "32")] - pub cx_waveshaping: PackedFloat, - #[packed_field(size_bits = "32")] - pub cy_waveshaping: PackedFloat, + #[packed_field(size_bits = "8")] + pub ax_waveshaping: u8, + #[packed_field(size_bits = "8")] + pub ay_waveshaping: u8, + #[packed_field(size_bits = "8")] + pub cx_waveshaping: u8, + #[packed_field(size_bits = "8")] + 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_y = (ay_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 * adc_scale; - let raw_cstick_y = (cy_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; + 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; + + 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, diff --git a/src/main.rs b/src/main.rs index 0ea5ebb..7dc1138 100644 --- a/src/main.rs +++ b/src/main.rs @@ -4,6 +4,7 @@ #![no_std] #![no_main] +mod filter; mod gcc_hid; mod input; mod stick; diff --git a/src/stick.rs b/src/stick.rs index 0789af1..e871e0f 100644 --- a/src/stick.rs +++ b/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_y: [f32; N_COEFFS], + pub fit_coeffs_x: [f32; NUM_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_y: [f64; N_COEFFS], + pub fit_coeffs_x: [f64; NUM_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( matrix: &[[f64; N]; N], t: &[f64; MAX_ORDER], @@ -96,24 +99,32 @@ fn sub_col( 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(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(matrix: &[[f64; N]; N]) -> i32 { let mut sign = 1; let mut matrix = *matrix; @@ -148,22 +159,22 @@ fn trianglize(matrix: &[[f64; N]; N]) -> i32 { sign } -fn fit_curve( +fn fit_curve( order: i32, px: &[f64; N], py: &[f64; N], -) -> [f64; NCoeffs] { - let mut coeffs = [0f64; NCoeffs]; +) -> [f64; 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( 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 j in 0..NCoeffs { + for i in 0..NCOEFFS { + for j in 0..NCOEFFS { matrix[i][j] = s[i + j]; } } let denom = det(&matrix); - for i in 0..NCoeffs { - coeffs[NCoeffs - i - 1] = det(&sub_col(&matrix, &t, i, NCoeffs)) / denom; + for i in 0..NCOEFFS { + 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[1] = (in_x[6 + 1] as f64 + in_x[10 + 1] as f64) / 2.0f64; - fit_points_x[2] = in_x[0] as f64; - fit_points_x[3] = (in_x[2 + 1] as f64 + in_x[14 + 1] as f64) / 2.0f64; - fit_points_x[4] = in_x[0 + 1] as f64; + fit_points_x[0] = in_x[8 + 1]; + fit_points_x[1] = (in_x[6 + 1] + in_x[10 + 1]) / 2.0f64; + fit_points_x[2] = in_x[0]; + fit_points_x[3] = (in_x[2 + 1] + in_x[14 + 1]) / 2.0f64; + fit_points_x[4] = in_x[0 + 1]; - fit_points_y[0] = in_y[12 + 1] as f64; - fit_points_y[1] = (in_y[10 + 1] as f64 + in_y[14 + 1] as f64) / 2.0f64; - fit_points_y[2] = in_y[0] as f64; - fit_points_y[3] = (in_y[6 + 1] as f64 + in_y[2 + 1] as f64) / 2.0f64; - fit_points_y[4] = in_y[4 + 1] as f64; + fit_points_y[0] = in_y[12 + 1]; + fit_points_y[1] = (in_y[10 + 1] + in_y[14 + 1]) / 2.0f64; + fit_points_y[2] = in_y[0]; + fit_points_y[3] = (in_y[6 + 1] + in_y[2 + 1]) / 2.0f64; + 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_y = fit_curve::<5, N_COEFFS>(FIT_ORDER as i32, &fit_points_y, &y_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, 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_y[i] = linearize(in_y[i], &fit_coeffs_y.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] as f32, &fit_coeffs_y.map(|e| e as f32)); } LinearizeCalibrationOutput {