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<f32>,
+ c_linearized: XyValuePair<f32>,
+ a_raw: XyValuePair<f32>,
+ c_raw: XyValuePair<f32>,
+ a_unfiltered: XyValuePair<f32>,
+ c_unfiltered: XyValuePair<f32>,
}
#[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,