diff --git a/src/config.rs b/src/config.rs index b7c580f..aea4871 100644 --- a/src/config.rs +++ b/src/config.rs @@ -11,7 +11,7 @@ use embassy_rp::{ use packed_struct::{derive::PackedStruct, PackedStruct}; use crate::{ - packed_float::{PackedFloat, ToPackedFloatArray}, + helpers::{PackedFloat, ToPackedFloatArray}, stick::{NotchStatus, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES}, ADDR_OFFSET, FLASH_SIZE, }; diff --git a/src/filter.rs b/src/filter.rs index 90c880c..8ee15ac 100644 --- a/src/filter.rs +++ b/src/filter.rs @@ -1,33 +1,68 @@ use defmt::Format; -use libm::{fminf, powf}; +use libm::{fmaxf, fminf, powf}; -use crate::config::ControllerConfig; +use crate::{ + config::{ControllerConfig, StickConfig}, + helpers::XyValuePair, +}; + +macro_rules! run_kalman_on_axis { + ($self:ident, $axis:ident, $snapback:expr, $vel_weight1:ident, $vel_weight2:ident, $old_pos_filt:ident, $filter_gains:ident, $old_vel_filt:ident, $old_pos_diff:ident, $accel:ident, $vel_smooth:ident, $stick_distance6:ident) => { + if $snapback > 0 { + $self.vel_filt.$axis = $vel_weight1 * $self.vel.$axis + + (1. - $filter_gains.vel_decay.$axis) * $vel_weight2 * $old_vel_filt.$axis + + $filter_gains.vel_pos_factor.$axis * $old_pos_diff.$axis; + + let pos_weight_vel_acc = 1. + - fminf( + 1., + $vel_smooth.$axis * $vel_smooth.$axis * $filter_gains.vel_thresh + + $accel.$axis * $accel.$axis * $filter_gains.accel_thresh, + ); + let pos_weight1 = fmaxf(pos_weight_vel_acc, $stick_distance6); + let pos_weight2 = 1. - pos_weight1; + + $self.pos_filt.$axis = pos_weight1 * $self.pos.$axis + + pos_weight2 + * ($old_pos_filt.$axis + + (1. - $filter_gains.vel_damp.$axis) * $self.vel_filt.$axis) + } else if $snapback < 0 { + let lpf = $old_pos_filt.$axis * $filter_gains.vel_damp.$axis + + $self.pos.$axis * (1. - $filter_gains.vel_damp.$axis); + + let pos_weight_vel_acc = 1. + - fminf( + 1., + $vel_smooth.$axis * $vel_smooth.$axis * $filter_gains.vel_thresh + + $accel.$axis * $accel.$axis * $filter_gains.accel_thresh, + ); + let pos_weight1 = fmaxf(pos_weight_vel_acc, $stick_distance6); + let pos_weight2 = 1. - pos_weight1; + + $self.pos_filt.$axis = pos_weight1 * $self.pos.$axis + pos_weight2 * lpf; + } else { + $self.pos_filt.$axis = $self.pos.$axis; + } + }; +} /// Filter gains for 800Hz, the ones for 1000Hz are provided by `get_norm_gains` pub const FILTER_GAINS: FilterGains = FilterGains { max_stick: 100., - x_vel_decay: 0.1, - y_vel_decay: 0.1, - x_vel_pos_factor: 0.01, - y_vel_pos_factor: 0.01, - x_vel_damp: 0.125, - y_vel_damp: 0.125, + vel_decay: XyValuePair { x: 0.1, y: 0.1 }, + vel_pos_factor: XyValuePair { x: 0.01, y: 0.01 }, + vel_damp: XyValuePair { x: 0.125, y: 0.125 }, vel_thresh: 1., accel_thresh: 3., - x_smoothing: 0.0, - y_smoothing: 0.0, - c_xsmoothing: 0.0, - c_ysmoothing: 0.0, + smoothing: XyValuePair { x: 0.0, y: 0.0 }, + c_smoothing: XyValuePair { x: 0.0, y: 0.0 }, }; #[derive(Debug, Clone, Default)] 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, + pub old_pos: XyValuePair, + pub old_vel: XyValuePair, + pub old_out: XyValuePair, } fn calc_waveshaping_mult(setting: u8) -> f32 { @@ -54,17 +89,14 @@ pub struct FilterGains { /// filtered velocity terms /// how fast the filtered velocity falls off in the absence of stick movement. /// Probably don't touch this. - pub x_vel_decay: f32, //0.1 default for 1.2ms timesteps, larger for bigger timesteps - pub y_vel_decay: f32, + pub vel_decay: XyValuePair, //0.1 default for 1.2ms timesteps, larger for bigger timesteps /// how much the current position disagreement impacts the filtered velocity. /// Probably don't touch this. - pub x_vel_pos_factor: f32, //0.01 default for 1.2ms timesteps, larger for bigger timesteps - pub y_vel_pos_factor: f32, + pub vel_pos_factor: XyValuePair, //0.01 default for 1.2ms timesteps, larger for bigger timesteps /// how much to ignore filtered velocity when computing the new stick position. /// DO CHANGE THIS /// Higher gives shorter rise times and slower fall times (more pode, less snapback) - pub x_vel_damp: f32, //0.125 default for 1.2ms timesteps, smaller for bigger timesteps - pub y_vel_damp: f32, + pub vel_damp: XyValuePair, //0.125 default for 1.2ms timesteps, smaller for bigger timesteps /// speed and accel thresholds below which we try to follow the stick better /// These may need tweaking according to how noisy the signal is /// If it's noisier, we may need to add additional filtering @@ -76,11 +108,9 @@ pub struct FilterGains { /// This just applies a low-pass filter. /// The purpose is to provide delay for single-axis ledgedashes. /// Must be between 0 and 1. Larger = more smoothing and delay. - pub x_smoothing: f32, - pub y_smoothing: f32, + pub smoothing: XyValuePair, /// Same thing but for C-stick - pub c_xsmoothing: f32, - pub c_ysmoothing: f32, + pub c_smoothing: XyValuePair, } impl FilterGains { @@ -88,14 +118,14 @@ impl FilterGains { pub fn get_normalized_gains(&self, controller_config: &ControllerConfig) -> Self { let mut gains = self.clone(); - gains.x_vel_damp = vel_damp_from_snapback(controller_config.astick_config.x_snapback); - gains.y_vel_damp = vel_damp_from_snapback(controller_config.astick_config.y_snapback); + gains.vel_damp.x = vel_damp_from_snapback(controller_config.astick_config.x_snapback); + gains.vel_damp.y = vel_damp_from_snapback(controller_config.astick_config.y_snapback); - gains.x_smoothing = controller_config.astick_config.x_smoothing as f32 / 10.; - gains.y_smoothing = controller_config.astick_config.y_smoothing as f32 / 10.; + gains.smoothing.x = controller_config.astick_config.x_smoothing as f32 / 10.; + gains.smoothing.y = controller_config.astick_config.y_smoothing as f32 / 10.; - gains.c_xsmoothing = controller_config.cstick_config.x_smoothing as f32 / 10.; - gains.c_ysmoothing = controller_config.cstick_config.y_smoothing as f32 / 10.; + gains.c_smoothing.x = controller_config.cstick_config.x_smoothing as f32 / 10.; + gains.c_smoothing.y = controller_config.cstick_config.y_smoothing as f32 / 10.; // The below is assuming the sticks to be polled at 1000Hz let time_factor = 1.0 / 1.2; @@ -106,37 +136,149 @@ impl FilterGains { FilterGains { max_stick: gains.max_stick * gains.max_stick, - x_vel_decay: gains.x_vel_decay * time_factor, - y_vel_decay: gains.y_vel_decay * time_factor, - x_vel_pos_factor: gains.x_vel_pos_factor * time_factor, - y_vel_pos_factor: gains.y_vel_pos_factor * time_factor, - x_vel_damp: gains.x_vel_damp - * match controller_config.astick_config.x_snapback { - a if a >= 0 => time_factor, - _ => 1.0, - }, - y_vel_damp: gains.y_vel_damp - * match controller_config.astick_config.y_snapback { - a if a >= 0 => time_factor, - _ => 1.0, - }, + vel_decay: XyValuePair { + x: gains.vel_decay.x * time_factor, + y: gains.vel_decay.y * time_factor, + }, + vel_pos_factor: XyValuePair { + x: gains.vel_pos_factor.x * time_factor, + y: gains.vel_pos_factor.y * time_factor, + }, + vel_damp: XyValuePair { + x: gains.vel_damp.x + * match controller_config.astick_config.x_snapback { + a if a >= 0 => time_factor, + _ => 1.0, + }, + y: gains.vel_damp.y + * match controller_config.astick_config.y_snapback { + a if a >= 0 => time_factor, + _ => 1.0, + }, + }, vel_thresh, accel_thresh, - x_smoothing: powf(1.0 - gains.x_smoothing, time_divisor), - y_smoothing: powf(1.0 - gains.y_smoothing, time_divisor), - c_xsmoothing: powf(1.0 - gains.c_xsmoothing, time_divisor), - c_ysmoothing: powf(1.0 - gains.c_ysmoothing, time_divisor), + smoothing: XyValuePair { + x: powf(1.0 - gains.smoothing.x, time_divisor), + y: powf(1.0 - gains.smoothing.y, time_divisor), + }, + c_smoothing: XyValuePair { + x: powf(1.0 - gains.c_smoothing.x, time_divisor), + y: powf(1.0 - gains.c_smoothing.y, time_divisor), + }, } } } -pub fn run_kalman( - x_z: f32, - y_z: f32, - controller_config: &ControllerConfig, - filter_gains: &FilterGains, -) -> (f32, f32) { - todo!() +#[derive(Clone, Debug, Format, Default)] +pub struct KalmanState { + pos: XyValuePair, + vel: XyValuePair, + vel_filt: XyValuePair, + pos_filt: XyValuePair, +} + +impl KalmanState { + // runs kalman filter + pub fn run_kalman( + &mut self, + x_z: f32, + y_z: f32, + stick_config: &StickConfig, + filter_gains: &FilterGains, + ) -> (f32, f32) { + let old_pos = self.pos; + let old_vel = self.vel; + let old_vel_filt = self.vel_filt; + let old_pos_filt = self.pos_filt; + + self.pos.x = x_z; + self.pos.y = y_z; + self.vel.x = x_z - old_pos.x; + self.vel.y = y_z - old_pos.y; + + let vel_smooth = XyValuePair { + x: 0.5 * (self.vel.x + old_vel.x), + y: 0.5 * (self.vel.y + old_vel.y), + }; + let accel = XyValuePair { + x: self.vel.x - old_vel.x, + y: self.vel.y - old_vel.y, + }; + let old_pos_diff = XyValuePair { + x: old_pos.x - old_pos_filt.x, + y: old_pos.y - old_pos_filt.y, + }; + + let stick_distance2 = fminf( + filter_gains.max_stick, + self.pos.x * self.pos.x + self.pos.y * self.pos.y, + ) / filter_gains.max_stick; + let stick_distance6 = stick_distance2 * stick_distance2 * stick_distance2; + + let vel_weight1 = stick_distance2; + let vel_weight2 = 1. - vel_weight1; + + //modified velocity to feed into our kalman filter. + //We don't actually want an accurate model of the velocity, we want to suppress snapback without adding delay + //term 1: weight current velocity according to r^2 + //term 2: the previous filtered velocity, weighted the opposite and also set to decay + //term 3: a corrective factor based on the disagreement between real and filtered position + + //the current position weight used for the filtered position is whatever is larger of + // a) 1 minus the sum of the squares of + // 1) the smoothed velocity divided by the velocity threshold + // 2) the acceleration divided by the accel threshold + // b) stick r^6 + //When the stick is moving slowly, we want to weight it highly, in order to achieve + // quick control for inputs such as tilts. We lock out using both velocity and + // acceleration in order to rule out snapback. + //When the stick is near the rim, we also want instant response, and we know snapback + // doesn't reach the rim. + + //In calculating the filtered stick position, we have the following components + //term 1: current position, weighted according to the above weight + //term 2: a predicted position based on the filtered velocity and previous filtered position, + // with the filtered velocity damped, and the overall term weighted inverse of the previous term + //term 3: the integral error correction term + + //But if we xSnapback or ySnapback is zero, we skip the calculation + run_kalman_on_axis!( + self, + x, + stick_config.x_snapback, + vel_weight1, + vel_weight2, + old_pos_filt, + filter_gains, + old_vel_filt, + old_pos_diff, + accel, + vel_smooth, + stick_distance6 + ); + + run_kalman_on_axis!( + self, + y, + stick_config.y_snapback, + vel_weight1, + vel_weight2, + old_pos_filt, + filter_gains, + old_vel_filt, + old_pos_diff, + accel, + vel_smooth, + stick_distance6 + ); + + self.get_xy() + } + + pub fn get_xy(&self) -> (f32, f32) { + (self.pos_filt.x, self.pos_filt.y) + } } /// This simulates an idealized sort of pode: @@ -160,11 +302,11 @@ pub fn run_waveshaping( let x_factor = calc_waveshaping_mult(x_waveshaping); let y_factor = calc_waveshaping_mult(y_waveshaping); - let x_vel = x_pos - waveshaping_values.old_x_pos; - let y_vel = y_pos - waveshaping_values.old_y_pos; + let x_vel = x_pos - waveshaping_values.old_pos.x; + let y_vel = y_pos - waveshaping_values.old_pos.y; - 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_vel_smooth = 0.5 * (x_vel + waveshaping_values.old_vel.x); + let y_vel_smooth = 0.5 * (y_vel + waveshaping_values.old_vel.y); let old_x_pos_weight = fminf( 1., @@ -177,15 +319,15 @@ pub fn run_waveshaping( ); 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; + let x_out = x_pos * new_x_pos_weight + waveshaping_values.old_out.x * old_x_pos_weight; + let y_out = y_pos * new_y_pos_weight + waveshaping_values.old_out.y * 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; + waveshaping_values.old_pos.x = x_pos; + waveshaping_values.old_pos.y = y_pos; + waveshaping_values.old_vel.x = x_vel_smooth; + waveshaping_values.old_vel.y = y_vel_smooth; + waveshaping_values.old_out.x = x_out; + waveshaping_values.old_out.y = y_out; (x_out, y_out) } diff --git a/src/packed_float.rs b/src/helpers.rs similarity index 91% rename from src/packed_float.rs rename to src/helpers.rs index fb47cae..9380dc8 100644 --- a/src/packed_float.rs +++ b/src/helpers.rs @@ -46,3 +46,9 @@ impl Deref for PackedFloat { &self.0 } } + +#[derive(Debug, Clone, Format, Default, Copy)] +pub struct XyValuePair { + pub x: T, + pub y: T, +} diff --git a/src/input.rs b/src/input.rs index 0b46852..86074e4 100644 --- a/src/input.rs +++ b/src/input.rs @@ -16,7 +16,7 @@ use libm::{fmaxf, fminf}; use crate::{ config::ControllerConfig, - filter::{run_kalman, run_waveshaping, FilterGains, WaveshapingValues, FILTER_GAINS}, + filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS}, gcc_hid::GcReport, stick::{linearize, notch_remap, StickParams}, FLASH_SIZE, @@ -125,6 +125,7 @@ async fn update_stick_states< cstick_waveshaping_values: &mut WaveshapingValues, old_stick_pos: &mut StickPositions, raw_stick_values: &mut RawStickValues, + kalman_state: &mut KalmanState, ) -> StickState { let mut adc_count = 0u32; let mut ax_sum = 0u32; @@ -203,7 +204,8 @@ async fn update_stick_states< 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 (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( x_pos_filt, @@ -215,9 +217,9 @@ async fn update_stick_states< ); let pos_x: f32 = - filter_gains.x_smoothing * shaped_x + (1.0 - filter_gains.x_smoothing) * old_stick_pos.x; + filter_gains.smoothing.x * shaped_x + (1.0 - filter_gains.smoothing.x) * old_stick_pos.x; let pos_y = - filter_gains.y_smoothing * shaped_y + (1.0 - filter_gains.y_smoothing) * old_stick_pos.y; + filter_gains.smoothing.y * shaped_y + (1.0 - filter_gains.smoothing.y) * old_stick_pos.y; old_stick_pos.x = pos_x; old_stick_pos.y = pos_y; @@ -235,9 +237,9 @@ async fn update_stick_states< old_stick_pos.cx = shaped_cx; old_stick_pos.cy = shaped_cy; - let x_weight_1 = filter_gains.c_xsmoothing; + 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_ysmoothing; + 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; @@ -401,6 +403,7 @@ pub async fn input_loop( let mut old_stick_pos = StickPositions::default(); let mut cstick_waveshaping_values = WaveshapingValues::default(); let mut controlstick_waveshaping_values = WaveshapingValues::default(); + let mut kalman_state = KalmanState::default(); loop { current_stick_state = update_stick_states( @@ -416,6 +419,7 @@ pub async fn input_loop( &mut cstick_waveshaping_values, &mut old_stick_pos, &mut raw_stick_values, + &mut kalman_state, ) .await; diff --git a/src/main.rs b/src/main.rs index e4501ce..6ef5e68 100644 --- a/src/main.rs +++ b/src/main.rs @@ -7,8 +7,8 @@ mod config; mod filter; mod gcc_hid; +mod helpers; mod input; -mod packed_float; mod stick; use defmt::{debug, info}; diff --git a/src/stick.rs b/src/stick.rs index 9eea967..11c69f8 100644 --- a/src/stick.rs +++ b/src/stick.rs @@ -7,8 +7,8 @@ use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf}; use crate::{ config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS}, + helpers::ToRegularArray, input::Stick, - packed_float::ToRegularArray, }; /// fit order for the linearization