forked from NaxdyOrg/NaxGCC-FW
begin implementing stick filter algorithms
This commit is contained in:
parent
5244f1a75e
commit
baa1ab1235
4 changed files with 218 additions and 48 deletions
82
src/filter.rs
Normal file
82
src/filter.rs
Normal file
|
|
@ -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)
|
||||
}
|
||||
106
src/input.rs
106
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")]
|
||||
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,
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
|
||||
#![no_std]
|
||||
#![no_main]
|
||||
mod filter;
|
||||
mod gcc_hid;
|
||||
mod input;
|
||||
mod stick;
|
||||
|
|
|
|||
77
src/stick.rs
77
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<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] {
|
||||
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<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 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 {
|
||||
|
|
|
|||
Loading…
Reference in a new issue