forked from NaxdyOrg/NaxGCC-FW
feat: implement stick calibration logic
This commit is contained in:
parent
fe4afce386
commit
f0d99a234b
GPG key ID:
CC15075846BCE91B
9 changed files with 555 additions and 266 deletions
7
Cargo.lock
generated
7
Cargo.lock
generated
|
|
@ -897,6 +897,7 @@ dependencies = [
|
|||
"portable-atomic",
|
||||
"rand",
|
||||
"static_cell",
|
||||
"tiny_sort",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -1427,6 +1428,12 @@ dependencies = [
|
|||
"crunchy",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "tiny_sort"
|
||||
version = "1.0.5"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "c3bbd7912d5028a8f218a772a794fee0104b46ea1f6e747ff0a4fdbb5dc024a6"
|
||||
|
||||
[[package]]
|
||||
name = "typenum"
|
||||
version = "1.17.0"
|
||||
|
|
|
|||
|
|
@ -51,6 +51,9 @@ panic-probe = { version = "0.3", features = ["print-defmt"] }
|
|||
packed_struct = { version = "0.10.1", default_features = false }
|
||||
format_no_std = "1.0.2"
|
||||
rand = { version = "0.8.5", default-features = false }
|
||||
tiny_sort = { version = "1.0.5", default-features = false, features = [
|
||||
"unstable",
|
||||
] }
|
||||
|
||||
# cargo build/run
|
||||
[profile.dev]
|
||||
|
|
|
|||
|
|
@ -78,7 +78,7 @@
|
|||
"--target=${CARGO_BUILD_TARGET}"
|
||||
];
|
||||
|
||||
# inherit RUSTFLAGS;
|
||||
DEFMT_LOG = "warn";
|
||||
};
|
||||
|
||||
devShells.default = pkgs.mkShell {
|
||||
|
|
|
|||
138
src/filter.rs
138
src/filter.rs
|
|
@ -1,8 +1,23 @@
|
|||
use libm::fminf;
|
||||
use defmt::Format;
|
||||
use libm::{fminf, powf};
|
||||
|
||||
use crate::{
|
||||
input::{ControllerConfig, Stick},
|
||||
stick::FilterGains,
|
||||
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)]
|
||||
|
|
@ -25,6 +40,105 @@ fn calc_waveshaping_mult(setting: u8) -> f32 {
|
|||
}
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -38,26 +152,20 @@ fn calc_waveshaping_mult(setting: u8) -> f32 {
|
|||
pub fn run_waveshaping(
|
||||
x_pos: f32,
|
||||
y_pos: f32,
|
||||
which_stick: Stick,
|
||||
x_waveshaping: u8,
|
||||
y_waveshaping: u8,
|
||||
waveshaping_values: &mut WaveshapingValues,
|
||||
controller_config: &ControllerConfig,
|
||||
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 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,
|
||||
|
|
|
|||
|
|
@ -1,25 +0,0 @@
|
|||
use core::slice::from_raw_parts;
|
||||
|
||||
use rp2040_flash::flash::{flash_range_erase, flash_range_program};
|
||||
|
||||
const XIP_BASE: u32 = 0x10000000;
|
||||
const FLASH_PAGE_SIZE: usize = 1usize << 8;
|
||||
const FLASH_SECTOR_SIZE: u32 = 1u32 << 12;
|
||||
|
||||
const FLASH_TARGET_OFFSET: u32 = 256 * 1024;
|
||||
|
||||
pub fn read_from_flash() -> u8 {
|
||||
let flash_target_contents = (XIP_BASE + FLASH_TARGET_OFFSET) as *const u8;
|
||||
|
||||
let d = unsafe { from_raw_parts(flash_target_contents, FLASH_PAGE_SIZE) };
|
||||
|
||||
d[0]
|
||||
}
|
||||
|
||||
pub unsafe fn write_to_flash(b: u8) {
|
||||
let mut data = [0u8; FLASH_PAGE_SIZE];
|
||||
data[0] = b;
|
||||
|
||||
flash_range_erase(FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE, true);
|
||||
flash_range_program(FLASH_TARGET_OFFSET, &data, true);
|
||||
}
|
||||
99
src/input.rs
99
src/input.rs
|
|
@ -18,10 +18,14 @@ use libm::{fmaxf, fmin, fminf};
|
|||
use packed_struct::{derive::PackedStruct, PackedStruct};
|
||||
|
||||
use crate::{
|
||||
filter::{run_waveshaping, WaveshapingValues},
|
||||
filter::{run_kalman, run_waveshaping, FilterGains, WaveshapingValues, FILTER_GAINS},
|
||||
gcc_hid::GcReport,
|
||||
stick::{linearize, notch_remap, run_kalman, FilterGains, StickParams},
|
||||
PackedFloat, ADDR_OFFSET, FLASH_SIZE,
|
||||
packed_float::{PackedFloat, ToPackedFloatArray},
|
||||
stick::{
|
||||
linearize, notch_remap, StickParams, DEFAULT_CAL_POINTS_X, DEFAULT_CAL_POINTS_Y,
|
||||
NO_OF_NOTCHES,
|
||||
},
|
||||
ADDR_OFFSET, FLASH_SIZE,
|
||||
};
|
||||
|
||||
pub static GCC_SIGNAL: Signal<CriticalSectionRawMutex, GcReport> = Signal::new();
|
||||
|
|
@ -63,6 +67,18 @@ pub struct ControllerConfig {
|
|||
pub c_xsmoothing: u8,
|
||||
#[packed_field(size_bits = "8")]
|
||||
pub c_ysmoothing: u8,
|
||||
#[packed_field(element_size_bytes = "4")]
|
||||
pub temp_cal_points_ax: [PackedFloat; 32],
|
||||
#[packed_field(element_size_bytes = "4")]
|
||||
pub temp_cal_points_ay: [PackedFloat; 32],
|
||||
#[packed_field(element_size_bytes = "4")]
|
||||
pub temp_cal_points_cx: [PackedFloat; 32],
|
||||
#[packed_field(element_size_bytes = "4")]
|
||||
pub temp_cal_points_cy: [PackedFloat; 32],
|
||||
#[packed_field(element_size_bytes = "4")]
|
||||
pub a_angles: [PackedFloat; 16],
|
||||
#[packed_field(element_size_bytes = "4")]
|
||||
pub c_angles: [PackedFloat; 16],
|
||||
}
|
||||
|
||||
impl Default for ControllerConfig {
|
||||
|
|
@ -82,10 +98,49 @@ impl Default for ControllerConfig {
|
|||
y_smoothing: 0,
|
||||
c_xsmoothing: 0,
|
||||
c_ysmoothing: 0,
|
||||
temp_cal_points_ax: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
|
||||
temp_cal_points_ay: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
|
||||
temp_cal_points_cx: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
|
||||
temp_cal_points_cy: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
|
||||
a_angles: [PackedFloat::default(); NO_OF_NOTCHES],
|
||||
c_angles: [PackedFloat::default(); NO_OF_NOTCHES],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ControllerConfig {
|
||||
pub fn from_flash_memory(
|
||||
mut flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
|
||||
) -> Result<Self, embassy_rp::flash::Error> {
|
||||
let mut controller_config_packed: <ControllerConfig as packed_struct::PackedStruct>::ByteArray = [0u8; 654]; // ControllerConfig byte size
|
||||
flash.blocking_read(ADDR_OFFSET, &mut controller_config_packed)?;
|
||||
|
||||
match ControllerConfig::unpack(&controller_config_packed).unwrap() {
|
||||
a if a.config_revision == CONTROLLER_CONFIG_REVISION => {
|
||||
info!("Controller config loaded from flash: {}", a);
|
||||
Ok(a)
|
||||
}
|
||||
a => {
|
||||
warn!("Outdated controller config detected ({:02X}), or controller config was never present, using default.", a.config_revision);
|
||||
let cfg = ControllerConfig::default();
|
||||
info!("Going to save default controller config.");
|
||||
cfg.write_to_flash(&mut flash)?;
|
||||
Ok(cfg)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn write_to_flash(
|
||||
&self,
|
||||
flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
|
||||
) -> Result<(), embassy_rp::flash::Error> {
|
||||
info!("Writing controller config to flash.");
|
||||
flash.blocking_erase(ADDR_OFFSET, ADDR_OFFSET + ERASE_SIZE as u32)?;
|
||||
flash.blocking_write(ADDR_OFFSET, &self.pack().unwrap())?;
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Default)]
|
||||
struct StickState {
|
||||
ax: u8,
|
||||
|
|
@ -253,15 +308,15 @@ 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) = 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,
|
||||
controller_config.ax_waveshaping,
|
||||
controller_config.ay_waveshaping,
|
||||
controlstick_waveshaping_values,
|
||||
controller_config,
|
||||
filter_gains,
|
||||
&filter_gains,
|
||||
);
|
||||
|
||||
let pos_x: f32 =
|
||||
|
|
@ -274,10 +329,10 @@ async fn update_stick_states<
|
|||
let (shaped_cx, shaped_cy) = run_waveshaping(
|
||||
pos_cx,
|
||||
pos_cy,
|
||||
Stick::CStick,
|
||||
controller_config.cx_waveshaping,
|
||||
controller_config.cy_waveshaping,
|
||||
cstick_waveshaping_values,
|
||||
controller_config,
|
||||
filter_gains,
|
||||
&filter_gains,
|
||||
);
|
||||
|
||||
let old_cx_pos = old_stick_pos.cx;
|
||||
|
|
@ -427,26 +482,12 @@ pub async fn input_loop(
|
|||
gcc_state.cstick_x = 127;
|
||||
gcc_state.cstick_y = 127;
|
||||
|
||||
let mut controller_config_packed = [0u8; 14]; // ControllerConfig byte size
|
||||
flash
|
||||
.blocking_read(ADDR_OFFSET, &mut controller_config_packed)
|
||||
.unwrap();
|
||||
let controller_config = ControllerConfig::from_flash_memory(&mut flash).unwrap();
|
||||
|
||||
let controller_config = match ControllerConfig::unpack(&controller_config_packed).unwrap() {
|
||||
a if a.config_revision == CONTROLLER_CONFIG_REVISION => a,
|
||||
a => {
|
||||
warn!("Outdated controller config detected ({:02X}), or controller config was never present, using default.", a.config_revision);
|
||||
let cfg = ControllerConfig::default();
|
||||
info!("Writing default controller config to flash.");
|
||||
flash
|
||||
.blocking_erase(ADDR_OFFSET, ADDR_OFFSET + ERASE_SIZE as u32)
|
||||
.unwrap();
|
||||
flash
|
||||
.blocking_write(ADDR_OFFSET, &cfg.pack().unwrap())
|
||||
.unwrap();
|
||||
cfg
|
||||
}
|
||||
};
|
||||
let (controlstick_params, cstick_params) =
|
||||
StickParams::from_controller_config(&controller_config);
|
||||
|
||||
let filter_gains = FILTER_GAINS.normalize_gains(&controller_config);
|
||||
|
||||
let stick_state_fut = async {
|
||||
let mut current_stick_state = StickState {
|
||||
|
|
|
|||
32
src/main.rs
32
src/main.rs
|
|
@ -7,11 +7,10 @@
|
|||
mod filter;
|
||||
mod gcc_hid;
|
||||
mod input;
|
||||
mod packed_float;
|
||||
mod stick;
|
||||
|
||||
use core::ops::Deref;
|
||||
|
||||
use defmt::{debug, info, Format};
|
||||
use defmt::{debug, info};
|
||||
use embassy_executor::Executor;
|
||||
use embassy_rp::{
|
||||
bind_interrupts,
|
||||
|
|
@ -26,7 +25,7 @@ use embassy_rp::{
|
|||
use gcc_hid::usb_transfer_loop;
|
||||
use gpio::{Level, Output};
|
||||
use input::input_loop;
|
||||
use packed_struct::PackedStruct;
|
||||
|
||||
use static_cell::StaticCell;
|
||||
use {defmt_rtt as _, panic_probe as _};
|
||||
|
||||
|
|
@ -37,31 +36,6 @@ static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
|
|||
const FLASH_SIZE: usize = 2 * 1024 * 1024;
|
||||
const ADDR_OFFSET: u32 = 0x100000;
|
||||
|
||||
/// wrapper type because packed_struct doesn't implement float
|
||||
/// packing by default for some reason
|
||||
#[derive(Debug, Format, Clone, Default)]
|
||||
pub struct PackedFloat(f32);
|
||||
|
||||
impl PackedStruct for PackedFloat {
|
||||
type ByteArray = [u8; 4];
|
||||
|
||||
fn pack(&self) -> packed_struct::PackingResult<Self::ByteArray> {
|
||||
Ok(self.to_be_bytes())
|
||||
}
|
||||
|
||||
fn unpack(src: &Self::ByteArray) -> packed_struct::PackingResult<Self> {
|
||||
Ok(Self(f32::from_be_bytes(*src)))
|
||||
}
|
||||
}
|
||||
|
||||
impl Deref for PackedFloat {
|
||||
type Target = f32;
|
||||
|
||||
fn deref(&self) -> &Self::Target {
|
||||
&self.0
|
||||
}
|
||||
}
|
||||
|
||||
bind_interrupts!(struct Irqs {
|
||||
USBCTRL_IRQ => InterruptHandler<USB>;
|
||||
});
|
||||
|
|
|
|||
48
src/packed_float.rs
Normal file
48
src/packed_float.rs
Normal file
|
|
@ -0,0 +1,48 @@
|
|||
use core::ops::Deref;
|
||||
use defmt::Format;
|
||||
use packed_struct::PackedStruct;
|
||||
|
||||
/// wrapper type because packed_struct doesn't implement float
|
||||
/// packing by default
|
||||
#[derive(Debug, Format, Clone, Default, Copy)]
|
||||
pub struct PackedFloat(f32);
|
||||
|
||||
pub trait ToRegularArray<const T: usize> {
|
||||
fn to_regular_array(&self) -> &[f32; T];
|
||||
}
|
||||
|
||||
impl<const T: usize> ToRegularArray<T> for [PackedFloat; T] {
|
||||
fn to_regular_array(&self) -> &[f32; T] {
|
||||
unsafe { &*(self as *const _ as *const _) }
|
||||
}
|
||||
}
|
||||
|
||||
pub trait ToPackedFloatArray<const T: usize> {
|
||||
fn to_packed_float_array(&self) -> &[PackedFloat; T];
|
||||
}
|
||||
|
||||
impl<const T: usize> ToPackedFloatArray<T> for [f32; T] {
|
||||
fn to_packed_float_array(&self) -> &[PackedFloat; T] {
|
||||
unsafe { &*(self as *const _ as *const _) }
|
||||
}
|
||||
}
|
||||
|
||||
impl PackedStruct for PackedFloat {
|
||||
type ByteArray = [u8; 4];
|
||||
|
||||
fn pack(&self) -> packed_struct::PackingResult<Self::ByteArray> {
|
||||
Ok(self.to_be_bytes())
|
||||
}
|
||||
|
||||
fn unpack(src: &Self::ByteArray) -> packed_struct::PackingResult<Self> {
|
||||
Ok(Self(f32::from_be_bytes(*src)))
|
||||
}
|
||||
}
|
||||
|
||||
impl Deref for PackedFloat {
|
||||
type Target = f32;
|
||||
|
||||
fn deref(&self) -> &Self::Target {
|
||||
&self.0
|
||||
}
|
||||
}
|
||||
467
src/stick.rs
467
src/stick.rs
|
|
@ -1,37 +1,83 @@
|
|||
// vast majority of this is taken from Phob firmware
|
||||
|
||||
use core::{f32::consts::PI, iter::Filter};
|
||||
use core::f32::consts::PI;
|
||||
|
||||
use defmt::Format;
|
||||
use libm::{atan2f, fabs, powf};
|
||||
use defmt::{debug, Format};
|
||||
use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf};
|
||||
|
||||
use crate::{
|
||||
input::{ControllerConfig, Stick},
|
||||
stick,
|
||||
packed_float::ToRegularArray,
|
||||
};
|
||||
|
||||
/// fit order for the linearization
|
||||
const FIT_ORDER: usize = 3;
|
||||
const NUM_COEFFS: usize = FIT_ORDER + 1;
|
||||
const NO_OF_NOTCHES: usize = 16;
|
||||
pub const NO_OF_NOTCHES: usize = 16;
|
||||
pub const NO_OF_CALIBRATION_POINTS: usize = 32;
|
||||
const MAX_ORDER: usize = 20;
|
||||
|
||||
/// 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,
|
||||
};
|
||||
/// 28 degrees; this is the max angular deflection of the stick.
|
||||
const MAX_STICK_ANGLE: f32 = 0.4886921906;
|
||||
|
||||
const NOTCH_STATUS_DEFAULTS: [NotchStatus; NO_OF_NOTCHES] = [
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
];
|
||||
|
||||
#[rustfmt::skip]
|
||||
pub const DEFAULT_CAL_POINTS_X: [f32; NO_OF_CALIBRATION_POINTS] = [
|
||||
0.3010610568,0.3603937084,// right
|
||||
0.3010903951,0.3000194135,
|
||||
0.3005567843,0.3471911134,// up right
|
||||
0.3006904343,0.3009976295,
|
||||
0.3000800899,0.300985051,// up
|
||||
0.3001020858,0.300852804,
|
||||
0.3008746305,0.2548450139,// up left
|
||||
0.3001434092,0.3012600593,
|
||||
0.3011594091,0.2400535218,// left
|
||||
0.3014621077,0.3011248469,
|
||||
0.3010860944,0.2552106305,// down left
|
||||
0.3002197989,0.3001679513,
|
||||
0.3004438517,0.300486505,// down
|
||||
0.3002766984,0.3012828579,
|
||||
0.3014959877,0.346512936,// down right
|
||||
0.3013398149,0.3007809916
|
||||
];
|
||||
|
||||
#[rustfmt::skip]
|
||||
pub const DEFAULT_CAL_POINTS_Y: [f32; NO_OF_CALIBRATION_POINTS] = [
|
||||
0.300092277, 0.3003803475,// right
|
||||
0.3002205792,0.301004752,
|
||||
0.3001241394,0.3464200104,// up right
|
||||
0.3001331245,0.3011881186,
|
||||
0.3010685972,0.3606900641,// up
|
||||
0.3001520488,0.3010662947,
|
||||
0.3008837105,0.3461478452,// up left
|
||||
0.3011732026,0.3007367683,
|
||||
0.3011345742,0.3000566197,// left
|
||||
0.3006843288,0.3009673425,
|
||||
0.3011228978,0.2547579852,// down left
|
||||
0.3011177285,0.301264851,
|
||||
0.3002376991,0.2403885431,// down
|
||||
0.3006540818,0.3010588401,
|
||||
0.3011093054,0.2555000655,// down right
|
||||
0.3000802760,0.3008482317
|
||||
];
|
||||
|
||||
#[derive(Clone, Debug, Default, Format)]
|
||||
pub struct StickParams {
|
||||
|
|
@ -44,44 +90,177 @@ pub struct StickParams {
|
|||
pub boundary_angles: [f32; 4], // angles at the boundaries between regions of the stick (in the plane)
|
||||
}
|
||||
|
||||
#[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 StickParams {
|
||||
/// Generate StickParams structs for the sticks, returned as a tuple of (analog_stick, c_stick)
|
||||
pub fn from_controller_config(controller_config: &ControllerConfig) -> (Self, Self) {
|
||||
let cleaned_cal_points_astick = CleanedCalibrationPoints::from_temp_calibration_points(
|
||||
&controller_config.temp_cal_points_ax.to_regular_array(),
|
||||
&controller_config.temp_cal_points_ay.to_regular_array(),
|
||||
&controller_config.a_angles.to_regular_array(),
|
||||
);
|
||||
|
||||
let cleaned_cal_points_cstick = CleanedCalibrationPoints::from_temp_calibration_points(
|
||||
&controller_config.temp_cal_points_cx.to_regular_array(),
|
||||
&controller_config.temp_cal_points_cy.to_regular_array(),
|
||||
&controller_config.c_angles.to_regular_array(),
|
||||
);
|
||||
|
||||
todo!()
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Format, Copy)]
|
||||
enum NotchStatus {
|
||||
TertInactive,
|
||||
TertActive,
|
||||
Secondary,
|
||||
Cardinal,
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug)]
|
||||
struct CleanedCalibrationPoints {
|
||||
pub cleaned_points_x: [f32; NO_OF_NOTCHES + 1],
|
||||
pub cleaned_points_y: [f32; NO_OF_NOTCHES + 1],
|
||||
pub notch_points_x: [f32; NO_OF_NOTCHES + 1],
|
||||
pub notch_points_y: [f32; NO_OF_NOTCHES + 1],
|
||||
pub notch_status: [NotchStatus; NO_OF_NOTCHES],
|
||||
}
|
||||
|
||||
impl Default for CleanedCalibrationPoints {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
cleaned_points_x: [0f32; NO_OF_NOTCHES + 1],
|
||||
cleaned_points_y: [0f32; NO_OF_NOTCHES + 1],
|
||||
notch_points_x: [0f32; NO_OF_NOTCHES + 1],
|
||||
notch_points_y: [0f32; NO_OF_NOTCHES + 1],
|
||||
notch_status: NOTCH_STATUS_DEFAULTS,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl CleanedCalibrationPoints {
|
||||
pub fn from_temp_calibration_points(
|
||||
cal_points_x: &[f32; NO_OF_CALIBRATION_POINTS],
|
||||
cal_points_y: &[f32; NO_OF_CALIBRATION_POINTS],
|
||||
notch_angles: &[f32; NO_OF_NOTCHES],
|
||||
) -> Self {
|
||||
let mut out = Self::default();
|
||||
|
||||
debug!("Raw calibration points:");
|
||||
for i in 0..NO_OF_CALIBRATION_POINTS {
|
||||
debug!("({}, {})", cal_points_x[i], cal_points_y[i])
|
||||
}
|
||||
|
||||
debug!("Notch angles: {}", notch_angles);
|
||||
|
||||
for i in 0..NO_OF_NOTCHES {
|
||||
// add the origin values to the first x,y point
|
||||
out.cleaned_points_x[0] += cal_points_x[i * 2];
|
||||
out.cleaned_points_y[0] += cal_points_y[i * 2];
|
||||
|
||||
// copy the cal point into the cleaned list
|
||||
out.cleaned_points_x[i + 1] = cal_points_x[i * 2 + 1];
|
||||
out.cleaned_points_y[i + 1] = cal_points_y[i * 2 + 1];
|
||||
|
||||
(out.notch_points_x[i + 1], out.notch_points_y[i + 1]) =
|
||||
match calc_stick_values(notch_angles[i]) {
|
||||
(a, b) => (roundf(a), roundf(b)),
|
||||
};
|
||||
}
|
||||
|
||||
// TODO: put the below in a macro to clean it up a bit, once it's confirmed to work
|
||||
// remove the largest and smallest two origin values to remove outliers
|
||||
// first, find their indices
|
||||
let mut i = 0;
|
||||
let x_by_size = &mut cal_points_x.map(|e| {
|
||||
i += 1;
|
||||
(i - 1, e)
|
||||
});
|
||||
|
||||
tiny_sort::unstable::sort_by(x_by_size, |a, b| a.1.partial_cmp(&b.1).unwrap());
|
||||
|
||||
let smallest_x = x_by_size[0].0;
|
||||
let small_x = x_by_size[1].0;
|
||||
let large_x = x_by_size[x_by_size.len() - 2].0;
|
||||
let largest_x = x_by_size[x_by_size.len() - 1].0;
|
||||
|
||||
// do the same for y
|
||||
let mut i = 0;
|
||||
let y_by_size = &mut cal_points_y.map(|e| {
|
||||
i += 1;
|
||||
(i - 1, e)
|
||||
});
|
||||
|
||||
tiny_sort::unstable::sort_by(y_by_size, |a, b| a.1.partial_cmp(&b.1).unwrap());
|
||||
|
||||
let smallest_y = y_by_size[0].0;
|
||||
let small_y = y_by_size[1].0;
|
||||
let large_y = y_by_size[y_by_size.len() - 2].0;
|
||||
let largest_y = y_by_size[y_by_size.len() - 1].0;
|
||||
|
||||
// TODO: make this whole thing a function? it looks very ugly
|
||||
out.cleaned_points_x[0] -= cal_points_x[smallest_x];
|
||||
out.cleaned_points_x[0] -= cal_points_x[small_x];
|
||||
out.cleaned_points_x[0] -= cal_points_x[large_x];
|
||||
out.cleaned_points_x[0] -= cal_points_x[largest_x];
|
||||
|
||||
out.cleaned_points_y[0] -= cal_points_y[smallest_y];
|
||||
out.cleaned_points_y[0] -= cal_points_y[small_y];
|
||||
out.cleaned_points_y[0] -= cal_points_y[large_y];
|
||||
out.cleaned_points_y[0] -= cal_points_y[largest_y];
|
||||
|
||||
out.cleaned_points_x[0] /= (NO_OF_NOTCHES - 4) as f32;
|
||||
out.cleaned_points_y[0] /= (NO_OF_NOTCHES - 4) as f32;
|
||||
|
||||
for i in 0..NO_OF_NOTCHES {
|
||||
let delta_x = out.cleaned_points_x[i + 1] - out.cleaned_points_x[0];
|
||||
let delta_y = out.cleaned_points_y[i + 1] - out.cleaned_points_y[0];
|
||||
let mag = sqrtf(delta_x * delta_x + delta_y * delta_y);
|
||||
|
||||
// if the cleaned point was at the center and would be a firefox notch
|
||||
// average the previous and next points (cardinal & diagonal) for some sanity
|
||||
if mag < 0.02 && (i % 2 == 0) {
|
||||
let prev_index = ((i + NO_OF_NOTCHES - 1) % NO_OF_NOTCHES) + 1;
|
||||
let next_index = ((i + 1) % NO_OF_NOTCHES) + 1;
|
||||
|
||||
out.cleaned_points_x[i + 1] =
|
||||
(out.cleaned_points_x[prev_index] + out.cleaned_points_x[next_index]) / 2.0;
|
||||
out.cleaned_points_y[i + 1] =
|
||||
(out.cleaned_points_y[prev_index] + out.cleaned_points_y[next_index]) / 2.0;
|
||||
|
||||
out.notch_points_x[i + 1] =
|
||||
(out.notch_points_x[prev_index] + out.notch_points_x[next_index]) / 2.0;
|
||||
out.notch_points_y[i + 1] =
|
||||
(out.notch_points_y[prev_index] + out.notch_points_y[next_index]) / 2.0;
|
||||
|
||||
debug!("Skipping notch {}", i + 1);
|
||||
|
||||
// Mark that notch adjustment should be skipped for this
|
||||
out.notch_status[i] = NotchStatus::TertInactive;
|
||||
} else {
|
||||
out.notch_status[i] = NOTCH_STATUS_DEFAULTS[i];
|
||||
}
|
||||
}
|
||||
|
||||
debug!("Final points:");
|
||||
for i in 0..=NO_OF_NOTCHES {
|
||||
debug!(
|
||||
"Cleaned: ({}, {}), Notch: ({}, {})",
|
||||
out.cleaned_points_x[i],
|
||||
out.cleaned_points_y[i],
|
||||
out.notch_points_x[i],
|
||||
out.notch_points_y[i],
|
||||
);
|
||||
}
|
||||
|
||||
debug!("The notch statuses are: {:?}", out.notch_status);
|
||||
|
||||
out
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Default)]
|
||||
struct LinearizeCalibrationOutput {
|
||||
struct LinearizedCalibration {
|
||||
pub fit_coeffs_x: [f64; NUM_COEFFS],
|
||||
pub fit_coeffs_y: [f64; NUM_COEFFS],
|
||||
|
||||
|
|
@ -89,13 +268,60 @@ struct LinearizeCalibrationOutput {
|
|||
pub out_y: [f32; NO_OF_NOTCHES],
|
||||
}
|
||||
|
||||
pub fn run_kalman(
|
||||
x_z: f32,
|
||||
y_z: f32,
|
||||
controller_config: &ControllerConfig,
|
||||
filter_gains: &FilterGains,
|
||||
) -> (f32, f32) {
|
||||
todo!()
|
||||
impl LinearizedCalibration {
|
||||
///
|
||||
/// Generate a fit to linearize the stick response.
|
||||
///
|
||||
/// Inputs:
|
||||
/// cleaned points X and Y, (must be 17 points for each of these, the first being the center, the others starting at 3 oclock and going around counterclockwise)
|
||||
///
|
||||
/// Outputs:
|
||||
/// linearization fit coefficients for X and Y
|
||||
pub fn from_points(in_x: &[f64; 17], in_y: &[f64; 17]) -> Self {
|
||||
let mut fit_points_x = [0f64; 5];
|
||||
let mut fit_points_y = [0f64; 5];
|
||||
|
||||
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];
|
||||
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, 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));
|
||||
|
||||
fit_coeffs_x[3] = fit_coeffs_x[3] - x_zero_error as f64;
|
||||
fit_coeffs_y[3] = fit_coeffs_y[3] - y_zero_error as f64;
|
||||
|
||||
let mut out_x = [0f32; NO_OF_NOTCHES];
|
||||
let mut out_y = [0f32; NO_OF_NOTCHES];
|
||||
|
||||
for i in 0..=NO_OF_NOTCHES {
|
||||
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));
|
||||
}
|
||||
|
||||
Self {
|
||||
fit_coeffs_x,
|
||||
fit_coeffs_y,
|
||||
out_x,
|
||||
out_y,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Calculate the power of a number
|
||||
|
|
@ -236,6 +462,17 @@ fn fit_curve<const N: usize, const NCOEFFS: usize>(
|
|||
coeffs
|
||||
}
|
||||
|
||||
/// Compute the stick x/y coordinates from a given angle.
|
||||
/// The stick moves spherically, so it requires 3D trigonometry.
|
||||
fn calc_stick_values(angle: f32) -> (f32, f32) {
|
||||
let x =
|
||||
100. * atan2f(sinf(MAX_STICK_ANGLE) * cosf(angle), cosf(MAX_STICK_ANGLE)) / MAX_STICK_ANGLE;
|
||||
let y =
|
||||
100. * atan2f(sinf(MAX_STICK_ANGLE) * sinf(angle), cosf(MAX_STICK_ANGLE)) / MAX_STICK_ANGLE;
|
||||
|
||||
(x, y)
|
||||
}
|
||||
|
||||
pub fn linearize(point: f32, coefficients: &[f32; 4]) -> f32 {
|
||||
coefficients[0] * (point * point * point)
|
||||
+ coefficients[1] * (point * point)
|
||||
|
|
@ -243,58 +480,6 @@ pub fn linearize(point: f32, coefficients: &[f32; 4]) -> f32 {
|
|||
+ coefficients[3]
|
||||
}
|
||||
|
||||
///
|
||||
/// Generate a fit to linearize the stick response.
|
||||
///
|
||||
/// Inputs:
|
||||
/// cleaned points X and Y, (must be 17 points for each of these, the first being the center, the others starting at 3 oclock and going around counterclockwise)
|
||||
///
|
||||
/// Outputs:
|
||||
/// linearization fit coefficients for X and Y
|
||||
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];
|
||||
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];
|
||||
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, 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));
|
||||
|
||||
fit_coeffs_x[3] = fit_coeffs_x[3] - x_zero_error as f64;
|
||||
fit_coeffs_y[3] = fit_coeffs_y[3] - y_zero_error as f64;
|
||||
|
||||
let mut out_x = [0f32; NO_OF_NOTCHES];
|
||||
let mut out_y = [0f32; NO_OF_NOTCHES];
|
||||
|
||||
for i in 0..=NO_OF_NOTCHES {
|
||||
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 {
|
||||
fit_coeffs_x,
|
||||
fit_coeffs_y,
|
||||
out_x,
|
||||
out_y,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn notch_remap(
|
||||
x_in: f32,
|
||||
y_in: f32,
|
||||
|
|
@ -337,55 +522,3 @@ pub fn notch_remap(
|
|||
|
||||
(x_out, y_out)
|
||||
}
|
||||
|
||||
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),
|
||||
}
|
||||
}
|
||||
|
||||
/// Returns filter gains for 1000Hz polling rate
|
||||
pub fn get_norm_gains(controller_config: &ControllerConfig) -> FilterGains {
|
||||
let mut gains = FILTER_GAINS.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),
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue