From 2d7239c04a13474a64cf98bf668c46e8a45ae904 Mon Sep 17 00:00:00 2001 From: Naxdy Date: Fri, 29 Mar 2024 19:54:52 +0100 Subject: [PATCH] feat(config, stick): implement more calibration logic --- Cargo.toml | 1 + src/config.rs | 253 +++++++++++++++++++++++++++++++++++++++++-------- src/helpers.rs | 6 ++ src/input.rs | 128 +++++++++++++------------ src/main.rs | 9 +- src/stick.rs | 213 +++++++++++++++++++++++++++++++++++++---- 6 files changed, 489 insertions(+), 121 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index edc2b59..ccf65e3 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -62,6 +62,7 @@ debug = 2 debug-assertions = true incremental = false opt-level = 3 +lto = "fat" overflow-checks = true # cargo build/run --release diff --git a/src/config.rs b/src/config.rs index 8c917de..0a117d8 100644 --- a/src/config.rs +++ b/src/config.rs @@ -4,7 +4,7 @@ */ use core::f32::consts::PI; -use defmt::{info, warn, Format}; +use defmt::{error, info, warn, Format}; use embassy_rp::{ flash::{Async, Flash, ERASE_SIZE}, peripherals::FLASH, @@ -12,16 +12,38 @@ use embassy_rp::{ use packed_struct::{derive::PackedStruct, PackedStruct}; use crate::{ - helpers::{PackedFloat, ToPackedFloatArray}, - stick::{NotchStatus, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES}, + helpers::{PackedFloat, ToPackedFloatArray, XyValuePair}, + input::{read_ext_adc, Stick, StickAxis, SPI_ACS_SHARED, SPI_CCS_SHARED, SPI_SHARED}, + stick::{NotchStatus, NO_OF_ADJ_NOTCHES, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES}, ADDR_OFFSET, FLASH_SIZE, }; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pubsub::Subscriber}; +use embassy_sync::{ + blocking_mutex::raw::{CriticalSectionRawMutex, RawMutex}, + pubsub::Subscriber, + signal::Signal, +}; use embassy_time::Timer; use crate::{gcc_hid::GcReport, input::CHANNEL_GCC_STATE}; +/// Whether we are currently calibrating the sticks. Updates are dispatched when the status changes. +/// Initial status is assumed to be false. +pub static SIGNAL_IS_CALIBRATING: Signal = Signal::new(); + +/// Dispatched when we want to override the GCC state for a short amount of time. +pub static SIGNAL_OVERRIDE_GCC_STATE: Signal = + Signal::new(); + +/// Struct used for overriding the GCC state for a given amount of +/// time, useful for providing feedback to the user e.g. if we just entered +/// a certain mode. +#[derive(Default, Debug, Clone, Format)] +pub struct OverrideGcReportInstruction { + pub report: GcReport, + pub duration_ms: u64, +} + const CONFIG_MODE_ENTRY_COMBO: [AwaitableButtons; 4] = [ AwaitableButtons::Start, AwaitableButtons::A, @@ -29,6 +51,20 @@ const CONFIG_MODE_ENTRY_COMBO: [AwaitableButtons; 4] = [ AwaitableButtons::Y, ]; +const LSTICK_CALIBRATION_COMBO: [AwaitableButtons; 4] = [ + AwaitableButtons::A, + AwaitableButtons::X, + AwaitableButtons::Y, + AwaitableButtons::L, +]; + +const RSTICK_CALIBRATION_COMBO: [AwaitableButtons; 4] = [ + AwaitableButtons::A, + AwaitableButtons::X, + AwaitableButtons::Y, + AwaitableButtons::R, +]; + /// This doesn't need to be super fast, since it's only used /// in config mode. const BUTTON_POLL_INTERVAL_MILLIS: u64 = 20; @@ -96,7 +132,7 @@ const DEFAULT_CAL_POINTS_Y: [f32; NO_OF_CALIBRATION_POINTS] = [ 0.3000802760,0.3008482317 ]; -const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [ +pub const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [ 0., PI / 8.0, PI * 2. / 8., @@ -150,9 +186,9 @@ pub struct StickConfig { #[packed_field(size_bits = "8")] pub y_smoothing: u8, #[packed_field(element_size_bytes = "4")] - pub temp_cal_points_x: [PackedFloat; 32], + pub cal_points_x: [PackedFloat; 32], #[packed_field(element_size_bytes = "4")] - pub temp_cal_points_y: [PackedFloat; 32], + pub cal_points_y: [PackedFloat; 32], #[packed_field(element_size_bytes = "4")] pub angles: [PackedFloat; 16], } @@ -168,8 +204,8 @@ impl Default for StickConfig { y_smoothing: 0, cardinal_snapping: 0, analog_scaler: 100, - temp_cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(), - temp_cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(), + cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(), + cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(), angles: *DEFAULT_ANGLES.to_packed_float_array(), } } @@ -255,16 +291,17 @@ trait WaitForButtonPress { ) -> usize; } -impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress - for Subscriber<'a, CriticalSectionRawMutex, GcReport, I, J, K> +impl<'a, T: RawMutex, const I: usize, const J: usize, const K: usize> WaitForButtonPress + for Subscriber<'a, T, GcReport, I, J, K> { async fn wait_for_button_press(&mut self, button_to_wait_for: &AwaitableButtons) { loop { - if match self.next_message_pure().await { - report => is_awaitable_button_pressed(&report, button_to_wait_for), - } { + let report = self.next_message_pure().await; + + if is_awaitable_button_pressed(&report, button_to_wait_for) { break; } + Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await; } } @@ -274,13 +311,15 @@ impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress buttons_to_wait_for: &[AwaitableButtons; N], ) { loop { - if match self.next_message_pure().await { - report => buttons_to_wait_for - .iter() - .all(|button| is_awaitable_button_pressed(&report, button)), - } { + let report = self.next_message_pure().await; + + if buttons_to_wait_for + .iter() + .all(|button| is_awaitable_button_pressed(&report, button)) + { break; } + Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await; } } @@ -290,15 +329,14 @@ impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress buttons_to_wait_for: &[AwaitableButtons; N], ) -> AwaitableButtons { loop { - match self.next_message_pure().await { - report => { - for button in buttons_to_wait_for { - if is_awaitable_button_pressed(&report, button) { - return *button; - } - } + let report = self.next_message_pure().await; + + for button in buttons_to_wait_for { + if is_awaitable_button_pressed(&report, button) { + return *button; } } + Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await; } } @@ -308,18 +346,17 @@ impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress buttons_to_wait_for: &[[AwaitableButtons; N]; M], ) -> usize { loop { - match self.next_message_pure().await { - report => { - for (i, buttons) in buttons_to_wait_for.iter().enumerate() { - if buttons - .iter() - .all(|button| is_awaitable_button_pressed(&report, button)) - { - return i; - } - } + let report = self.next_message_pure().await; + + for (i, buttons) in buttons_to_wait_for.iter().enumerate() { + if buttons + .iter() + .all(|button| is_awaitable_button_pressed(&report, button)) + { + return i; } } + Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await; } } @@ -336,18 +373,156 @@ fn is_awaitable_button_pressed(report: &GcReport, button_to_wait_for: &Awaitable AwaitableButtons::Left => report.buttons_1.dpad_left, AwaitableButtons::Right => report.buttons_1.dpad_right, AwaitableButtons::Start => report.buttons_2.button_start, - AwaitableButtons::L => report.buttons_2.button_l, - AwaitableButtons::R => report.buttons_2.button_r, + AwaitableButtons::L => report.buttons_2.button_l || report.trigger_l > 10, + AwaitableButtons::R => report.buttons_2.button_r || report.trigger_r > 10, + } +} + +#[derive(Debug, Format)] +struct StickCalibrationProcess<'a> { + which_stick: Stick, + calibration_step: u8, + gcc_config: &'a mut ControllerConfig, + cal_points: [XyValuePair; NO_OF_CALIBRATION_POINTS], +} + +impl<'a> StickCalibrationProcess<'a> { + pub fn new(gcc_config: &'a mut ControllerConfig, which_stick: Stick) -> Self { + Self { + which_stick, + calibration_step: 0, + gcc_config, + cal_points: [XyValuePair::default(); NO_OF_CALIBRATION_POINTS], + } + } + + async fn calibration_advance(&mut self) { + let mut spi_unlocked = SPI_SHARED.lock().await; + let mut spi_acs_unlocked = SPI_ACS_SHARED.lock().await; + let mut spi_ccs_unlocked = SPI_CCS_SHARED.lock().await; + + let spi = spi_unlocked.as_mut().unwrap(); + let spi_acs = spi_acs_unlocked.as_mut().unwrap(); + let spi_ccs = spi_ccs_unlocked.as_mut().unwrap(); + + if self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 { + let mut x: f32 = 0.; + let mut y: f32 = 0.; + + for _ in 0..128 { + x += read_ext_adc(self.which_stick, StickAxis::XAxis, spi, spi_acs, spi_ccs) as f32 + / 4096.0; + y += read_ext_adc(self.which_stick, StickAxis::YAxis, spi, spi_acs, spi_ccs) as f32 + / 4096.0; + } + + x /= 128.; + y /= 128.; + + self.cal_points[self.calibration_step as usize] = XyValuePair { x, y }; + } + + self.calibration_step += 1; + + // TODO: phob does something related to undo here + + if self.calibration_step == NO_OF_CALIBRATION_POINTS as u8 { + // TODO + } + + if self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8 { + let stick_config = match self.which_stick { + Stick::ControlStick => &mut self.gcc_config.astick_config, + Stick::CStick => &mut self.gcc_config.cstick_config, + }; + + stick_config.cal_points_x = self.cal_points.map(|p| p.x.into()); + stick_config.cal_points_y = self.cal_points.map(|p| p.y.into()); + } + } + + pub async fn calibrate_stick(&mut self) { + todo!() + } +} + +async fn configuration_main_loop< + 'a, + M: RawMutex, + const C: usize, + const S: usize, + const P: usize, +>( + current_config: &ControllerConfig, + mut flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>, + gcc_subscriber: &mut Subscriber<'a, M, GcReport, C, S, P>, +) { + let mut final_config = current_config.clone(); + let config_options = [LSTICK_CALIBRATION_COMBO, RSTICK_CALIBRATION_COMBO]; + + 'main: loop { + match gcc_subscriber + .wait_and_filter_simultaneous_button_presses(&config_options) + .await + { + selection => match selection { + 1 => { + StickCalibrationProcess::new(&mut final_config, Stick::ControlStick) + .calibrate_stick() + .await + } + 2 => { + StickCalibrationProcess::new(&mut final_config, Stick::CStick) + .calibrate_stick() + .await + } + _ => { + error!("Invalid selection: {}", selection); + break 'main; + } + }, + }; + + final_config.write_to_flash(&mut flash).unwrap(); } } #[embassy_executor::task] -pub async fn config_task() { +pub async fn config_task( + current_config: ControllerConfig, + mut flash: Flash<'static, FLASH, Async, FLASH_SIZE>, +) { let mut gcc_subscriber = CHANNEL_GCC_STATE.subscriber().unwrap(); loop { gcc_subscriber .wait_for_simultaneous_button_presses(&CONFIG_MODE_ENTRY_COMBO) .await; + + info!("Entering config mode."); + + SIGNAL_OVERRIDE_GCC_STATE.signal(OverrideGcReportInstruction { + report: match GcReport::default() { + mut a => { + a.trigger_r = 255; + a.trigger_l = 255; + a.buttons_2.button_l = true; + a.buttons_2.button_r = true; + a.buttons_1.button_x = true; + a.buttons_1.button_y = true; + a.buttons_1.button_a = true; + a.stick_x = 127; + a.stick_y = 127; + a.cstick_x = 127; + a.cstick_y = 127; + a + } + }, + duration_ms: 1000, + }); + + configuration_main_loop(¤t_config, &mut flash, &mut gcc_subscriber).await; + + info!("Exiting config mode."); } } diff --git a/src/helpers.rs b/src/helpers.rs index 9380dc8..cde645d 100644 --- a/src/helpers.rs +++ b/src/helpers.rs @@ -47,6 +47,12 @@ impl Deref for PackedFloat { } } +impl From for PackedFloat { + fn from(f: f32) -> Self { + Self(f) + } +} + #[derive(Debug, Clone, Format, Default, Copy)] pub struct XyValuePair { pub x: T, diff --git a/src/input.rs b/src/input.rs index 8db9014..309b0d2 100644 --- a/src/input.rs +++ b/src/input.rs @@ -5,19 +5,22 @@ use embassy_rp::{ gpio::{AnyPin, 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, - PIN_24, PIN_5, PIN_8, PIN_9, PWM_CH4, PWM_CH6, SPI0, + PIN_24, PIN_5, PIN_8, PIN_9, PWM_CH4, PWM_CH6, SPI0, SPI1, }, pwm::Pwm, - spi::Spi, + spi::{Blocking, Spi}, }; use embassy_sync::{ - blocking_mutex::raw::CriticalSectionRawMutex, pubsub::PubSubChannel, signal::Signal, + blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex, ThreadModeRawMutex}, + mutex::Mutex, + pubsub::PubSubChannel, + signal::Signal, }; use embassy_time::{Duration, Instant, Timer}; use libm::{fmaxf, fminf}; use crate::{ - config::ControllerConfig, + config::{ControllerConfig, SIGNAL_IS_CALIBRATING, SIGNAL_OVERRIDE_GCC_STATE}, filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS}, gcc_hid::GcReport, helpers::XyValuePair, @@ -30,7 +33,17 @@ pub static CHANNEL_GCC_STATE: PubSubChannel = Signal::new(); +static SIGNAL_STICK_STATE: Signal = Signal::new(); + +/// Used to send the raw stick values for the calibration task +static SIGNAL_RAW_STICK_VALUES: Signal = Signal::new(); + +pub static SPI_SHARED: Mutex>> = + Mutex::new(None); +pub static SPI_ACS_SHARED: Mutex>> = + Mutex::new(None); +pub static SPI_CCS_SHARED: Mutex>> = + Mutex::new(None); const STICK_HYST_VAL: f32 = 0.3; const FLOAT_ORIGIN: f32 = 127.5; @@ -61,7 +74,7 @@ struct RawStickValues { c_unfiltered: XyValuePair, } -#[derive(PartialEq, Eq)] +#[derive(PartialEq, Eq, Debug, Clone, Format, Copy)] pub enum Stick { ControlStick, CStick, @@ -74,7 +87,13 @@ pub enum StickAxis { } #[link_section = ".time_critical.read_ext_adc"] -fn read_ext_adc<'a, Acs: Pin, Ccs: Pin, I: embassy_rp::spi::Instance, M: embassy_rp::spi::Mode>( +pub fn read_ext_adc< + 'a, + Acs: Pin, + Ccs: Pin, + I: embassy_rp::spi::Instance, + M: embassy_rp::spi::Mode, +>( which_stick: Stick, which_axis: StickAxis, spi: &mut Spi<'a, I, M>, @@ -110,16 +129,7 @@ fn read_ext_adc<'a, Acs: Pin, Ccs: Pin, I: embassy_rp::spi::Instance, M: embassy /// Gets the average stick state over a 1ms interval in a non-blocking fashion. /// Will wait until end_time is reached before continuing after reading the ADCs. #[link_section = ".time_critical.update_stick_states"] -async fn update_stick_states< - 'a, - Acs: Pin, - Ccs: Pin, - I: embassy_rp::spi::Instance, - M: embassy_rp::spi::Mode, ->( - mut spi: &mut Spi<'a, I, M>, - mut spi_acs: &mut Output<'a, Acs>, - mut spi_ccs: &mut Output<'a, Ccs>, +async fn update_stick_states( current_stick_state: &StickState, controlstick_params: &StickParams, cstick_params: &StickParams, @@ -130,6 +140,7 @@ async fn update_stick_states< old_stick_pos: &mut StickPositions, raw_stick_values: &mut RawStickValues, kalman_state: &mut KalmanState, + is_calibrating: bool, ) -> StickState { let mut adc_count = 0u32; let mut ax_sum = 0u32; @@ -139,39 +150,23 @@ async fn update_stick_states< let end_time = Instant::now() + Duration::from_micros(300); // this seems kinda magic, and it is, but + let mut spi_unlocked = SPI_SHARED.lock().await; + let mut spi_acs_unlocked = SPI_ACS_SHARED.lock().await; + let mut spi_ccs_unlocked = SPI_CCS_SHARED.lock().await; + + let spi = spi_unlocked.as_mut().unwrap(); + let spi_acs = spi_acs_unlocked.as_mut().unwrap(); + let spi_ccs = spi_ccs_unlocked.as_mut().unwrap(); + // "do-while at home" while { let loop_start = Instant::now(); adc_count += 1; - ax_sum += read_ext_adc( - Stick::ControlStick, - StickAxis::XAxis, - &mut spi, - &mut spi_acs, - &mut spi_ccs, - ) as u32; - ay_sum += read_ext_adc( - Stick::ControlStick, - StickAxis::YAxis, - &mut spi, - &mut spi_acs, - &mut spi_ccs, - ) as u32; - cx_sum += read_ext_adc( - Stick::CStick, - StickAxis::XAxis, - &mut spi, - &mut spi_acs, - &mut spi_ccs, - ) as u32; - cy_sum += read_ext_adc( - Stick::CStick, - StickAxis::YAxis, - &mut spi, - &mut spi_acs, - &mut spi_ccs, - ) as u32; + ax_sum += read_ext_adc(Stick::ControlStick, StickAxis::XAxis, spi, spi_acs, spi_ccs) as u32; + ay_sum += read_ext_adc(Stick::ControlStick, StickAxis::YAxis, spi, spi_acs, spi_ccs) as u32; + cx_sum += read_ext_adc(Stick::CStick, StickAxis::XAxis, spi, spi_acs, spi_ccs) as u32; + cy_sum += read_ext_adc(Stick::CStick, StickAxis::YAxis, spi, spi_acs, spi_ccs) as u32; let loop_end = Instant::now(); loop_end < end_time - (loop_end - loop_start) @@ -265,7 +260,7 @@ async fn update_stick_states< controlstick_params, controller_config, Stick::ControlStick, - false, + is_calibrating, ) { (x, y) => XyValuePair { x, y }, }; @@ -275,7 +270,7 @@ async fn update_stick_states< cstick_params, controller_config, Stick::CStick, - false, + is_calibrating, ) { (x, y) => XyValuePair { x, y }, }; @@ -285,7 +280,7 @@ async fn update_stick_states< controlstick_params, controller_config, Stick::ControlStick, - false, + is_calibrating, ) { (x, y) => XyValuePair { x, y }, }; @@ -295,7 +290,7 @@ async fn update_stick_states< cstick_params, controller_config, Stick::CStick, - false, + is_calibrating, ) { (x, y) => XyValuePair { x, y }, }; @@ -440,16 +435,19 @@ pub async fn update_button_state_task( ); // not every loop pass is going to update the stick state - match STICK_SIGNAL.try_take() { - Some(stick_state) => { - gcc_state.stick_x = stick_state.ax; - gcc_state.stick_y = stick_state.ay; - gcc_state.cstick_x = stick_state.cx; - gcc_state.cstick_y = stick_state.cy; - } - None => (), + if let Some(stick_state) = SIGNAL_STICK_STATE.try_take() { + gcc_state.stick_x = stick_state.ax; + gcc_state.stick_y = stick_state.ay; + gcc_state.cstick_x = stick_state.cx; + gcc_state.cstick_y = stick_state.cy; } + // check for a gcc state override (usually coming from the config task) + if let Some(override_gcc_state) = SIGNAL_OVERRIDE_GCC_STATE.try_take() { + gcc_publisher.publish_immediate(override_gcc_state.report); + Timer::after_millis(override_gcc_state.duration_ms).await; + }; + gcc_publisher.publish_immediate(gcc_state); // give other tasks a chance to do something @@ -459,6 +457,8 @@ pub async fn update_button_state_task( /// Task responsible for updating the stick states. /// Publishes the result to STICK_SIGNAL. +/// +/// Has to run on core0 because it makes use of SPI0. #[embassy_executor::task] pub async fn update_stick_states_task( mut spi: Spi<'static, SPI0, embassy_rp::spi::Blocking>, @@ -490,13 +490,12 @@ pub async fn update_stick_states_task( // the time at which the current loop iteration should end let mut end_time = Instant::now() + Duration::from_micros(1000); + let mut is_calibrating = false; + loop { let timer = Timer::at(end_time); current_stick_state = update_stick_states( - &mut spi, - &mut spi_acs, - &mut spi_ccs, ¤t_stick_state, &controlstick_params, &cstick_params, @@ -507,22 +506,27 @@ pub async fn update_stick_states_task( &mut old_stick_pos, &mut raw_stick_values, &mut kalman_state, + is_calibrating, ) .await; timer.await; end_time += Duration::from_micros(1000); + if let Some(new_calibrating) = SIGNAL_IS_CALIBRATING.try_take() { + is_calibrating = new_calibrating; + } + match Instant::now() { n => { match (n - last_loop_time).as_micros() { - a if a > 1100 => debug!("Loop took {} us", a), + a if a > 1999 => debug!("Loop took {} us", a), _ => {} }; last_loop_time = n; } }; - STICK_SIGNAL.signal(current_stick_state.clone()); + SIGNAL_STICK_STATE.signal(current_stick_state.clone()); } } diff --git a/src/main.rs b/src/main.rs index 4974835..898e527 100644 --- a/src/main.rs +++ b/src/main.rs @@ -118,7 +118,14 @@ fn main() -> ! { // pwm_brake.set_counter(255); executor0.run(|spawner| { - spawner.spawn(config_task()).unwrap(); + // Config task has to run on core0 because it reads and writes to flash. + spawner + .spawn(config_task(controller_config.clone(), flash)) + .unwrap(); + + // Stick loop has to run on core0 because it makes use of SPI0. + // Perhaps in the future we can rewire the board to have it make use of SPI1 instead. + // This way it could be the sole task running on core1, and everything else could happen on core0. spawner .spawn(update_stick_states_task( spi, diff --git a/src/stick.rs b/src/stick.rs index f3307e8..a5a150b 100644 --- a/src/stick.rs +++ b/src/stick.rs @@ -6,7 +6,7 @@ use defmt::{debug, Format}; use libm::{atan2f, cosf, fabs, fabsf, roundf, sinf, sqrtf}; use crate::{ - config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS}, + config::{ControllerConfig, StickConfig, DEFAULT_ANGLES, DEFAULT_NOTCH_STATUS}, helpers::{ToRegularArray, XyValuePair}, input::Stick, }; @@ -15,7 +15,7 @@ use crate::{ const FIT_ORDER: usize = 3; const NUM_COEFFS: usize = FIT_ORDER + 1; pub const NO_OF_NOTCHES: usize = 16; -const NO_OF_ADJ_NOTCHES: usize = 12; +pub const NO_OF_ADJ_NOTCHES: usize = 12; pub const NO_OF_CALIBRATION_POINTS: usize = 32; const MAX_ORDER: usize = 20; @@ -45,9 +45,9 @@ impl StickParams { /// Generate StickParams structs for the sticks, returned as a tuple of (analog_stick, c_stick) pub fn from_stick_config(stick_config: &StickConfig) -> Self { let cleaned_cal_points = CleanedCalibrationPoints::from_temp_calibration_points( - &stick_config.temp_cal_points_x.to_regular_array(), - &stick_config.temp_cal_points_y.to_regular_array(), - &stick_config.angles.to_regular_array(), + stick_config.cal_points_x.to_regular_array(), + stick_config.cal_points_y.to_regular_array(), + stick_config.angles.to_regular_array(), ); let linearized_cal = LinearizedCalibration::from_calibration_points(&cleaned_cal_points); @@ -68,7 +68,7 @@ impl StickParams { } } -#[derive(Clone, Debug, Format, Copy)] +#[derive(Clone, Debug, Format, Copy, Eq, PartialEq)] pub enum NotchStatus { TertInactive, TertActive, @@ -387,6 +387,190 @@ impl NotchCalibration { } } +struct AppliedCalibration { + stick_params: StickParams, + cleaned_calibration: CleanedCalibrationPoints, + notch_angles: [f32; NO_OF_NOTCHES], + measured_notch_angles: [f32; NO_OF_NOTCHES], +} + +impl AppliedCalibration { + pub fn from_points( + cal_points_x: &[f32; NO_OF_CALIBRATION_POINTS], + cal_points_y: &[f32; NO_OF_CALIBRATION_POINTS], + stick_config: &StickConfig, + which_stick: Stick, + ) -> Self { + let mut stick_params = StickParams::from_stick_config(stick_config); + + let angles = stick_config.angles; + + let (stripped_cal_points_x, stripped_cal_points_y) = + strip_cal_points(cal_points_x, cal_points_y); + + let stripped_cleaned_calibration = CleanedCalibrationPoints::from_temp_calibration_points( + &stripped_cal_points_x, + &stripped_cal_points_y, + &DEFAULT_ANGLES, + ); + + let linearized_calibration = + LinearizedCalibration::from_calibration_points(&stripped_cleaned_calibration); + + stick_params.fit_coeffs = XyValuePair { + x: linearized_calibration.fit_coeffs.x.map(|e| e as f32), + y: linearized_calibration.fit_coeffs.y.map(|e| e as f32), + }; + + let notch_calibration = NotchCalibration::from_cleaned_and_linearized_calibration( + &stripped_cleaned_calibration, + &linearized_calibration, + ); + + stick_params.affine_coeffs = notch_calibration.affine_coeffs; + stick_params.boundary_angles = notch_calibration.boundary_angles; + + let original_cleaned_calibration = CleanedCalibrationPoints::from_temp_calibration_points( + cal_points_x, + cal_points_y, + &DEFAULT_ANGLES, + ); + + let (transformed_cal_points_x, transformed_cal_points_y) = transform_cal_points( + &original_cleaned_calibration.cleaned_points.x, + &original_cleaned_calibration.cleaned_points.y, + &stick_params, + stick_config, + ); + + let measured_notch_angles = + compute_stick_angles(&transformed_cal_points_x, &transformed_cal_points_y); + + let cleaned_with_measured_notch_angles = + CleanedCalibrationPoints::from_temp_calibration_points( + cal_points_x, + cal_points_y, + &measured_notch_angles, + ); + + let cleaned_notch_angles = clean_notches( + &measured_notch_angles, + &cleaned_with_measured_notch_angles.notch_status, + ); + + let cleaned_full = CleanedCalibrationPoints::from_temp_calibration_points( + cal_points_x, + cal_points_y, + &cleaned_notch_angles, + ); + + let linearized_full = LinearizedCalibration::from_calibration_points(&cleaned_full); + + stick_params.fit_coeffs = XyValuePair { + x: linearized_full.fit_coeffs.x.map(|e| e as f32), + y: linearized_full.fit_coeffs.y.map(|e| e as f32), + }; + + let notch_calibrate_full = NotchCalibration::from_cleaned_and_linearized_calibration( + &cleaned_full, + &linearized_full, + ); + + stick_params.affine_coeffs = notch_calibrate_full.affine_coeffs; + stick_params.boundary_angles = notch_calibrate_full.boundary_angles; + + Self { + stick_params, + measured_notch_angles, + notch_angles: cleaned_notch_angles, + cleaned_calibration: cleaned_full, + } + } +} + +/// Sets notches to measured values if absent. +fn clean_notches( + measured_notch_angles: &[f32; NO_OF_NOTCHES], + notch_status: &[NotchStatus; NO_OF_NOTCHES], +) -> [f32; NO_OF_NOTCHES] { + let mut out = [0f32; NO_OF_NOTCHES]; + + for i in 0..NO_OF_NOTCHES { + if notch_status[i] == NotchStatus::TertInactive { + out[i] = measured_notch_angles[i]; + } + } + + out +} + +fn angle_on_sphere(x: f32, y: f32) -> f32 { + let xx = sinf(x * MAX_STICK_ANGLE / 100.) * cosf(y * MAX_STICK_ANGLE / 100.); + let yy = cosf(x * MAX_STICK_ANGLE / 100.) * sinf(y * MAX_STICK_ANGLE / 100.); + match atan2f(yy, xx) { + a if a < 0. => a + 2. * PI, + a => a, + } +} + +fn compute_stick_angles( + x_in: &[f32; NO_OF_NOTCHES + 1], + y_in: &[f32; NO_OF_NOTCHES + 1], +) -> [f32; NO_OF_NOTCHES] { + let mut angles = [0f32; NO_OF_NOTCHES]; + + for i in 0..NO_OF_NOTCHES { + if i % 2 == 0 { + angles[i] = DEFAULT_ANGLES[i]; + } else { + angles[i] = angle_on_sphere(x_in[i], y_in[i]); + } + } + + angles +} + +fn transform_cal_points( + cal_points_x: &[f32; NO_OF_NOTCHES + 1], + cal_points_y: &[f32; NO_OF_NOTCHES + 1], + stick_params: &StickParams, + stick_config: &StickConfig, +) -> ([f32; NO_OF_NOTCHES + 1], [f32; NO_OF_NOTCHES + 1]) { + let mut transformed_points_x = [0f32; NO_OF_NOTCHES + 1]; + let mut transformed_points_y = [0f32; NO_OF_NOTCHES + 1]; + + for i in 0..NO_OF_NOTCHES + 1 { + let x = linearize(cal_points_x[i], &stick_params.fit_coeffs.x); + let y = linearize(cal_points_y[i], &stick_params.fit_coeffs.y); + let (out_x, out_y) = notch_remap(x, y, stick_params, stick_config, true); + transformed_points_x[i] = out_x; + transformed_points_y[i] = out_y; + } + + (transformed_points_x, transformed_points_y) +} +/// Removes the notches from un-cleaned cal points +/// so we can get the original values of the notches after the affine transform. +fn strip_cal_points( + cal_points_x: &[f32; NO_OF_CALIBRATION_POINTS], + cal_points_y: &[f32; NO_OF_CALIBRATION_POINTS], +) -> ( + [f32; NO_OF_CALIBRATION_POINTS], + [f32; NO_OF_CALIBRATION_POINTS], +) { + let mut stripped_points_x = [0f32; NO_OF_CALIBRATION_POINTS]; + let mut stripped_points_y = [0f32; NO_OF_CALIBRATION_POINTS]; + for i in 0..NO_OF_CALIBRATION_POINTS { + (stripped_points_x[i], stripped_points_y[i]) = if (i + 1) % 4 == 0 { + (cal_points_x[0], cal_points_y[0]) + } else { + (cal_points_x[i], cal_points_y[i]) + } + } + + (stripped_points_x, stripped_points_y) +} + fn inverse(in_mat: &[[f32; 3]; 3]) -> [[f32; 3]; 3] { let mut out_mat = [[0f32; 3]; 3]; @@ -561,7 +745,7 @@ fn fit_curve( /// 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) { +pub 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 = @@ -571,7 +755,7 @@ fn calc_stick_values(angle: f32) -> (f32, f32) { } #[link_section = ".time_critical.linearize"] -pub fn linearize(point: f32, coefficients: &[f32; 4]) -> f32 { +pub fn linearize(point: f32, coefficients: &[f32; NUM_COEFFS]) -> f32 { coefficients[0] * (point * point * point) + coefficients[1] * (point * point) + coefficients[2] * point @@ -583,8 +767,7 @@ pub fn notch_remap( x_in: f32, y_in: f32, stick_params: &StickParams, - controller_config: &ControllerConfig, - which_stick: Stick, + stick_config: &StickConfig, is_calibrating: bool, ) -> (f32, f32) { //determine the angle between the x unit vector and the current position vector @@ -606,10 +789,7 @@ pub fn notch_remap( NO_OF_NOTCHES - 1 }; - let stick_scale = match which_stick { - Stick::ControlStick => controller_config.astick_config.analog_scaler as f32 / 100., - Stick::CStick => controller_config.cstick_config.analog_scaler as f32 / 100., - }; + let stick_scale = stick_config.analog_scaler as f32 / 100.; let mut x_out = stick_scale * (stick_params.affine_coeffs[region][0] * x_in @@ -619,11 +799,6 @@ pub fn notch_remap( + stick_params.affine_coeffs[region][3] * y_in); if !is_calibrating { - let stick_config = match which_stick { - Stick::ControlStick => &controller_config.astick_config, - Stick::CStick => &controller_config.cstick_config, - }; - if stick_config.cardinal_snapping > 0 { if fabsf(x_out) < stick_config.cardinal_snapping as f32 + 0.5 && fabsf(y_out) >= 79.5 { x_out = 0.;