diff --git a/Cargo.lock b/Cargo.lock index 55f8bbd..d23faab 100644 --- a/Cargo.lock +++ b/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" diff --git a/Cargo.toml b/Cargo.toml index 2b51105..edc2b59 100644 --- a/Cargo.toml +++ b/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] diff --git a/flake.nix b/flake.nix index b25322e..d5def3a 100644 --- a/flake.nix +++ b/flake.nix @@ -78,7 +78,7 @@ "--target=${CARGO_BUILD_TARGET}" ]; - # inherit RUSTFLAGS; + DEFMT_LOG = "warn"; }; devShells.default = pkgs.mkShell { diff --git a/src/filter.rs b/src/filter.rs index de34202..e83f10a 100644 --- a/src/filter.rs +++ b/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, diff --git a/src/flash_mem.rs b/src/flash_mem.rs deleted file mode 100644 index 4c4e442..0000000 --- a/src/flash_mem.rs +++ /dev/null @@ -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); -} diff --git a/src/input.rs b/src/input.rs index 6245ddf..31932d4 100644 --- a/src/input.rs +++ b/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 = 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 { + let mut controller_config_packed: ::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 { diff --git a/src/main.rs b/src/main.rs index 940197e..c98491d 100644 --- a/src/main.rs +++ b/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 = 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 { - Ok(self.to_be_bytes()) - } - - fn unpack(src: &Self::ByteArray) -> packed_struct::PackingResult { - 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; }); diff --git a/src/packed_float.rs b/src/packed_float.rs new file mode 100644 index 0000000..fb47cae --- /dev/null +++ b/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 { + fn to_regular_array(&self) -> &[f32; T]; +} + +impl ToRegularArray for [PackedFloat; T] { + fn to_regular_array(&self) -> &[f32; T] { + unsafe { &*(self as *const _ as *const _) } + } +} + +pub trait ToPackedFloatArray { + fn to_packed_float_array(&self) -> &[PackedFloat; T]; +} + +impl ToPackedFloatArray 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 { + Ok(self.to_be_bytes()) + } + + fn unpack(src: &Self::ByteArray) -> packed_struct::PackingResult { + Ok(Self(f32::from_be_bytes(*src))) + } +} + +impl Deref for PackedFloat { + type Target = f32; + + fn deref(&self) -> &Self::Target { + &self.0 + } +} diff --git a/src/stick.rs b/src/stick.rs index 78e66c7..678f355 100644 --- a/src/stick.rs +++ b/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( 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), - } -}