Userman/NaxGCC-FW
1
0
Fork 0
forked from NaxdyOrg/NaxGCC-FW
Code Pull requests Activity

feat(calibration): implement notch adjustment

Browse source
This commit is contained in:
Naxdy 2024-04-01 23:54:08 +02:00
parent d5dbaaaa49
commit 84d531e80a
Signed by untrusted user: Naxdy
GPG key ID: CC15075846BCE91B
4 changed files with 113 additions and 42 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

69
src/config.rs
View file

@ -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;
}
};

8
src/gcc_hid.rs
View file

@ -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),

60
src/helpers.rs
View file

@ -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];

18
src/input.rs
View file

@ -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