From 090a97b5a97c8256a9e8067d43ab0b9eae974602 Mon Sep 17 00:00:00 2001 From: Naxdy Date: Fri, 29 Mar 2024 23:42:05 +0100 Subject: [PATCH] feat(calibrate): finish stick calibration logic --- src/config.rs | 195 ++++++++++++++++++++++++++++++++++++-------------- src/input.rs | 56 +++++++++++---- src/stick.rs | 27 +++---- 3 files changed, 199 insertions(+), 79 deletions(-) diff --git a/src/config.rs b/src/config.rs index ff7f1fd..870e6cd 100644 --- a/src/config.rs +++ b/src/config.rs @@ -4,7 +4,7 @@ */ use core::{cmp::min, f32::consts::PI}; -use defmt::{error, info, warn, Format}; +use defmt::{debug, error, info, warn, Format}; use embassy_rp::{ flash::{Async, Flash, ERASE_SIZE}, peripherals::FLASH, @@ -13,17 +13,20 @@ use packed_struct::{derive::PackedStruct, PackedStruct}; use crate::{ helpers::{PackedFloat, ToPackedFloatArray, XyValuePair}, - input::{read_ext_adc, Stick, StickAxis, SPI_ACS_SHARED, SPI_CCS_SHARED, SPI_SHARED}, + input::{ + read_ext_adc, Stick, StickAxis, StickState, FLOAT_ORIGIN, SPI_ACS_SHARED, SPI_CCS_SHARED, + SPI_SHARED, + }, stick::{ - legalize_notches, AppliedCalibration, NotchStatus, NOTCH_ADJUSTMENT_ORDER, - NO_OF_ADJ_NOTCHES, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES, + calc_stick_values, legalize_notches, AppliedCalibration, NotchStatus, CALIBRATION_ORDER, + NOTCH_ADJUSTMENT_ORDER, NO_OF_ADJ_NOTCHES, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES, }, ADDR_OFFSET, FLASH_SIZE, }; use embassy_sync::{ blocking_mutex::raw::{CriticalSectionRawMutex, RawMutex}, - pubsub::Subscriber, + pubsub::{PubSubBehavior, Subscriber}, signal::Signal, }; use embassy_time::Timer; @@ -34,6 +37,12 @@ use crate::{gcc_hid::GcReport, input::CHANNEL_GCC_STATE}; /// Initial status is assumed to be false. pub static SIGNAL_IS_CALIBRATING: Signal = Signal::new(); +/// Signal used to override the stick state in order to display desired stick positions during calibration. +pub static SIGNAL_OVERRIDE_STICK_STATE: Signal< + CriticalSectionRawMutex, + Option, +> = 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(); @@ -54,19 +63,9 @@ const CONFIG_MODE_ENTRY_COMBO: [AwaitableButtons; 4] = [ AwaitableButtons::Y, ]; -const LSTICK_CALIBRATION_COMBO: [AwaitableButtons; 4] = [ - AwaitableButtons::A, - AwaitableButtons::X, - AwaitableButtons::Y, - AwaitableButtons::L, -]; +const LSTICK_CALIBRATION_COMBO: [AwaitableButtons; 2] = [AwaitableButtons::A, AwaitableButtons::X]; -const RSTICK_CALIBRATION_COMBO: [AwaitableButtons; 4] = [ - AwaitableButtons::A, - AwaitableButtons::X, - AwaitableButtons::Y, - AwaitableButtons::R, -]; +const RSTICK_CALIBRATION_COMBO: [AwaitableButtons; 2] = [AwaitableButtons::A, AwaitableButtons::Y]; /// This doesn't need to be super fast, since it's only used /// in config mode. @@ -74,7 +73,7 @@ const BUTTON_POLL_INTERVAL_MILLIS: u64 = 20; /// This needs to be incremented for ANY change to ControllerConfig /// else we risk loading uninitialized memory. -pub const CONTROLLER_CONFIG_REVISION: u8 = 2; +pub const CONTROLLER_CONFIG_REVISION: u8 = 1; pub const DEFAULT_NOTCH_STATUS: [NotchStatus; NO_OF_NOTCHES] = [ NotchStatus::Cardinal, @@ -154,6 +153,13 @@ pub const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [ PI * 15. / 8., ]; +#[derive(Clone, Format)] +pub struct OverrideStickState { + pub x: u8, + pub y: u8, + pub which_stick: Stick, +} + #[derive(Clone, Copy, Debug, Format)] enum AwaitableButtons { A, @@ -201,11 +207,11 @@ impl Default for StickConfig { Self { x_waveshaping: 0, y_waveshaping: 0, - x_snapback: 0, - y_snapback: 0, + x_snapback: 4, + y_snapback: 4, x_smoothing: 0, y_smoothing: 0, - cardinal_snapping: 0, + cardinal_snapping: 6, analog_scaler: 100, cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(), cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(), @@ -275,6 +281,9 @@ trait WaitForButtonPress { /// Wait for a single button press. async fn wait_for_button_press(&mut self, button_to_wait_for: &AwaitableButtons); + /// Wait for a single button release. + async fn wait_for_button_release(&mut self, button_to_wait_for: &AwaitableButtons); + /// Wait for multiple buttons to be pressed simultaneously. async fn wait_for_simultaneous_button_presses( &mut self, @@ -309,6 +318,18 @@ impl<'a, T: RawMutex, const I: usize, const J: usize, const K: usize> WaitForBut } } + async fn wait_for_button_release(&mut self, button_to_wait_for: &AwaitableButtons) { + loop { + 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; + } + } + async fn wait_for_simultaneous_button_presses( &mut self, buttons_to_wait_for: &[AwaitableButtons; N], @@ -401,7 +422,12 @@ impl<'a> StickCalibrationProcess<'a> { } } - async fn calibration_advance(&mut self) { + async fn calibration_advance(&mut self) -> bool { + info!( + "Running calibration advance on stick {} at step {}", + self.which_stick, self.calibration_step + ); + let stick_config = match self.which_stick { Stick::ControlStick => &mut self.gcc_config.astick_config, Stick::CStick => &mut self.gcc_config.cstick_config, @@ -428,7 +454,9 @@ impl<'a> StickCalibrationProcess<'a> { x /= 128.; y /= 128.; - self.cal_points[self.calibration_step as usize] = XyValuePair { x, y }; + let idx = CALIBRATION_ORDER[self.calibration_step as usize]; + + self.cal_points[idx] = XyValuePair { x, y }; } self.calibration_step += 1; @@ -451,41 +479,97 @@ impl<'a> StickCalibrationProcess<'a> { ); } - let mut notch_idx = NOTCH_ADJUSTMENT_ORDER[min( - self.calibration_step - NO_OF_CALIBRATION_POINTS as u8, - NO_OF_ADJ_NOTCHES as u8 - 1, - ) as usize]; - - while self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8 - && self.applied_calibration.cleaned_calibration.notch_status[notch_idx] - == NotchStatus::TertInactive - && self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8 - { - stick_config.angles = *legalize_notches( - self.calibration_step as usize, - &self.applied_calibration.measured_notch_angles, - &stick_config.angles.map(|e| *e), - ) - .to_packed_float_array(); - - self.calibration_step += 1; - - notch_idx = NOTCH_ADJUSTMENT_ORDER[min( + if self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8 { + let mut notch_idx = NOTCH_ADJUSTMENT_ORDER[min( self.calibration_step - NO_OF_CALIBRATION_POINTS as u8, NO_OF_ADJ_NOTCHES as u8 - 1, ) as usize]; + + while self.applied_calibration.cleaned_calibration.notch_status[notch_idx] + == NotchStatus::TertInactive + && self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8 + { + stick_config.angles = *legalize_notches( + self.calibration_step as usize, + &self.applied_calibration.measured_notch_angles, + &stick_config.angles.map(|e| *e), + ) + .to_packed_float_array(); + + self.calibration_step += 1; + + notch_idx = NOTCH_ADJUSTMENT_ORDER[min( + self.calibration_step - NO_OF_CALIBRATION_POINTS as u8, + NO_OF_ADJ_NOTCHES as u8 - 1, + ) as usize]; + } } if self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8 { 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()); + + info!("Finished calibrating stick {}", self.which_stick); + + return true; } - info!("Finished calibrating stick {}", self.which_stick); + return false; } pub async fn calibrate_stick(&mut self) { - todo!() + info!("Beginning stick calibration for {}", self.which_stick); + + let mut gcc_subscriber = CHANNEL_GCC_STATE.subscriber().unwrap(); + SIGNAL_IS_CALIBRATING.signal(true); + + while { + if self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 { + let (x, y) = get_stick_display_coords(self.calibration_step as usize); + debug!( + "Raw display coords for step {}: {}, {}", + self.calibration_step, x, y + ); + SIGNAL_OVERRIDE_STICK_STATE.signal(Some(OverrideStickState { + x: x as u8, + y: y as u8, + which_stick: match self.which_stick { + Stick::ControlStick => Stick::CStick, + Stick::CStick => Stick::ControlStick, + }, + })); + } else { + // TODO: phob calls `adjustNotch` here + } + + gcc_subscriber + .wait_for_button_release(&AwaitableButtons::A) + .await; + + // Prevent accidental double presses + Timer::after_millis(20).await; + + gcc_subscriber + .wait_for_button_press(&AwaitableButtons::A) + .await; + + !self.calibration_advance().await + } {} + + SIGNAL_IS_CALIBRATING.signal(false); + SIGNAL_OVERRIDE_STICK_STATE.signal(None); + } +} + +fn get_stick_display_coords(current_step: usize) -> (f32, f32) { + let idx = CALIBRATION_ORDER[current_step]; + if idx % 2 != 0 { + let notch_idx = idx / 2; + match calc_stick_values(DEFAULT_ANGLES[notch_idx]) { + (x, y) => (x + FLOAT_ORIGIN, y + FLOAT_ORIGIN), + } + } else { + (127.5, 127.5) } } @@ -509,25 +593,29 @@ async fn configuration_main_loop< .await { selection => match selection { - 1 => { + 0 => { StickCalibrationProcess::new(&mut final_config, Stick::ControlStick) .calibrate_stick() - .await + .await; } - 2 => { + 1 => { StickCalibrationProcess::new(&mut final_config, Stick::CStick) .calibrate_stick() - .await + .await; } - _ => { - error!("Invalid selection: {}", selection); - break 'main; + s => { + error!("Invalid selection in config loop: {}", s); + continue; } }, }; final_config.write_to_flash(&mut flash).unwrap(); + + break 'main; } + + info!("Exiting config main loop."); } #[embassy_executor::task] @@ -564,6 +652,9 @@ pub async fn config_task( duration_ms: 1000, }); + // Wait for the user to release the buttons + Timer::after_millis(500).await; + configuration_main_loop(¤t_config, &mut flash, &mut gcc_subscriber).await; info!("Exiting config mode."); diff --git a/src/input.rs b/src/input.rs index 244e8cd..d588bad 100644 --- a/src/input.rs +++ b/src/input.rs @@ -20,7 +20,10 @@ use embassy_time::{Duration, Instant, Timer}; use libm::{fmaxf, fminf}; use crate::{ - config::{ControllerConfig, SIGNAL_IS_CALIBRATING, SIGNAL_OVERRIDE_GCC_STATE}, + config::{ + ControllerConfig, OverrideStickState, SIGNAL_IS_CALIBRATING, SIGNAL_OVERRIDE_GCC_STATE, + SIGNAL_OVERRIDE_STICK_STATE, + }, filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS}, gcc_hid::GcReport, helpers::XyValuePair, @@ -29,7 +32,7 @@ use crate::{ }; /// Used to send the button state to the usb task and the calibration task -pub static CHANNEL_GCC_STATE: PubSubChannel = +pub static CHANNEL_GCC_STATE: PubSubChannel = PubSubChannel::new(); /// Used to send the stick state from the stick task to the main input task @@ -46,14 +49,14 @@ pub static SPI_CCS_SHARED: Mutex = None; + loop { update_button_states( &mut gcc_state, @@ -438,13 +443,33 @@ pub async fn update_button_state_task( gcc_state.cstick_y = stick_state.cy; } + if let Some(override_stick_state_opt) = SIGNAL_OVERRIDE_STICK_STATE.try_take() { + debug!("Overridden stick state: {:?}", override_stick_state_opt); + override_stick_state = override_stick_state_opt; + } + // 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); + if let Some(override_state) = &override_stick_state { + let mut overriden_gcc_state = gcc_state.clone(); + match override_state.which_stick { + Stick::ControlStick => { + overriden_gcc_state.stick_x = override_state.x; + overriden_gcc_state.stick_y = override_state.y; + } + Stick::CStick => { + overriden_gcc_state.cstick_x = override_state.x; + overriden_gcc_state.cstick_y = override_state.y; + } + } + gcc_publisher.publish_immediate(overriden_gcc_state); + } else { + gcc_publisher.publish_immediate(gcc_state); + } // give other tasks a chance to do something yield_now().await; @@ -457,14 +482,17 @@ pub async fn update_button_state_task( /// 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>, - mut spi_acs: Output<'static, AnyPin>, - mut spi_ccs: Output<'static, AnyPin>, + spi: Spi<'static, SPI0, embassy_rp::spi::Blocking>, + spi_acs: Output<'static, AnyPin>, + spi_ccs: Output<'static, AnyPin>, controller_config: ControllerConfig, ) { + *SPI_SHARED.lock().await = Some(spi); + *SPI_ACS_SHARED.lock().await = Some(spi_acs); + *SPI_CCS_SHARED.lock().await = Some(spi_ccs); + let controlstick_params = StickParams::from_stick_config(&controller_config.astick_config); let cstick_params = StickParams::from_stick_config(&controller_config.cstick_config); - let filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config); let mut current_stick_state = StickState { diff --git a/src/stick.rs b/src/stick.rs index 8890751..73b7e83 100644 --- a/src/stick.rs +++ b/src/stick.rs @@ -23,9 +23,9 @@ const MAX_ORDER: usize = 20; const MAX_STICK_ANGLE: f32 = 0.4886921906; #[rustfmt::skip] -// right notch 1 up right notch 2 up notch 3 up left notch 4 left notch 5 down left notch 6 down notch 7 down right notch 8 -// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 -const CALIBRATION_ORDER: [usize; NO_OF_CALIBRATION_POINTS] = [ 0, 1, 8, 9, 16, 17, 24, 25, 4, 5, 12, 13, 20, 21, 28, 29, 2, 3, 6, 7, 10, 11, 14, 15, 18, 19, 22, 23, 26, 27, 30, 31 ]; +// right notch 1 up right notch 2 up notch 3 up left notch 4 left notch 5 down left notch 6 down notch 7 down right notch 8 +// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 +pub const CALIBRATION_ORDER: [usize; NO_OF_CALIBRATION_POINTS] = [ 0, 1, 8, 9, 16, 17, 24, 25, 4, 5, 12, 13, 20, 21, 28, 29, 2, 3, 6, 7, 10, 11, 14, 15, 18, 19, 22, 23, 26, 27, 30, 31 ]; #[rustfmt::skip] // up right up left down left down right notch 1 notch 2 notch 3 notch 4 notch 5 notch 6 notch 7 notch 8 @@ -498,29 +498,29 @@ pub fn legalize_notches( for i in current_step..44 { let idx = NOTCH_ADJUSTMENT_ORDER[i - NO_OF_CALIBRATION_POINTS]; - out[idx] = legalize_notch(idx, measured_notch_angles, &out); + out[idx] = legalize_notch(idx as isize, measured_notch_angles, &out); } out } fn legalize_notch( - idx: usize, + idx: isize, measured_notch_angles: &[f32; NO_OF_NOTCHES], notch_angles: &[f32; NO_OF_NOTCHES], ) -> f32 { let is_diagonal = (idx - 2) % 4 == 0; let prev_idx = if is_diagonal { - (idx - 2 + NO_OF_NOTCHES) % NO_OF_NOTCHES + (idx - 2 + NO_OF_NOTCHES as isize) % NO_OF_NOTCHES as isize } else { - (idx - 1 + NO_OF_NOTCHES) % NO_OF_NOTCHES - }; + (idx - 1 + NO_OF_NOTCHES as isize) % NO_OF_NOTCHES as isize + } as usize; let next_idx = if is_diagonal { - (idx + 2) % NO_OF_NOTCHES + (idx + 2) % NO_OF_NOTCHES as isize } else { - (idx + 1) % NO_OF_NOTCHES - }; + (idx + 1) % NO_OF_NOTCHES as isize + } as usize; let prev_angle = notch_angles[prev_idx]; let next_angle = match notch_angles[next_idx] { @@ -529,7 +529,7 @@ fn legalize_notch( }; let prev_meas_angle = measured_notch_angles[prev_idx]; - let this_meas_angle = measured_notch_angles[idx]; + let this_meas_angle = measured_notch_angles[idx as usize]; let next_meas_angle = match measured_notch_angles[next_idx] { a if a < prev_meas_angle => a + 2. * PI, a => a, @@ -575,7 +575,7 @@ fn legalize_notch( fminf( upper_distort_limit, - fmaxf(notch_angles[idx], lower_distort_limit), + fmaxf(notch_angles[idx as usize], lower_distort_limit), ) } @@ -853,6 +853,7 @@ pub fn linearize(point: f32, coefficients: &[f32; NUM_COEFFS]) -> f32 { + coefficients[3] } +// TODO: currently broken! #[link_section = ".time_critical.notch_remap"] pub fn notch_remap( x_in: f32,