From 1861b89505044c3da61cb22f08e3e878a2804475 Mon Sep 17 00:00:00 2001 From: Naxdy Date: Wed, 27 Mar 2024 22:12:28 +0100 Subject: [PATCH] feat: finish notch remap --- src/config.rs | 11 +++-- src/input.rs | 133 ++++++++++++++++++++++++++++++++------------------ src/stick.rs | 35 +++++++++++-- 3 files changed, 124 insertions(+), 55 deletions(-) diff --git a/src/config.rs b/src/config.rs index 7943b75..3a42c36 100644 --- a/src/config.rs +++ b/src/config.rs @@ -112,6 +112,8 @@ pub struct StickConfig { #[packed_field(size_bits = "8")] pub y_snapback: i8, // not used for CStick #[packed_field(size_bits = "8")] + pub cardinal_snapping: i8, // not used for CStick + #[packed_field(size_bits = "8")] pub x_smoothing: u8, #[packed_field(size_bits = "8")] pub y_smoothing: u8, @@ -132,7 +134,8 @@ impl Default for StickConfig { y_snapback: 0, x_smoothing: 0, y_smoothing: 0, - analog_scaler: 0, + cardinal_snapping: 0, + analog_scaler: 100, temp_cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(), temp_cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(), angles: *DEFAULT_ANGLES.to_packed_float_array(), @@ -147,9 +150,9 @@ pub struct ControllerConfig { pub config_revision: u8, #[packed_field(size_bits = "8")] pub config_version: u8, - #[packed_field(size_bytes = "327")] + #[packed_field(size_bytes = "328")] pub astick_config: StickConfig, - #[packed_field(size_bytes = "327")] + #[packed_field(size_bytes = "328")] pub cstick_config: StickConfig, } @@ -168,7 +171,7 @@ impl ControllerConfig { pub fn from_flash_memory( mut flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>, ) -> Result { - let mut controller_config_packed: ::ByteArray = [0u8; 656]; // ControllerConfig byte size + let mut controller_config_packed: ::ByteArray = [0u8; 658]; // ControllerConfig byte size flash.blocking_read(ADDR_OFFSET, &mut controller_config_packed)?; match ControllerConfig::unpack(&controller_config_packed).unwrap() { diff --git a/src/input.rs b/src/input.rs index 1a3393a..ef3c311 100644 --- a/src/input.rs +++ b/src/input.rs @@ -1,4 +1,4 @@ -use defmt::{debug, info}; +use defmt::{debug, info, trace, Format}; use embassy_futures::{join::join, yield_now}; use embassy_rp::{ flash::{Async, Flash}, @@ -45,7 +45,7 @@ struct StickPositions { cy: f32, } -#[derive(Clone, Debug, Default)] +#[derive(Clone, Debug, Default, Format)] struct RawStickValues { a_linearized: XyValuePair, c_linearized: XyValuePair, @@ -182,8 +182,7 @@ async fn update_stick_states< y: (cy_sum as f32) / (adc_count as f32) / 4096.0f32, }; - debug!("Raw Control Stick: {:?}", raw_controlstick); - debug!("Raw C Stick: {:?}", raw_cstick); + trace!("Raw Control Stick: {}", raw_controlstick); raw_stick_values.a_raw = raw_controlstick; raw_stick_values.c_raw = raw_cstick; @@ -199,107 +198,145 @@ async fn update_stick_states< raw_stick_values.c_linearized.x = pos_cx; raw_stick_values.c_linearized.y = pos_cy; + trace!("Raw Stick Values 001: {:?}", raw_stick_values); + let (x_pos_filt, y_pos_filt) = kalman_state.run_kalman(x_z, y_z, &controller_config.astick_config, &filter_gains); - let (shaped_x, shaped_y) = run_waveshaping( + let shaped_astick = match run_waveshaping( x_pos_filt, y_pos_filt, controller_config.astick_config.x_waveshaping, controller_config.astick_config.y_waveshaping, controlstick_waveshaping_values, &filter_gains, - ); + ) { + (x, y) => XyValuePair { x, y }, + }; - let pos_x: f32 = - filter_gains.smoothing.x * shaped_x + (1.0 - filter_gains.smoothing.x) * old_stick_pos.x; - let pos_y = - filter_gains.smoothing.y * shaped_y + (1.0 - filter_gains.smoothing.y) * old_stick_pos.y; + trace!("Shaped Controlstick: {}", shaped_astick); + + let pos_x: f32 = filter_gains.smoothing.x * shaped_astick.x + + (1.0 - filter_gains.smoothing.x) * old_stick_pos.x; + let pos_y = filter_gains.smoothing.y * shaped_astick.y + + (1.0 - filter_gains.smoothing.y) * old_stick_pos.y; old_stick_pos.x = pos_x; old_stick_pos.y = pos_y; - let (shaped_cx, shaped_cy) = run_waveshaping( + let shaped_cstick = match run_waveshaping( pos_cx, pos_cy, controller_config.cstick_config.x_waveshaping, controller_config.cstick_config.y_waveshaping, cstick_waveshaping_values, &filter_gains, - ); + ) { + (x, y) => XyValuePair { x, y }, + }; - let old_cx_pos = old_stick_pos.cx; - let old_cy_pos = old_stick_pos.cy; - old_stick_pos.cx = shaped_cx; - old_stick_pos.cy = shaped_cy; + let old_c_pos = XyValuePair { + x: old_stick_pos.cx, + y: old_stick_pos.cy, + }; + old_stick_pos.cx = shaped_cstick.x; + old_stick_pos.cy = shaped_cstick.y; let x_weight_1 = filter_gains.c_smoothing.x; let x_weight_2 = 1.0 - x_weight_1; let y_weight_1 = filter_gains.c_smoothing.y; let y_weight_2 = 1.0 - y_weight_1; - let pos_cx_filt = x_weight_1 * shaped_cx + x_weight_2 * old_cx_pos; - let pos_cy_filt = y_weight_1 * shaped_cy + y_weight_2 * old_cy_pos; + let pos_cx_filt = x_weight_1 * shaped_cstick.x + x_weight_2 * old_c_pos.x; + let pos_cy_filt = y_weight_1 * shaped_cstick.y + y_weight_2 * old_c_pos.y; // phob optionally runs a median filter here, but we leave it for now - let (mut remapped_x, mut remapped_y) = notch_remap( + trace!("Controlstick position: {}, {}", pos_x, pos_y); + + let mut remapped = match notch_remap( pos_x, pos_y, controlstick_params, controller_config, Stick::ControlStick, - ); - let (mut remapped_cx, mut remapped_cy) = notch_remap( + false, + ) { + (x, y) => XyValuePair { x, y }, + }; + let mut remapped_c = match notch_remap( pos_cx_filt, pos_cy_filt, cstick_params, controller_config, Stick::CStick, - ); - let (remapped_x_unfiltered, remapped_y_unfiltered) = notch_remap( + false, + ) { + (x, y) => XyValuePair { x, y }, + }; + let remapped_unfiltered = match notch_remap( 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( + false, + ) { + (x, y) => XyValuePair { x, y }, + }; + let remapped_c_unfiltered = match notch_remap( raw_stick_values.c_linearized.x, raw_stick_values.c_linearized.y, cstick_params, controller_config, Stick::CStick, - ); + false, + ) { + (x, y) => XyValuePair { x, y }, + }; - 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.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)); + trace!("Remapped Control Stick: {}", remapped); + + remapped = XyValuePair { + x: fminf(125., fmaxf(-125., remapped.x)), + y: fminf(125., fmaxf(-125., remapped.y)), + }; + remapped_c = XyValuePair { + x: fminf(125., fmaxf(-125., remapped_c.x)), + y: fminf(125., fmaxf(-125., remapped_c.y)), + }; + raw_stick_values.a_unfiltered.x = fminf(125., fmaxf(-125., remapped_unfiltered.x)); + raw_stick_values.a_unfiltered.y = fminf(125., fmaxf(-125., remapped_unfiltered.y)); + raw_stick_values.c_unfiltered.x = fminf(125., fmaxf(-125., remapped_c_unfiltered.x)); + raw_stick_values.c_unfiltered.y = fminf(125., fmaxf(-125., remapped_c_unfiltered.y)); let mut out_stick_state = current_stick_state.clone(); - let diff_x = (remapped_x + FLOAT_ORIGIN) - current_stick_state.ax as f32; + 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; + out_stick_state.ax = (remapped.x + FLOAT_ORIGIN) as u8; } - let diff_y = (remapped_y + FLOAT_ORIGIN) - current_stick_state.ay as f32; + 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; + out_stick_state.ay = (remapped.y + FLOAT_ORIGIN) as u8; } - let diff_cx = (remapped_cx + FLOAT_ORIGIN) - current_stick_state.cx as f32; + let diff_cx = (remapped_c.x + 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; + out_stick_state.cx = (remapped_c.x + FLOAT_ORIGIN) as u8; } - let diff_cy = (remapped_cy + FLOAT_ORIGIN) - current_stick_state.cy as f32; + let diff_cy = (remapped_c.y + 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.cy = (remapped_c.y + FLOAT_ORIGIN) as u8; } + trace!( + "Control stick: {}, {}, C-stick: {}, {}", + out_stick_state.ax, + out_stick_state.ay, + out_stick_state.cx, + out_stick_state.cy + ); + out_stick_state } @@ -403,7 +440,7 @@ pub async fn input_loop( let mut last_loop_time = Instant::now(); loop { - let timer = Timer::after_millis(1); + let timer = Timer::after_micros(1000); current_stick_state = update_stick_states( &mut spi, @@ -424,10 +461,12 @@ pub async fn input_loop( timer.await; - debug!( - "Loop took {} us", - (Instant::now() - last_loop_time).as_micros() - ); + match (Instant::now() - last_loop_time).as_micros() { + a if a > 1100 => { + debug!("Loop took {} us", a); + } + _ => {} + }; last_loop_time = Instant::now(); diff --git a/src/stick.rs b/src/stick.rs index 42554a4..de56e1d 100644 --- a/src/stick.rs +++ b/src/stick.rs @@ -3,7 +3,7 @@ use core::f32::consts::PI; use defmt::{debug, Format}; -use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf}; +use libm::{atan2f, cosf, fabs, fabsf, roundf, sinf, sqrtf}; use crate::{ config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS}, @@ -583,6 +583,7 @@ pub fn notch_remap( stick_params: &StickParams, controller_config: &ControllerConfig, which_stick: Stick, + is_calibrating: bool, ) -> (f32, f32) { //determine the angle between the x unit vector and the current position vector let angle = match atan2f(y_in, x_in) { @@ -608,14 +609,40 @@ pub fn notch_remap( Stick::CStick => controller_config.cstick_config.analog_scaler as f32 / 100., }; - let x_out = stick_scale + let mut 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 + let mut 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 + if !is_calibrating { + let stick_config = match which_stick { + Stick::ControlStick => &controller_config.astick_config, + Stick::CStick => &controller_config.cstick_config, + }; + + if stick_config.cardinal_snapping > 0 { + if fabsf(x_out) < stick_config.cardinal_snapping as f32 + 0.5 && fabsf(y_out) >= 79.5 { + x_out = 0.; + } + if fabsf(y_out) < stick_config.cardinal_snapping as f32 + 0.5 && fabsf(x_out) >= 79.5 { + y_out = 0.; + } + } else if stick_config.cardinal_snapping == -1 { + if fabsf(x_out) < 6.5 && fabsf(y_out) >= 79.5 { + x_out = 0.; + } + if fabsf(y_out) < 6.5 && fabsf(x_out) >= 79.5 { + y_out = 0.; + } + } + + if fabsf(x_out) < 3. && fabsf(y_out) < 3. { + x_out = 0.; + y_out = 0.; + } + } (x_out, y_out) }