From 84d531e80af46d8f0ca062a9ecae17ef9ba034ac Mon Sep 17 00:00:00 2001
From: Naxdy <naxdy@polskalemon.com>
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<ThreadModeRawMutex, bool> = Signal::new();
+/// Config change signalled to the stick task.
+pub static SIGNAL_CONFIG_CHANGE: Signal<ThreadModeRawMutex, ControllerConfig> = 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<f32> 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<f32> 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<f32> 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<f32> for PackedFloat {
+ fn add_assign(&mut self, rhs: f32) {
+ self.0 += rhs;
+ }
+}
pub trait ToRegularArray<const T: usize> {
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