Naxdy
f0d99a234b
All checks were successful
Publish nightly release / build (push) Successful in 2m58s
191 lines
6.8 KiB
Rust
191 lines
6.8 KiB
Rust
use defmt::Format;
|
|
use libm::{fminf, powf};
|
|
|
|
use crate::input::{ControllerConfig, Stick};
|
|
|
|
/// 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_thresh: 1.,
|
|
accel_thresh: 3.,
|
|
x_smoothing: 0.0,
|
|
y_smoothing: 0.0,
|
|
c_xsmoothing: 0.0,
|
|
c_ysmoothing: 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,
|
|
}
|
|
|
|
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.
|
|
}
|
|
}
|
|
|
|
fn vel_damp_from_snapback(snapback: i8) -> f32 {
|
|
match snapback {
|
|
a if a >= 0 => 0.125 * powf(2., (snapback - 4) as f32 / 3.0),
|
|
_ => 1. - 0.25 * powf(2., (snapback + 4) as f32 / 3.0),
|
|
}
|
|
}
|
|
|
|
#[derive(Clone, Debug, Default, Format)]
|
|
pub struct FilterGains {
|
|
/// What's the max stick distance from the center
|
|
pub max_stick: f32,
|
|
/// 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,
|
|
/// 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,
|
|
/// 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,
|
|
/// 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
|
|
/// If the timesteps are *really small* then it may need to be increased to get
|
|
/// above the noise floor. Or some combination of filtering and playing with
|
|
/// the thresholds.
|
|
pub vel_thresh: f32, //1 default for 1.2ms timesteps, larger for bigger timesteps
|
|
pub accel_thresh: f32, //5 default for 1.2ms timesteps, larger for bigger timesteps
|
|
/// 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,
|
|
/// Same thing but for C-stick
|
|
pub c_xsmoothing: f32,
|
|
pub c_ysmoothing: f32,
|
|
}
|
|
|
|
impl FilterGains {
|
|
/// Returns filter gains for 1000Hz polling rate
|
|
pub fn normalize_gains(&self, controller_config: &ControllerConfig) -> Self {
|
|
let mut gains = self.clone();
|
|
|
|
gains.x_vel_damp = vel_damp_from_snapback(controller_config.x_snapback);
|
|
gains.y_vel_damp = vel_damp_from_snapback(controller_config.y_snapback);
|
|
|
|
gains.x_smoothing = controller_config.x_smoothing as f32 / 10.;
|
|
gains.y_smoothing = controller_config.y_smoothing as f32 / 10.;
|
|
|
|
gains.c_xsmoothing = controller_config.c_xsmoothing as f32 / 10.;
|
|
gains.c_ysmoothing = controller_config.c_ysmoothing as f32 / 10.;
|
|
|
|
// The below is assuming the sticks to be polled at 1000Hz
|
|
let time_factor = 1.0 / 1.2;
|
|
let time_divisor = 1.2 / 1.0;
|
|
|
|
let vel_thresh = 1.0 / (gains.vel_thresh * time_factor);
|
|
let accel_thresh = 1.0 / (gains.accel_thresh * time_factor);
|
|
|
|
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.x_snapback {
|
|
a if a >= 0 => time_factor,
|
|
_ => 1.0,
|
|
},
|
|
y_vel_damp: gains.y_vel_damp
|
|
* match controller_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),
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn run_kalman(
|
|
x_z: f32,
|
|
y_z: f32,
|
|
controller_config: &ControllerConfig,
|
|
filter_gains: &FilterGains,
|
|
) -> (f32, f32) {
|
|
todo!()
|
|
}
|
|
|
|
/// 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,
|
|
x_waveshaping: u8,
|
|
y_waveshaping: u8,
|
|
waveshaping_values: &mut WaveshapingValues,
|
|
filter_gains: &FilterGains,
|
|
) -> (f32, f32) {
|
|
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_smooth = 0.5 * (x_vel + waveshaping_values.old_x_vel);
|
|
let y_vel_smooth = 0.5 * (y_vel + waveshaping_values.old_y_vel);
|
|
|
|
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)
|
|
}
|