From b0e1eb6309d1575ac4be1aecc5c4ee34f474a091 Mon Sep 17 00:00:00 2001
From: Naxdy <naxdy@polskalemon.com>
Date: Wed, 27 Mar 2024 20:36:41 +0100
Subject: [PATCH] chore(config): separate stick configs / refactor
---
src/config.rs | 104 ++++++++++++++++++++++++--------------------------
src/filter.rs | 16 ++++----
src/input.rs | 14 +++----
src/stick.rs | 60 +++++++++--------------------
4 files changed, 82 insertions(+), 112 deletions(-)
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<Self, embassy_rp::flash::Error> {
- let mut controller_config_packed: <ControllerConfig as packed_struct::PackedStruct>::ByteArray = [0u8; 654]; // ControllerConfig byte size
+ let mut controller_config_packed: <ControllerConfig as packed_struct::PackedStruct>::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