From 84d531e80af46d8f0ca062a9ecae17ef9ba034ac Mon Sep 17 00:00:00 2001 From: Naxdy Date: Mon, 1 Apr 2024 23:54:08 +0200 Subject: [PATCH] feat(calibration): implement notch adjustment --- src/config.rs | 69 +++++++++++++++++++++++++------------------------- src/gcc_hid.rs | 8 +++++- src/helpers.rs | 60 +++++++++++++++++++++++++++++++++++++++++-- src/input.rs | 18 +++++++++---- 4 files changed, 113 insertions(+), 42 deletions(-) diff --git a/src/config.rs b/src/config.rs index 6f971c0..8fd1302 100644 --- a/src/config.rs +++ b/src/config.rs @@ -5,6 +5,7 @@ use core::{cmp::min, f32::consts::PI}; use defmt::{debug, error, info, warn, Format}; +use embassy_futures::yield_now; use embassy_rp::{ flash::{Async, Flash, ERASE_SIZE}, peripherals::FLASH, @@ -12,7 +13,7 @@ use embassy_rp::{ use packed_struct::{derive::PackedStruct, PackedStruct}; use crate::{ - helpers::{PackedFloat, ToPackedFloatArray, XyValuePair}, + helpers::{PackedFloat, ToPackedFloatArray, ToRegularArray, XyValuePair}, input::{ read_ext_adc, Stick, StickAxis, StickState, FLOAT_ORIGIN, SPI_ACS_SHARED, SPI_CCS_SHARED, SPI_SHARED, @@ -38,6 +39,9 @@ use crate::{gcc_hid::GcReport, input::CHANNEL_GCC_STATE}; /// Initial status is assumed to be false. pub static SIGNAL_IS_CALIBRATING: Signal = Signal::new(); +/// Config change signalled to the stick task. +pub static SIGNAL_CONFIG_CHANGE: 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, @@ -74,7 +78,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, @@ -458,30 +462,32 @@ impl<'a> StickCalibrationProcess<'a> { } } - fn adjust_notch(&mut self, notch_adjustment_type: NotchAdjustmentType) -> Option<(f32, f32)> { + fn adjust_notch(&mut self, notch_adjustment_type: NotchAdjustmentType) { + let stick_config = match self.which_stick { + Stick::ControlStick => &mut self.gcc_config.astick_config, + Stick::CStick => &mut self.gcc_config.cstick_config, + }; + let notch_idx = NOTCH_ADJUSTMENT_ORDER[self.calibration_step as usize - NO_OF_CALIBRATION_POINTS]; if self.applied_calibration.cleaned_calibration.notch_status[notch_idx] == NotchStatus::TertInactive - { - return None; - } + {} // assumes a tick rate of 1ms match notch_adjustment_type { NotchAdjustmentType::Clockwise => { - self.applied_calibration.notch_angles[notch_idx] -= 0.000075; + stick_config.angles[notch_idx] -= 0.0075; } NotchAdjustmentType::CounterClockwise => { - self.applied_calibration.notch_angles[notch_idx] += 0.000075; + stick_config.angles[notch_idx] += 0.0075; } NotchAdjustmentType::Reset => { - self.applied_calibration.notch_angles[notch_idx] = DEFAULT_ANGLES[notch_idx]; - } - NotchAdjustmentType::None => { - return None; + stick_config.angles[notch_idx] = + PackedFloat(self.applied_calibration.measured_notch_angles[notch_idx]); } + NotchAdjustmentType::None => {} } match notch_adjustment_type { @@ -492,7 +498,7 @@ impl<'a> StickCalibrationProcess<'a> { CleanedCalibrationPoints::from_temp_calibration_points( &self.cal_points.map(|e| e.x), &self.cal_points.map(|e| e.y), - &self.applied_calibration.notch_angles, + &self.applied_calibration.measured_notch_angles, ); let linearized_calibration = @@ -512,15 +518,18 @@ impl<'a> StickCalibrationProcess<'a> { notch_calibration.affine_coeffs; self.applied_calibration.stick_params.boundary_angles = notch_calibration.boundary_angles; - } - NotchAdjustmentType::None => return None, - } - Some( - match calc_stick_values(self.applied_calibration.measured_notch_angles[notch_idx]) { - (x, y) => (x + FLOAT_ORIGIN, y + FLOAT_ORIGIN), - }, - ) + stick_config.angles = *legalize_notches( + self.calibration_step as usize, + &self.applied_calibration.measured_notch_angles, + &stick_config.angles.to_regular_array(), + ) + .to_packed_float_array(); + + SIGNAL_CONFIG_CHANGE.signal(self.gcc_config.clone()); + } + NotchAdjustmentType::None => {} + } } async fn calibration_advance(&mut self) -> bool { @@ -593,7 +602,7 @@ impl<'a> StickCalibrationProcess<'a> { stick_config.angles = *legalize_notches( self.calibration_step as usize, &self.applied_calibration.measured_notch_angles, - &stick_config.angles.map(|e| *e), + &stick_config.angles.to_regular_array(), ) .to_packed_float_array(); @@ -610,6 +619,8 @@ impl<'a> StickCalibrationProcess<'a> { 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()); + SIGNAL_CONFIG_CHANGE.signal(self.gcc_config.clone()); + info!("Finished calibrating stick {}", self.which_stick); return true; @@ -687,7 +698,7 @@ impl<'a> StickCalibrationProcess<'a> { AwaitableButtons::Y, ]); - let override_result = match btn_result { + match btn_result { Some(btn) => match btn { AwaitableButtons::A => { debug!("Btn A release pressed"); @@ -705,18 +716,8 @@ impl<'a> StickCalibrationProcess<'a> { None => self.adjust_notch(NotchAdjustmentType::None), }; - if let Some((x, y)) = override_result { - 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, - }, - })); - } - ticker.next().await; + yield_now().await; } }; diff --git a/src/gcc_hid.rs b/src/gcc_hid.rs index 499d84e..da2f34a 100644 --- a/src/gcc_hid.rs +++ b/src/gcc_hid.rs @@ -360,7 +360,13 @@ pub async fn usb_transfer_task( trace!("Report Written: {:08b}", report); let currtime = Instant::now(); let polltime = currtime.duration_since(lasttime); - trace!("Report written in {}us", polltime.as_micros()); + let micros = polltime.as_micros(); + debug!("Report written in {}us", micros); + // If we're sending reports too fast, reset the ticker. + // This might happen right after plug-in, or after suspend. + if micros < 8150 { + ticker.reset(); + } lasttime = currtime; } Err(e) => warn!("Failed to send report: {:?}", e), diff --git a/src/helpers.rs b/src/helpers.rs index cde645d..4a41a7d 100644 --- a/src/helpers.rs +++ b/src/helpers.rs @@ -1,11 +1,67 @@ -use core::ops::Deref; +use core::ops::{Add, AddAssign, Deref, Sub, SubAssign}; 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 struct PackedFloat(pub f32); + +impl Add for PackedFloat { + type Output = Self; + + fn add(self, rhs: Self) -> Self::Output { + Self(self.0 + rhs.0) + } +} + +impl Add for PackedFloat { + type Output = Self; + + fn add(self, rhs: f32) -> Self::Output { + Self(self.0 + rhs) + } +} + +impl Sub for PackedFloat { + type Output = Self; + + fn sub(self, rhs: Self) -> Self::Output { + Self(self.0 - rhs.0) + } +} + +impl Sub for PackedFloat { + type Output = Self; + + fn sub(self, rhs: f32) -> Self::Output { + Self(self.0 - rhs) + } +} + +impl SubAssign for PackedFloat { + fn sub_assign(&mut self, rhs: Self) { + self.0 -= rhs.0; + } +} + +impl SubAssign for PackedFloat { + fn sub_assign(&mut self, rhs: f32) { + self.0 -= rhs; + } +} + +impl AddAssign for PackedFloat { + fn add_assign(&mut self, rhs: Self) { + self.0 += rhs.0; + } +} + +impl AddAssign for PackedFloat { + fn add_assign(&mut self, rhs: f32) { + self.0 += rhs; + } +} pub trait ToRegularArray { fn to_regular_array(&self) -> &[f32; T]; diff --git a/src/input.rs b/src/input.rs index 30fbe9c..1defb1e 100644 --- a/src/input.rs +++ b/src/input.rs @@ -21,8 +21,8 @@ use libm::{fmaxf, fminf}; use crate::{ config::{ - ControllerConfig, OverrideGcReportInstruction, OverrideStickState, SIGNAL_IS_CALIBRATING, - SIGNAL_OVERRIDE_GCC_STATE, SIGNAL_OVERRIDE_STICK_STATE, + ControllerConfig, OverrideGcReportInstruction, OverrideStickState, SIGNAL_CONFIG_CHANGE, + SIGNAL_IS_CALIBRATING, SIGNAL_OVERRIDE_GCC_STATE, SIGNAL_OVERRIDE_STICK_STATE, }, filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS}, gcc_hid::GcReport, @@ -506,9 +506,10 @@ pub async fn update_stick_states_task( *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 controller_config = controller_config; + let mut controlstick_params = StickParams::from_stick_config(&controller_config.astick_config); + let mut cstick_params = StickParams::from_stick_config(&controller_config.cstick_config); + let mut filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config); info!("Controlstick params: {:?}", controlstick_params); @@ -572,6 +573,13 @@ pub async fn update_stick_states_task( yield_now().await; } + if let Some(new_config) = SIGNAL_CONFIG_CHANGE.try_take() { + controller_config = new_config; + controlstick_params = StickParams::from_stick_config(&controller_config.astick_config); + cstick_params = StickParams::from_stick_config(&controller_config.cstick_config); + filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config); + } + #[cfg(debug_assertions)] { // give other tasks a chance to do something