diff --git a/src/config.rs b/src/config.rs index 01a1676..b7c580f 100644 --- a/src/config.rs +++ b/src/config.rs @@ -98,6 +98,48 @@ const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [ PI * 15. / 8., ]; +#[derive(Debug, Clone, Format, PackedStruct)] +#[packed_struct(endian = "msb")] +pub struct StickConfig { + #[packed_field(size_bits = "8")] + pub x_waveshaping: u8, + #[packed_field(size_bits = "8")] + pub y_waveshaping: u8, + #[packed_field(size_bits = "8")] + pub analog_scaler: u8, + #[packed_field(size_bits = "8")] + pub x_snapback: i8, // not used for CStick + #[packed_field(size_bits = "8")] + pub y_snapback: i8, // not used for CStick + #[packed_field(size_bits = "8")] + pub x_smoothing: u8, + #[packed_field(size_bits = "8")] + pub y_smoothing: u8, + #[packed_field(element_size_bytes = "4")] + pub temp_cal_points_x: [PackedFloat; 32], + #[packed_field(element_size_bytes = "4")] + pub temp_cal_points_y: [PackedFloat; 32], + #[packed_field(element_size_bytes = "4")] + pub angles: [PackedFloat; 16], +} + +impl Default for StickConfig { + fn default() -> Self { + Self { + x_waveshaping: 0, + y_waveshaping: 0, + x_snapback: 0, + y_snapback: 0, + x_smoothing: 0, + y_smoothing: 0, + analog_scaler: 0, + 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(), + } + } +} + #[derive(Debug, Clone, Format, PackedStruct)] #[packed_struct(endian = "msb")] pub struct ControllerConfig { @@ -105,42 +147,10 @@ pub struct ControllerConfig { pub config_revision: u8, #[packed_field(size_bits = "8")] pub config_version: u8, - #[packed_field(size_bits = "8")] - pub ax_waveshaping: u8, - #[packed_field(size_bits = "8")] - pub ay_waveshaping: u8, - #[packed_field(size_bits = "8")] - 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, - #[packed_field(size_bits = "8")] - pub x_snapback: i8, - #[packed_field(size_bits = "8")] - pub y_snapback: i8, - #[packed_field(size_bits = "8")] - pub x_smoothing: u8, - #[packed_field(size_bits = "8")] - pub y_smoothing: u8, - #[packed_field(size_bits = "8")] - pub c_xsmoothing: u8, - #[packed_field(size_bits = "8")] - pub c_ysmoothing: u8, - #[packed_field(element_size_bytes = "4")] - pub temp_cal_points_ax: [PackedFloat; 32], - #[packed_field(element_size_bytes = "4")] - pub temp_cal_points_ay: [PackedFloat; 32], - #[packed_field(element_size_bytes = "4")] - pub temp_cal_points_cx: [PackedFloat; 32], - #[packed_field(element_size_bytes = "4")] - pub temp_cal_points_cy: [PackedFloat; 32], - #[packed_field(element_size_bytes = "4")] - pub a_angles: [PackedFloat; 16], - #[packed_field(element_size_bytes = "4")] - pub c_angles: [PackedFloat; 16], + #[packed_field(size_bytes = "327")] + pub astick_config: StickConfig, + #[packed_field(size_bytes = "327")] + pub cstick_config: StickConfig, } impl Default for ControllerConfig { @@ -148,24 +158,8 @@ impl Default for ControllerConfig { Self { config_revision: CONTROLLER_CONFIG_REVISION, config_version: 0, - ax_waveshaping: 0, - ay_waveshaping: 0, - cx_waveshaping: 0, - cy_waveshaping: 0, - astick_analog_scaler: 0, - cstick_analog_scaler: 0, - x_snapback: 0, - y_snapback: 0, - x_smoothing: 0, - y_smoothing: 0, - c_xsmoothing: 0, - c_ysmoothing: 0, - temp_cal_points_ax: *DEFAULT_CAL_POINTS_X.to_packed_float_array(), - temp_cal_points_ay: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(), - temp_cal_points_cx: *DEFAULT_CAL_POINTS_X.to_packed_float_array(), - temp_cal_points_cy: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(), - a_angles: *DEFAULT_ANGLES.to_packed_float_array(), - c_angles: *DEFAULT_ANGLES.to_packed_float_array(), + astick_config: StickConfig::default(), + cstick_config: StickConfig::default(), } } } @@ -174,7 +168,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; 654]; // ControllerConfig byte size + let mut controller_config_packed: ::ByteArray = [0u8; 656]; // ControllerConfig byte size flash.blocking_read(ADDR_OFFSET, &mut controller_config_packed)?; match ControllerConfig::unpack(&controller_config_packed).unwrap() { diff --git a/src/filter.rs b/src/filter.rs index 20ee7b8..90c880c 100644 --- a/src/filter.rs +++ b/src/filter.rs @@ -88,14 +88,14 @@ impl FilterGains { pub fn get_normalized_gains(&self, controller_config: &ControllerConfig) -> Self { let mut gains = self.clone(); - gains.x_vel_damp = vel_damp_from_snapback(controller_config.x_snapback); - gains.y_vel_damp = vel_damp_from_snapback(controller_config.y_snapback); + gains.x_vel_damp = vel_damp_from_snapback(controller_config.astick_config.x_snapback); + gains.y_vel_damp = vel_damp_from_snapback(controller_config.astick_config.y_snapback); - gains.x_smoothing = controller_config.x_smoothing as f32 / 10.; - gains.y_smoothing = controller_config.y_smoothing as f32 / 10.; + gains.x_smoothing = controller_config.astick_config.x_smoothing as f32 / 10.; + gains.y_smoothing = controller_config.astick_config.y_smoothing as f32 / 10.; - gains.c_xsmoothing = controller_config.c_xsmoothing as f32 / 10.; - gains.c_ysmoothing = controller_config.c_ysmoothing as f32 / 10.; + gains.c_xsmoothing = controller_config.cstick_config.x_smoothing as f32 / 10.; + gains.c_ysmoothing = controller_config.cstick_config.y_smoothing as f32 / 10.; // The below is assuming the sticks to be polled at 1000Hz let time_factor = 1.0 / 1.2; @@ -111,12 +111,12 @@ impl FilterGains { x_vel_pos_factor: gains.x_vel_pos_factor * time_factor, y_vel_pos_factor: gains.y_vel_pos_factor * time_factor, x_vel_damp: gains.x_vel_damp - * match controller_config.x_snapback { + * match controller_config.astick_config.x_snapback { a if a >= 0 => time_factor, _ => 1.0, }, y_vel_damp: gains.y_vel_damp - * match controller_config.y_snapback { + * match controller_config.astick_config.y_snapback { a if a >= 0 => time_factor, _ => 1.0, }, diff --git a/src/input.rs b/src/input.rs index fd02b8d..0b46852 100644 --- a/src/input.rs +++ b/src/input.rs @@ -1,5 +1,3 @@ -use core::task::Poll; - use defmt::info; use embassy_futures::{join::join, yield_now}; use embassy_rp::{ @@ -210,8 +208,8 @@ async fn update_stick_states< let (shaped_x, shaped_y) = run_waveshaping( x_pos_filt, y_pos_filt, - controller_config.ax_waveshaping, - controller_config.ay_waveshaping, + controller_config.astick_config.x_waveshaping, + controller_config.astick_config.y_waveshaping, controlstick_waveshaping_values, &filter_gains, ); @@ -226,8 +224,8 @@ async fn update_stick_states< let (shaped_cx, shaped_cy) = run_waveshaping( pos_cx, pos_cy, - controller_config.cx_waveshaping, - controller_config.cy_waveshaping, + controller_config.cstick_config.x_waveshaping, + controller_config.cstick_config.y_waveshaping, cstick_waveshaping_values, &filter_gains, ); @@ -387,8 +385,8 @@ pub async fn input_loop( let controller_config = ControllerConfig::from_flash_memory(&mut flash).unwrap(); - let (controlstick_params, cstick_params) = - StickParams::from_controller_config(&controller_config); + let controlstick_params = StickParams::from_stick_config(&controller_config.astick_config); + let cstick_params = StickParams::from_stick_config(&controller_config.cstick_config); let filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config); diff --git a/src/stick.rs b/src/stick.rs index 6ff207d..9eea967 100644 --- a/src/stick.rs +++ b/src/stick.rs @@ -6,7 +6,7 @@ use defmt::{debug, Format}; use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf}; use crate::{ - config::{ControllerConfig, DEFAULT_NOTCH_STATUS}, + config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS}, input::Stick, packed_float::ToRegularArray, }; @@ -44,48 +44,26 @@ pub struct StickParams { impl StickParams { /// Generate StickParams structs for the sticks, returned as a tuple of (analog_stick, c_stick) - pub fn from_controller_config(controller_config: &ControllerConfig) -> (Self, Self) { - let cleaned_cal_points_astick = CleanedCalibrationPoints::from_temp_calibration_points( - &controller_config.temp_cal_points_ax.to_regular_array(), - &controller_config.temp_cal_points_ay.to_regular_array(), - &controller_config.a_angles.to_regular_array(), + pub fn from_stick_config(stick_config: &StickConfig) -> Self { + let cleaned_cal_points = CleanedCalibrationPoints::from_temp_calibration_points( + &stick_config.temp_cal_points_x.to_regular_array(), + &stick_config.temp_cal_points_y.to_regular_array(), + &stick_config.angles.to_regular_array(), ); - let cleaned_cal_points_cstick = CleanedCalibrationPoints::from_temp_calibration_points( - &controller_config.temp_cal_points_cx.to_regular_array(), - &controller_config.temp_cal_points_cy.to_regular_array(), - &controller_config.c_angles.to_regular_array(), + let linearized_cal = LinearizedCalibration::from_calibration_points(&cleaned_cal_points); + + let notch_cal = NotchCalibration::from_cleaned_and_linearized_calibration( + &cleaned_cal_points, + &linearized_cal, ); - let linearized_cal_astick = - LinearizedCalibration::from_calibration_points(&cleaned_cal_points_astick); - let linearized_cal_cstick = - LinearizedCalibration::from_calibration_points(&cleaned_cal_points_cstick); - - let notch_cal_astick = NotchCalibration::from_cleaned_and_linearized_calibration( - &cleaned_cal_points_astick, - &linearized_cal_astick, - ); - let notch_cal_cstick = NotchCalibration::from_cleaned_and_linearized_calibration( - &cleaned_cal_points_cstick, - &linearized_cal_cstick, - ); - - let stick_params_astick = Self { - fit_coeffs_x: linearized_cal_astick.fit_coeffs_x.map(|e| e as f32), - fit_coeffs_y: linearized_cal_astick.fit_coeffs_y.map(|e| e as f32), - affine_coeffs: notch_cal_astick.affine_coeffs, - boundary_angles: notch_cal_astick.boundary_angles, - }; - - let stick_params_cstick = Self { - fit_coeffs_x: linearized_cal_cstick.fit_coeffs_x.map(|e| e as f32), - fit_coeffs_y: linearized_cal_cstick.fit_coeffs_y.map(|e| e as f32), - affine_coeffs: notch_cal_cstick.affine_coeffs, - boundary_angles: notch_cal_cstick.boundary_angles, - }; - - (stick_params_astick, stick_params_cstick) + 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), + affine_coeffs: notch_cal.affine_coeffs, + boundary_angles: notch_cal.boundary_angles, + } } } @@ -619,8 +597,8 @@ pub fn notch_remap( }; 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., + Stick::ControlStick => controller_config.astick_config.analog_scaler as f32 / 100., + Stick::CStick => controller_config.cstick_config.analog_scaler as f32 / 100., }; let x_out = stick_scale