feat(input, stick): implement controller config revisioning & norm gains

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,

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<CriticalSectionRawMutex, GcReport> = Signal::new();
 
 static STICK_SIGNAL: Signal<CriticalSectionRawMutex, StickState> = 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,16 +427,58 @@ 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);
+    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
+        }
+    };
 
     // TODO: load controller config here
 
     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;
+            // current_stick_state = update_stick_states(
+            //     &mut spi,
+            //     &mut spi_acs,
+            //     &mut spi_ccs,
+            //     &current_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());
         }
     };

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),
+    }
+}