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

chore(project): implement more xy value pairs

Browse source
This commit is contained in:
Naxdy 2024-03-27 21:41:33 +01:00
parent eb0ba44b1c
commit 0f11c659c4
Signed by untrusted user: Naxdy
GPG key ID: CC15075846BCE91B
3 changed files with 140 additions and 125 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/config.rs
View file

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

96
src/input.rs
View file

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

167
src/stick.rs
View file

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