NaxdyOrg/NaxGCC-FW
2
0
Fork 2
Code Issues Pull requests 1 Releases 4 Activity Actions

finish up the stick read function (minus calibration steps)

Browse source
This commit is contained in:
Naxdy 2024-03-22 22:33:27 +01:00
parent baa1ab1235
commit 71aab17866
No known key found for this signature in database
GPG key ID: C0437AAE9755550F
2 changed files with 126 additions and 15 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

83
src/input.rs
View file

@ -14,18 +14,21 @@ use embassy_rp::{
};
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
use embassy_time::{Instant, Timer};
use libm::{fmaxf, fmin, fminf};
use packed_struct::derive::PackedStruct;
use crate::{
filter::{run_waveshaping, WaveshapingValues},
gcc_hid::GcReport,
stick::{linearize, run_kalman, FilterGains, StickParams},
stick::{linearize, notch_remap, run_kalman, FilterGains, StickParams},
PackedFloat, ADDR_OFFSET, FLASH_SIZE,
};
pub static GCC_SIGNAL: Signal<CriticalSectionRawMutex, GcReport> = Signal::new();
static STICK_SIGNAL: Signal<CriticalSectionRawMutex, StickState> = Signal::new();
static STICK_HYST_VAL: f32 = 0.3;
static FLOAT_ORIGIN: f32 = 127.5;
#[derive(Debug, Clone, Default, Format, PackedStruct)]
#[packed_struct(endian = "msb")]
@ -40,8 +43,13 @@ pub struct ControllerConfig {
pub cx_waveshaping: u8,
#[packed_field(size_bits = "8")]
pub cy_waveshaping: u8,
#[packed_field(size_bits = "8")]
pub astick_analog_scaler: u8,
#[packed_field(size_bits = "8")]
pub cstick_analog_scaler: u8,
}
#[derive(Clone, Debug, Default)]
struct StickState {
ax: u8,
ay: u8,
@ -127,6 +135,7 @@ async fn update_stick_states<
mut spi: &mut Spi<'a, I, M>,
mut spi_acs: &mut Output<'a, Acs>,
mut spi_ccs: &mut Output<'a, Ccs>,
current_stick_state: &StickState,
controlstick_params: &StickParams,
cstick_params: &StickParams,
controller_config: &ControllerConfig,
@ -135,7 +144,7 @@ async fn update_stick_states<
cstick_waveshaping_values: &mut WaveshapingValues,
old_stick_pos: &mut StickPositions,
raw_stick_values: &mut RawStickValues,
) {
) -> StickState {
let mut adc_count = 0u32;
let mut ax_sum = 0u32;
let mut ay_sum = 0u32;
@ -216,7 +225,7 @@ async fn update_stick_states<
filter_gains,
);
let pos_x =
let pos_x: f32 =
filter_gains.x_smoothing * shaped_x + (1.0 - filter_gains.x_smoothing) * old_stick_pos.x;
let pos_y =
filter_gains.y_smoothing * shaped_y + (1.0 - filter_gains.y_smoothing) * old_stick_pos.y;
@ -247,12 +256,65 @@ async fn update_stick_states<
// phob optionally runs a median filter here, but we leave it for now
STICK_SIGNAL.signal(StickState {
ax: 127,
ay: 127,
cx: 127,
cy: 127,
})
let (mut remapped_x, mut remapped_y) = notch_remap(
pos_x,
pos_y,
controlstick_params,
controller_config,
Stick::ControlStick,
);
let (mut remapped_cx, mut remapped_cy) = notch_remap(
pos_cx_filt,
pos_cy_filt,
cstick_params,
controller_config,
Stick::CStick,
);
let (remapped_x_unfiltered, remapped_y_unfiltered) = notch_remap(
raw_stick_values.ax_linearized,
raw_stick_values.ay_linearized,
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,
cstick_params,
controller_config,
Stick::CStick,
);
remapped_x = fminf(125., fmaxf(-125., remapped_x));
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));
let mut out_stick_state = current_stick_state.clone();
let diff_x = (remapped_x + FLOAT_ORIGIN) - current_stick_state.ax as f32;
if (diff_x > (1.0 + STICK_HYST_VAL)) || (diff_x < -STICK_HYST_VAL) {
out_stick_state.ax = (remapped_x + FLOAT_ORIGIN) as u8;
}
let diff_y = (remapped_y + FLOAT_ORIGIN) - current_stick_state.ay as f32;
if (diff_y > (1.0 + STICK_HYST_VAL)) || (diff_y < -STICK_HYST_VAL) {
out_stick_state.ay = (remapped_y + FLOAT_ORIGIN) as u8;
}
let diff_cx = (remapped_cx + FLOAT_ORIGIN) - current_stick_state.cx as f32;
if (diff_cx > (1.0 + STICK_HYST_VAL)) || (diff_cx < -STICK_HYST_VAL) {
out_stick_state.cx = (remapped_cx + FLOAT_ORIGIN) as u8;
}
let diff_cy = (remapped_cy + FLOAT_ORIGIN) - current_stick_state.cy as f32;
if (diff_cy > (1.0 + STICK_HYST_VAL)) || (diff_cy < -STICK_HYST_VAL) {
out_stick_state.cy = (remapped_cy + FLOAT_ORIGIN) as u8;
}
out_stick_state
}
fn update_button_states<
@ -335,7 +397,8 @@ pub async fn input_loop(
let stick_state_fut = async {
loop {
// update_stick_states(&mut spi, &mut spi_acs, &mut spi_ccs, 1.0).await;
// current_stick_state = update_stick_states(&mut spi, &mut spi_acs, &mut spi_ccs, 1.0).await;
// STICK_SIGNAL.signal(current_stick_state.clone());
}
};

58
src/stick.rs
View file

@ -1,9 +1,14 @@
// vast majority of this is taken from Phob firmware
use defmt::Format;
use libm::fabs;
use core::f32::consts::PI;
use crate::input::ControllerConfig;
use defmt::Format;
use libm::{atan2f, fabs};
use crate::{
input::{ControllerConfig, Stick},
stick,
};
/// fit order for the linearization
const FIT_ORDER: usize = 3;
@ -18,8 +23,8 @@ pub struct StickParams {
pub fit_coeffs_y: [f32; NUM_COEFFS],
// these are the notch remap parameters
pub affine_coeffs_x: [[f32; 16]; 4], // affine transformation coefficients for all regions of the stick
pub boundary_angles_x: [f32; 4], // angles at the boundaries between regions of the stick (in the plane)
pub affine_coeffs: [[f32; 16]; 4], // affine transformation coefficients for all regions of the stick
pub boundary_angles: [f32; 4], // angles at the boundaries between regions of the stick (in the plane)
}
#[derive(Clone, Debug, Default, Format)]
@ -272,3 +277,46 @@ pub fn linearize_calibration(in_x: &[f64; 17], in_y: &[f64; 17]) -> LinearizeCal
out_y,
}
}
pub fn notch_remap(
x_in: f32,
y_in: f32,
stick_params: &StickParams,
controller_config: &ControllerConfig,
which_stick: Stick,
) -> (f32, f32) {
//determine the angle between the x unit vector and the current position vector
let angle = match atan2f(y_in, x_in) {
//unwrap the angle based on the first region boundary
a if a < stick_params.boundary_angles[0] => a + PI * 2.0,
a => a,
};
//go through the region boundaries from lowest angle to highest, checking if the current position vector is in that region
//if the region is not found then it must be between the first and the last boundary, ie the last region
//we check GATE_REGIONS*2 because each notch has its own very small region we use to make notch values more consistent
let region = 'a: {
for i in 1..NO_OF_NOTCHES {
if angle < stick_params.boundary_angles[i] {
break 'a i - 1;
}
}
NO_OF_NOTCHES - 1
};
let stick_scale = match which_stick {
Stick::ControlStick => controller_config.astick_analog_scaler as f32 / 100.,
Stick::CStick => controller_config.cstick_analog_scaler as f32 / 100.,
};
let x_out = stick_scale
* (stick_params.affine_coeffs[region][0] * x_in
+ stick_params.affine_coeffs[region][1] * y_in);
let y_out = stick_scale
* (stick_params.affine_coeffs[region][2] * x_in
+ stick_params.affine_coeffs[region][3] * y_in);
// TODO: here, add calibration step shenanigans
(x_out, y_out)
}