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

chore(config): separate stick configs / refactor

Browse source
This commit is contained in:
Naxdy 2024-03-27 20:36:41 +01:00
parent 7fd36a91fd
commit b0e1eb6309
Signed by: Naxdy
GPG key ID: CC15075846BCE91B
4 changed files with 82 additions and 112 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

104
src/config.rs
View file

@ -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() {

16
src/filter.rs
View file

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

14
src/input.rs
View file

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

60
src/stick.rs
View file

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