From fe4afce386c6eaf90136e8132dc0df50766dbe85 Mon Sep 17 00:00:00 2001 From: Naxdy Date: Wed, 27 Mar 2024 14:16:42 +0100 Subject: [PATCH] feat(input, stick): implement controller config revisioning & norm gains --- src/filter.rs | 3 +- src/input.rs | 105 +++++++++++++++++++++++++++++++++++++++++++------- src/stick.rs | 73 ++++++++++++++++++++++++++++++++++- 3 files changed, 165 insertions(+), 16 deletions(-) diff --git a/src/filter.rs b/src/filter.rs index 52782af..de34202 100644 --- a/src/filter.rs +++ b/src/filter.rs @@ -1,10 +1,11 @@ -use libm::{fmin, fminf}; +use libm::fminf; use crate::{ input::{ControllerConfig, Stick}, stick::FilterGains, }; +#[derive(Debug, Clone, Default)] pub struct WaveshapingValues { pub old_x_pos: f32, pub old_y_pos: f32, diff --git a/src/input.rs b/src/input.rs index 563e481..6245ddf 100644 --- a/src/input.rs +++ b/src/input.rs @@ -1,9 +1,9 @@ use core::task::Poll; -use defmt::{debug, Format}; +use defmt::{debug, info, warn, Format}; use embassy_futures::{join::join, yield_now}; use embassy_rp::{ - flash::{Async, Flash}, + flash::{Async, Flash, ERASE_SIZE}, gpio::{Input, Output, Pin}, peripherals::{ FLASH, PIN_10, PIN_11, PIN_16, PIN_17, PIN_18, PIN_19, PIN_20, PIN_21, PIN_22, PIN_23, @@ -15,7 +15,7 @@ use embassy_rp::{ use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal}; use embassy_time::{Instant, Timer}; use libm::{fmaxf, fmin, fminf}; -use packed_struct::derive::PackedStruct; +use packed_struct::{derive::PackedStruct, PackedStruct}; use crate::{ filter::{run_waveshaping, WaveshapingValues}, @@ -27,12 +27,16 @@ use crate::{ pub static GCC_SIGNAL: Signal = Signal::new(); static STICK_SIGNAL: Signal = Signal::new(); -static STICK_HYST_VAL: f32 = 0.3; -static FLOAT_ORIGIN: f32 = 127.5; +const STICK_HYST_VAL: f32 = 0.3; +const FLOAT_ORIGIN: f32 = 127.5; -#[derive(Debug, Clone, Default, Format, PackedStruct)] +pub const CONTROLLER_CONFIG_REVISION: u8 = 1; + +#[derive(Debug, Clone, Format, PackedStruct)] #[packed_struct(endian = "msb")] pub struct ControllerConfig { + #[packed_field(size_bits = "8")] + pub config_revision: u8, #[packed_field(size_bits = "8")] pub config_version: u8, #[packed_field(size_bits = "8")] @@ -47,6 +51,39 @@ pub struct ControllerConfig { pub astick_analog_scaler: u8, #[packed_field(size_bits = "8")] pub cstick_analog_scaler: u8, + #[packed_field(size_bits = "8")] + pub x_snapback: i8, + #[packed_field(size_bits = "8")] + pub y_snapback: i8, + #[packed_field(size_bits = "8")] + pub x_smoothing: u8, + #[packed_field(size_bits = "8")] + pub y_smoothing: u8, + #[packed_field(size_bits = "8")] + pub c_xsmoothing: u8, + #[packed_field(size_bits = "8")] + pub c_ysmoothing: u8, +} + +impl Default for ControllerConfig { + fn default() -> Self { + Self { + config_revision: CONTROLLER_CONFIG_REVISION, + config_version: 0, + ax_waveshaping: 0, + ay_waveshaping: 0, + cx_waveshaping: 0, + cy_waveshaping: 0, + astick_analog_scaler: 0, + cstick_analog_scaler: 0, + x_snapback: 0, + y_snapback: 0, + x_smoothing: 0, + y_smoothing: 0, + c_xsmoothing: 0, + c_ysmoothing: 0, + } + } } #[derive(Clone, Debug, Default)] @@ -57,6 +94,7 @@ struct StickState { cy: u8, } +#[derive(Clone, Debug, Default)] struct StickPositions { x: f32, y: f32, @@ -64,6 +102,7 @@ struct StickPositions { cy: f32, } +#[derive(Clone, Debug, Default)] struct RawStickValues { ax_linearized: f32, ay_linearized: f32, @@ -388,17 +427,57 @@ pub async fn input_loop( gcc_state.cstick_x = 127; gcc_state.cstick_y = 127; - let mut uid = [0u8; 1]; - flash.blocking_read(ADDR_OFFSET, &mut uid).unwrap(); + let mut controller_config_packed = [0u8; 14]; // ControllerConfig byte size + flash + .blocking_read(ADDR_OFFSET, &mut controller_config_packed) + .unwrap(); - debug!("Read from flash: {:02X}", uid); - - // TODO: load controller config here + 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 stick_state_fut = async { + let mut current_stick_state = StickState { + ax: 127, + ay: 127, + cx: 127, + cy: 127, + }; + let mut raw_stick_values = RawStickValues::default(); + let mut old_stick_pos = StickPositions::default(); + let mut cstick_waveshaping_values = WaveshapingValues::default(); + let mut controlstick_waveshaping_values = WaveshapingValues::default(); + loop { - // current_stick_state = update_stick_states(&mut spi, &mut spi_acs, &mut spi_ccs, 1.0).await; - // STICK_SIGNAL.signal(current_stick_state.clone()); + current_stick_state = update_stick_states( + &mut spi, + &mut spi_acs, + &mut spi_ccs, + ¤t_stick_state, + &controlstick_params, + &cstick_params, + &controller_config, + &filter_gains, + &mut controlstick_waveshaping_values, + &mut cstick_waveshaping_values, + &mut old_stick_pos, + &mut raw_stick_values, + ) + .await; + + STICK_SIGNAL.signal(current_stick_state.clone()); } }; diff --git a/src/stick.rs b/src/stick.rs index c6e846d..78e66c7 100644 --- a/src/stick.rs +++ b/src/stick.rs @@ -1,9 +1,9 @@ // vast majority of this is taken from Phob firmware -use core::f32::consts::PI; +use core::{f32::consts::PI, iter::Filter}; use defmt::Format; -use libm::{atan2f, fabs}; +use libm::{atan2f, fabs, powf}; use crate::{ input::{ControllerConfig, Stick}, @@ -16,6 +16,23 @@ const NUM_COEFFS: usize = FIT_ORDER + 1; const NO_OF_NOTCHES: usize = 16; 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, +}; + #[derive(Clone, Debug, Default, Format)] pub struct StickParams { // these are the linearization coefficients @@ -320,3 +337,55 @@ 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), + } +}