feat: implement stick calibration logic

Cargo.lock

@@ -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"

Cargo.toml

@@ -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]

flake.nix

@@ -78,7 +78,7 @@
           "--target=${CARGO_BUILD_TARGET}"
         ];
 
-        # inherit RUSTFLAGS;
+        DEFMT_LOG = "warn";
       };
 
       devShells.default = pkgs.mkShell {

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,

src/flash_mem.rs

@@ -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);
-}

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 {

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>;
 });

src/packed_float.rs

@@ -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
+    }
+}

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