diff --git a/src/config.rs b/src/config.rs index aea4871..7943b75 100644 --- a/src/config.rs +++ b/src/config.rs @@ -18,7 +18,7 @@ use crate::{ /// 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, diff --git a/src/input.rs b/src/input.rs index 86074e4..1a3393a 100644 --- a/src/input.rs +++ b/src/input.rs @@ -1,4 +1,4 @@ -use defmt::info; +use defmt::{debug, info}; use embassy_futures::{join::join, yield_now}; use embassy_rp::{ flash::{Async, Flash}, @@ -18,6 +18,7 @@ use crate::{ config::ControllerConfig, filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS}, gcc_hid::GcReport, + helpers::XyValuePair, stick::{linearize, notch_remap, StickParams}, FLASH_SIZE, }; @@ -46,18 +47,12 @@ struct StickPositions { #[derive(Clone, Debug, Default)] struct RawStickValues { - ax_linearized: f32, - ay_linearized: f32, - cx_linearized: f32, - cy_linearized: f32, - ax_raw: f32, - ay_raw: f32, - cx_raw: f32, - cy_raw: f32, - ax_unfiltered: f32, - ay_unfiltered: f32, - cx_unfiltered: f32, - cy_unfiltered: f32, + a_linearized: XyValuePair, + c_linearized: XyValuePair, + a_raw: XyValuePair, + c_raw: XyValuePair, + a_unfiltered: XyValuePair, + c_unfiltered: XyValuePair, } #[derive(PartialEq, Eq)] @@ -85,7 +80,7 @@ fn read_ext_adc<'a, Acs: Pin, Ccs: Pin, I: embassy_rp::spi::Instance, M: embassy buf = [0b11110000; 3]; } - if which_stick == Stick::CStick { + if which_stick == Stick::ControlStick { spi_acs.set_low(); } else { spi_ccs.set_low(); @@ -135,9 +130,7 @@ async fn update_stick_states< // TODO: lower interval possible? - let end_time = Instant::now() + embassy_time::Duration::from_millis(1); - let timer = Timer::at(end_time); - + let end_time = Instant::now() + embassy_time::Duration::from_micros(500); let mut loop_time = Duration::from_millis(0); while Instant::now() < end_time - loop_time { @@ -173,36 +166,38 @@ async fn update_stick_states< &mut spi_ccs, ) as u32; - loop_time = Instant::now() - loop_start; - // with this, we can poll the sticks at 1000Hz (ish), while updating // the rest of the controller (the buttons) much faster, to ensure // better input integrity for button inputs. yield_now().await; + loop_time = Instant::now() - loop_start; } - timer.await; + let raw_controlstick = XyValuePair { + x: (ax_sum as f32) / (adc_count as f32) / 4096.0f32, + y: (ay_sum as f32) / (adc_count as f32) / 4096.0f32, + }; + let raw_cstick = XyValuePair { + x: (cx_sum as f32) / (adc_count as f32) / 4096.0f32, + y: (cy_sum as f32) / (adc_count as f32) / 4096.0f32, + }; - let raw_controlstick_x = (ax_sum as f32) / (adc_count as f32) / 4096.0f32; - let raw_controlstick_y = (ay_sum as f32) / (adc_count as f32) / 4096.0f32; - let raw_cstick_x = (cx_sum as f32) / (adc_count as f32) / 4096.0f32; - let raw_cstick_y = (cy_sum as f32) / (adc_count as f32) / 4096.0f32; + debug!("Raw Control Stick: {:?}", raw_controlstick); + debug!("Raw C Stick: {:?}", raw_cstick); - raw_stick_values.ax_raw = raw_controlstick_x; - raw_stick_values.ay_raw = raw_controlstick_y; - raw_stick_values.cx_raw = raw_cstick_x; - raw_stick_values.cy_raw = raw_cstick_y; + raw_stick_values.a_raw = raw_controlstick; + raw_stick_values.c_raw = raw_cstick; - let x_z = linearize(raw_controlstick_x, &controlstick_params.fit_coeffs_x); - let y_z = linearize(raw_controlstick_y, &controlstick_params.fit_coeffs_y); + let x_z = linearize(raw_controlstick.x, &controlstick_params.fit_coeffs.x); + let y_z = linearize(raw_controlstick.y, &controlstick_params.fit_coeffs.y); - let pos_cx = linearize(raw_cstick_x, &cstick_params.fit_coeffs_x); - let pos_cy = linearize(raw_cstick_y, &cstick_params.fit_coeffs_y); + let pos_cx = linearize(raw_cstick.x, &cstick_params.fit_coeffs.x); + let pos_cy = linearize(raw_cstick.y, &cstick_params.fit_coeffs.y); - raw_stick_values.ax_linearized = x_z; - raw_stick_values.ay_linearized = y_z; - raw_stick_values.cx_linearized = pos_cx; - raw_stick_values.cy_linearized = pos_cy; + raw_stick_values.a_linearized.x = x_z; + raw_stick_values.a_linearized.y = y_z; + raw_stick_values.c_linearized.x = pos_cx; + raw_stick_values.c_linearized.y = pos_cy; let (x_pos_filt, y_pos_filt) = kalman_state.run_kalman(x_z, y_z, &controller_config.astick_config, &filter_gains); @@ -262,15 +257,15 @@ async fn update_stick_states< Stick::CStick, ); let (remapped_x_unfiltered, remapped_y_unfiltered) = notch_remap( - raw_stick_values.ax_linearized, - raw_stick_values.ay_linearized, + raw_stick_values.a_linearized.x, + raw_stick_values.a_linearized.y, controlstick_params, controller_config, Stick::ControlStick, ); let (remapped_cx_unfiltered, remapped_cy_unfiltered) = notch_remap( - raw_stick_values.cx_linearized, - raw_stick_values.cy_linearized, + raw_stick_values.c_linearized.x, + raw_stick_values.c_linearized.y, cstick_params, controller_config, Stick::CStick, @@ -280,10 +275,10 @@ async fn update_stick_states< remapped_y = fminf(125., fmaxf(-125., remapped_y)); remapped_cx = fminf(125., fmaxf(-125., remapped_cx)); remapped_cy = fminf(125., fmaxf(-125., remapped_cy)); - raw_stick_values.ax_unfiltered = fminf(125., fmaxf(-125., remapped_x_unfiltered)); - raw_stick_values.ay_unfiltered = fminf(125., fmaxf(-125., remapped_y_unfiltered)); - raw_stick_values.cx_unfiltered = fminf(125., fmaxf(-125., remapped_cx_unfiltered)); - raw_stick_values.cy_unfiltered = fminf(125., fmaxf(-125., remapped_cy_unfiltered)); + raw_stick_values.a_unfiltered.x = fminf(125., fmaxf(-125., remapped_x_unfiltered)); + raw_stick_values.a_unfiltered.y = fminf(125., fmaxf(-125., remapped_y_unfiltered)); + raw_stick_values.c_unfiltered.x = fminf(125., fmaxf(-125., remapped_cx_unfiltered)); + raw_stick_values.c_unfiltered.y = fminf(125., fmaxf(-125., remapped_cy_unfiltered)); let mut out_stick_state = current_stick_state.clone(); @@ -405,7 +400,11 @@ pub async fn input_loop( let mut controlstick_waveshaping_values = WaveshapingValues::default(); let mut kalman_state = KalmanState::default(); + let mut last_loop_time = Instant::now(); + loop { + let timer = Timer::after_millis(1); + current_stick_state = update_stick_states( &mut spi, &mut spi_acs, @@ -423,6 +422,15 @@ pub async fn input_loop( ) .await; + timer.await; + + debug!( + "Loop took {} us", + (Instant::now() - last_loop_time).as_micros() + ); + + last_loop_time = Instant::now(); + STICK_SIGNAL.signal(current_stick_state.clone()); } }; diff --git a/src/stick.rs b/src/stick.rs index 11c69f8..42554a4 100644 --- a/src/stick.rs +++ b/src/stick.rs @@ -7,7 +7,7 @@ use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf}; use crate::{ config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS}, - helpers::ToRegularArray, + helpers::{ToRegularArray, XyValuePair}, input::Stick, }; @@ -34,8 +34,7 @@ const NOTCH_ADJUSTMENT_ORDER: [usize; NO_OF_ADJ_NOTCHES] = [2, 6, #[derive(Clone, Debug, Default, Format)] pub struct StickParams { // these are the linearization coefficients - pub fit_coeffs_x: [f32; NUM_COEFFS], - pub fit_coeffs_y: [f32; NUM_COEFFS], + pub fit_coeffs: XyValuePair<[f32; NUM_COEFFS]>, // these are the notch remap parameters pub affine_coeffs: [[f32; 4]; 16], // affine transformation coefficients for all regions of the stick @@ -59,8 +58,10 @@ impl StickParams { ); Self { - fit_coeffs_x: linearized_cal.fit_coeffs_x.map(|e| e as f32), - fit_coeffs_y: linearized_cal.fit_coeffs_y.map(|e| e as f32), + fit_coeffs: XyValuePair { + x: linearized_cal.fit_coeffs.x.map(|e| e as f32), + y: linearized_cal.fit_coeffs.y.map(|e| e as f32), + }, affine_coeffs: notch_cal.affine_coeffs, boundary_angles: notch_cal.boundary_angles, } @@ -77,20 +78,22 @@ pub enum NotchStatus { #[derive(Clone, Debug)] struct CleanedCalibrationPoints { - pub cleaned_points_x: [f32; NO_OF_NOTCHES + 1], - pub cleaned_points_y: [f32; NO_OF_NOTCHES + 1], - pub notch_points_x: [f32; NO_OF_NOTCHES + 1], - pub notch_points_y: [f32; NO_OF_NOTCHES + 1], + pub cleaned_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>, + pub notch_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>, pub notch_status: [NotchStatus; NO_OF_NOTCHES], } impl Default for CleanedCalibrationPoints { fn default() -> Self { Self { - cleaned_points_x: [0f32; NO_OF_NOTCHES + 1], - cleaned_points_y: [0f32; NO_OF_NOTCHES + 1], - notch_points_x: [0f32; NO_OF_NOTCHES + 1], - notch_points_y: [0f32; NO_OF_NOTCHES + 1], + cleaned_points: XyValuePair { + x: [0f32; NO_OF_NOTCHES + 1], + y: [0f32; NO_OF_NOTCHES + 1], + }, + notch_points: XyValuePair { + x: [0f32; NO_OF_NOTCHES + 1], + y: [0f32; NO_OF_NOTCHES + 1], + }, notch_status: DEFAULT_NOTCH_STATUS, } } @@ -113,14 +116,14 @@ impl CleanedCalibrationPoints { for i in 0..NO_OF_NOTCHES { // add the origin values to the first x,y point - out.cleaned_points_x[0] += cal_points_x[i * 2]; - out.cleaned_points_y[0] += cal_points_y[i * 2]; + out.cleaned_points.x[0] += cal_points_x[i * 2]; + out.cleaned_points.y[0] += cal_points_y[i * 2]; // copy the cal point into the cleaned list - out.cleaned_points_x[i + 1] = cal_points_x[i * 2 + 1]; - out.cleaned_points_y[i + 1] = cal_points_y[i * 2 + 1]; + out.cleaned_points.x[i + 1] = cal_points_x[i * 2 + 1]; + out.cleaned_points.y[i + 1] = cal_points_y[i * 2 + 1]; - (out.notch_points_x[i + 1], out.notch_points_y[i + 1]) = + (out.notch_points.x[i + 1], out.notch_points.y[i + 1]) = match calc_stick_values(notch_angles[i]) { (a, b) => (roundf(a), roundf(b)), }; @@ -157,22 +160,22 @@ impl CleanedCalibrationPoints { let largest_y = y_by_size[y_by_size.len() - 1].0; // TODO: make this whole thing a function? it looks very ugly - out.cleaned_points_x[0] -= cal_points_x[smallest_x]; - out.cleaned_points_x[0] -= cal_points_x[small_x]; - out.cleaned_points_x[0] -= cal_points_x[large_x]; - out.cleaned_points_x[0] -= cal_points_x[largest_x]; + out.cleaned_points.x[0] -= cal_points_x[smallest_x]; + out.cleaned_points.x[0] -= cal_points_x[small_x]; + out.cleaned_points.x[0] -= cal_points_x[large_x]; + out.cleaned_points.x[0] -= cal_points_x[largest_x]; - out.cleaned_points_y[0] -= cal_points_y[smallest_y]; - out.cleaned_points_y[0] -= cal_points_y[small_y]; - out.cleaned_points_y[0] -= cal_points_y[large_y]; - out.cleaned_points_y[0] -= cal_points_y[largest_y]; + out.cleaned_points.y[0] -= cal_points_y[smallest_y]; + out.cleaned_points.y[0] -= cal_points_y[small_y]; + out.cleaned_points.y[0] -= cal_points_y[large_y]; + out.cleaned_points.y[0] -= cal_points_y[largest_y]; - out.cleaned_points_x[0] /= (NO_OF_NOTCHES - 4) as f32; - out.cleaned_points_y[0] /= (NO_OF_NOTCHES - 4) as f32; + out.cleaned_points.x[0] /= (NO_OF_NOTCHES - 4) as f32; + out.cleaned_points.y[0] /= (NO_OF_NOTCHES - 4) as f32; for i in 0..NO_OF_NOTCHES { - let delta_x = out.cleaned_points_x[i + 1] - out.cleaned_points_x[0]; - let delta_y = out.cleaned_points_y[i + 1] - out.cleaned_points_y[0]; + let delta_x = out.cleaned_points.x[i + 1] - out.cleaned_points.x[0]; + let delta_y = out.cleaned_points.y[i + 1] - out.cleaned_points.y[0]; let mag = sqrtf(delta_x * delta_x + delta_y * delta_y); // if the cleaned point was at the center and would be a firefox notch @@ -181,15 +184,15 @@ impl CleanedCalibrationPoints { let prev_index = ((i + NO_OF_NOTCHES - 1) % NO_OF_NOTCHES) + 1; let next_index = ((i + 1) % NO_OF_NOTCHES) + 1; - out.cleaned_points_x[i + 1] = - (out.cleaned_points_x[prev_index] + out.cleaned_points_x[next_index]) / 2.0; - out.cleaned_points_y[i + 1] = - (out.cleaned_points_y[prev_index] + out.cleaned_points_y[next_index]) / 2.0; + out.cleaned_points.x[i + 1] = + (out.cleaned_points.x[prev_index] + out.cleaned_points.x[next_index]) / 2.0; + out.cleaned_points.y[i + 1] = + (out.cleaned_points.y[prev_index] + out.cleaned_points.y[next_index]) / 2.0; - out.notch_points_x[i + 1] = - (out.notch_points_x[prev_index] + out.notch_points_x[next_index]) / 2.0; - out.notch_points_y[i + 1] = - (out.notch_points_y[prev_index] + out.notch_points_y[next_index]) / 2.0; + out.notch_points.x[i + 1] = + (out.notch_points.x[prev_index] + out.notch_points.x[next_index]) / 2.0; + out.notch_points.y[i + 1] = + (out.notch_points.y[prev_index] + out.notch_points.y[next_index]) / 2.0; debug!("Skipping notch {}", i + 1); @@ -204,10 +207,10 @@ impl CleanedCalibrationPoints { for i in 0..=NO_OF_NOTCHES { debug!( "Cleaned: ({}, {}), Notch: ({}, {})", - out.cleaned_points_x[i], - out.cleaned_points_y[i], - out.notch_points_x[i], - out.notch_points_y[i], + out.cleaned_points.x[i], + out.cleaned_points.y[i], + out.notch_points.x[i], + out.notch_points.y[i], ); } @@ -219,11 +222,9 @@ impl CleanedCalibrationPoints { #[derive(Clone, Debug, Default)] struct LinearizedCalibration { - pub fit_coeffs_x: [f64; NUM_COEFFS], - pub fit_coeffs_y: [f64; NUM_COEFFS], + pub fit_coeffs: XyValuePair<[f64; NUM_COEFFS]>, - pub linearized_points_x: [f32; NO_OF_NOTCHES + 1], - pub linearized_points_y: [f32; NO_OF_NOTCHES + 1], + pub linearized_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>, } impl LinearizedCalibration { @@ -240,10 +241,12 @@ impl LinearizedCalibration { let mut fit_points_y = [0f64; 5]; let in_x = cleaned_calibration_points - .cleaned_points_x + .cleaned_points + .x .map(|e| e as f64); let in_y = cleaned_calibration_points - .cleaned_points_y + .cleaned_points + .y .map(|e| e as f64); fit_points_x[0] = in_x[8 + 1]; @@ -281,10 +284,14 @@ impl LinearizedCalibration { } Self { - fit_coeffs_x, - fit_coeffs_y, - linearized_points_x, - linearized_points_y, + fit_coeffs: XyValuePair { + x: fit_coeffs_x, + y: fit_coeffs_y, + }, + linearized_points: XyValuePair { + x: linearized_points_x, + y: linearized_points_y, + }, } } } @@ -307,40 +314,40 @@ impl NotchCalibration { let mut points_out = [[0f32; 3]; 3]; if i == NO_OF_NOTCHES { - points_in[0][0] = linearized_calibration.linearized_points_x[0]; - points_in[0][1] = linearized_calibration.linearized_points_x[i]; - points_in[0][2] = linearized_calibration.linearized_points_x[1]; - points_in[1][0] = linearized_calibration.linearized_points_y[0]; - points_in[1][1] = linearized_calibration.linearized_points_y[i]; - points_in[1][2] = linearized_calibration.linearized_points_y[1]; + points_in[0][0] = linearized_calibration.linearized_points.x[0]; + points_in[0][1] = linearized_calibration.linearized_points.x[i]; + points_in[0][2] = linearized_calibration.linearized_points.x[1]; + points_in[1][0] = linearized_calibration.linearized_points.y[0]; + points_in[1][1] = linearized_calibration.linearized_points.y[i]; + points_in[1][2] = linearized_calibration.linearized_points.y[1]; points_in[2][0] = 1.; points_in[2][1] = 1.; points_in[2][2] = 1.; - points_out[0][0] = cleaned_calibration_points.notch_points_x[0]; - points_out[0][1] = cleaned_calibration_points.notch_points_x[i]; - points_out[0][2] = cleaned_calibration_points.notch_points_x[1]; - points_out[1][0] = cleaned_calibration_points.notch_points_y[0]; - points_out[1][1] = cleaned_calibration_points.notch_points_y[i]; - points_out[1][2] = cleaned_calibration_points.notch_points_y[1]; + points_out[0][0] = cleaned_calibration_points.notch_points.x[0]; + points_out[0][1] = cleaned_calibration_points.notch_points.x[i]; + points_out[0][2] = cleaned_calibration_points.notch_points.x[1]; + points_out[1][0] = cleaned_calibration_points.notch_points.y[0]; + points_out[1][1] = cleaned_calibration_points.notch_points.y[i]; + points_out[1][2] = cleaned_calibration_points.notch_points.y[1]; points_out[2][0] = 1.; points_out[2][1] = 1.; points_out[2][2] = 1.; } else { - points_in[0][0] = linearized_calibration.linearized_points_x[0]; - points_in[0][1] = linearized_calibration.linearized_points_x[i]; - points_in[0][2] = linearized_calibration.linearized_points_x[i + 1]; - points_in[1][0] = linearized_calibration.linearized_points_y[0]; - points_in[1][1] = linearized_calibration.linearized_points_y[i]; - points_in[1][2] = linearized_calibration.linearized_points_y[i + 1]; + points_in[0][0] = linearized_calibration.linearized_points.x[0]; + points_in[0][1] = linearized_calibration.linearized_points.x[i]; + points_in[0][2] = linearized_calibration.linearized_points.x[i + 1]; + points_in[1][0] = linearized_calibration.linearized_points.y[0]; + points_in[1][1] = linearized_calibration.linearized_points.y[i]; + points_in[1][2] = linearized_calibration.linearized_points.y[i + 1]; points_in[2][0] = 1.; points_in[2][1] = 1.; points_in[2][2] = 1.; - points_out[0][0] = cleaned_calibration_points.notch_points_x[0]; - points_out[0][1] = cleaned_calibration_points.notch_points_x[i]; - points_out[0][2] = cleaned_calibration_points.notch_points_x[i + 1]; - points_out[1][0] = cleaned_calibration_points.notch_points_y[0]; - points_out[1][1] = cleaned_calibration_points.notch_points_y[i]; - points_out[1][2] = cleaned_calibration_points.notch_points_y[i + 1]; + points_out[0][0] = cleaned_calibration_points.notch_points.x[0]; + points_out[0][1] = cleaned_calibration_points.notch_points.x[i]; + points_out[0][2] = cleaned_calibration_points.notch_points.x[i + 1]; + points_out[1][0] = cleaned_calibration_points.notch_points.y[0]; + points_out[1][1] = cleaned_calibration_points.notch_points.y[i]; + points_out[1][2] = cleaned_calibration_points.notch_points.y[i + 1]; points_out[2][0] = 1.; points_out[2][1] = 1.; points_out[2][2] = 1.; @@ -366,10 +373,10 @@ impl NotchCalibration { ); out.boundary_angles[i - 1] = match atan2f( - linearized_calibration.linearized_points_y[i] - - linearized_calibration.linearized_points_y[0], - linearized_calibration.linearized_points_x[i] - - linearized_calibration.linearized_points_x[0], + linearized_calibration.linearized_points.y[i] + - linearized_calibration.linearized_points.y[0], + linearized_calibration.linearized_points.x[i] + - linearized_calibration.linearized_points.x[0], ) { a if a < out.boundary_angles[0] => a + 2. * PI, a => a,