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

feat(calibrate): finish stick calibration logic

Browse source
This commit is contained in:
Naxdy 2024-03-29 23:42:05 +01:00
parent 4306cce840
commit 090a97b5a9
Signed by: Naxdy
GPG key ID: CC15075846BCE91B
3 changed files with 199 additions and 79 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

159
src/config.rs
View file

@ -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<CriticalSectionRawMutex, bool> = 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<OverrideStickState>,
> = Signal::new();
/// Dispatched when we want to override the GCC state for a short amount of time.
pub static SIGNAL_OVERRIDE_GCC_STATE: Signal<CriticalSectionRawMutex, OverrideGcReportInstruction> =
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<const N: usize>(
&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<const N: usize>(
&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,13 +479,13 @@ impl<'a> StickCalibrationProcess<'a> {
);
}
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.calibration_step >= NO_OF_CALIBRATION_POINTS as u8
&& self.applied_calibration.cleaned_calibration.notch_status[notch_idx]
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
{
@ -475,17 +503,73 @@ impl<'a> StickCalibrationProcess<'a> {
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;
}
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(&current_config, &mut flash, &mut gcc_subscriber).await;
info!("Exiting config mode.");

54
src/input.rs
View file

@ -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<CriticalSectionRawMutex, GcReport, 1, 2, 1> =
pub static CHANNEL_GCC_STATE: PubSubChannel<CriticalSectionRawMutex, GcReport, 1, 3, 1> =
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<ThreadModeRawMutex, Option<Output<'static, AnyP
Mutex::new(None);
const STICK_HYST_VAL: f32 = 0.3;
const FLOAT_ORIGIN: f32 = 127.5;
pub const FLOAT_ORIGIN: f32 = 127.5;
#[derive(Clone, Debug, Default)]
struct StickState {
ax: u8,
ay: u8,
cx: u8,
cy: u8,
#[derive(Clone, Debug, Default, Format)]
pub struct StickState {
pub ax: u8,
pub ay: u8,
pub cx: u8,
pub cy: u8,
}
#[derive(Clone, Debug, Default)]
@ -413,6 +416,8 @@ pub async fn update_button_state_task(
let gcc_publisher = CHANNEL_GCC_STATE.publisher().unwrap();
let mut override_stick_state: Option<OverrideStickState> = 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;
};
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 {

23
src/stick.rs
View file

@ -25,7 +25,7 @@ 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 ];
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,