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

feat(input, stick): implement controller config revisioning & norm gains

Browse source
This commit is contained in:
Naxdy 2024-03-27 14:16:42 +01:00
parent 483a51c265
commit fe4afce386
Signed by: Naxdy
GPG key ID: CC15075846BCE91B
3 changed files with 165 additions and 16 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
src/filter.rs
View file

@ -1,10 +1,11 @@
use libm::{fmin, fminf};
use libm::fminf;
use crate::{
input::{ControllerConfig, Stick},
stick::FilterGains,
};
#[derive(Debug, Clone, Default)]
pub struct WaveshapingValues {
pub old_x_pos: f32,
pub old_y_pos: f32,

105
src/input.rs
View file

@ -1,9 +1,9 @@
use core::task::Poll;
use defmt::{debug, Format};
use defmt::{debug, info, warn, Format};
use embassy_futures::{join::join, yield_now};
use embassy_rp::{
flash::{Async, Flash},
flash::{Async, Flash, ERASE_SIZE},
gpio::{Input, Output, Pin},
peripherals::{
FLASH, PIN_10, PIN_11, PIN_16, PIN_17, PIN_18, PIN_19, PIN_20, PIN_21, PIN_22, PIN_23,
@ -15,7 +15,7 @@ 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 packed_struct::{derive::PackedStruct, PackedStruct};
use crate::{
filter::{run_waveshaping, WaveshapingValues},
@ -27,12 +27,16 @@ use crate::{
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;
const STICK_HYST_VAL: f32 = 0.3;
const FLOAT_ORIGIN: f32 = 127.5;
#[derive(Debug, Clone, Default, Format, PackedStruct)]
pub const CONTROLLER_CONFIG_REVISION: u8 = 1;
#[derive(Debug, Clone, Format, PackedStruct)]
#[packed_struct(endian = "msb")]
pub struct ControllerConfig {
#[packed_field(size_bits = "8")]
pub config_revision: u8,
#[packed_field(size_bits = "8")]
pub config_version: u8,
#[packed_field(size_bits = "8")]
@ -47,6 +51,39 @@ pub struct ControllerConfig {
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,
}
impl Default for ControllerConfig {
fn default() -> Self {
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,
}
}
}
#[derive(Clone, Debug, Default)]
@ -57,6 +94,7 @@ struct StickState {
cy: u8,
}
#[derive(Clone, Debug, Default)]
struct StickPositions {
x: f32,
y: f32,
@ -64,6 +102,7 @@ struct StickPositions {
cy: f32,
}
#[derive(Clone, Debug, Default)]
struct RawStickValues {
ax_linearized: f32,
ay_linearized: f32,
@ -388,17 +427,57 @@ pub async fn input_loop(
gcc_state.cstick_x = 127;
gcc_state.cstick_y = 127;
let mut uid = [0u8; 1];
flash.blocking_read(ADDR_OFFSET, &mut uid).unwrap();
let mut controller_config_packed = [0u8; 14]; // ControllerConfig byte size
flash
.blocking_read(ADDR_OFFSET, &mut controller_config_packed)
.unwrap();
debug!("Read from flash: {:02X}", uid);
// TODO: load controller config here
let controller_config = match ControllerConfig::unpack(&controller_config_packed).unwrap() {
a if a.config_revision == CONTROLLER_CONFIG_REVISION => a,
a => {
warn!("Outdated controller config detected ({:02X}), or controller config was never present, using default.", a.config_revision);
let cfg = ControllerConfig::default();
info!("Writing default controller config to flash.");
flash
.blocking_erase(ADDR_OFFSET, ADDR_OFFSET + ERASE_SIZE as u32)
.unwrap();
flash
.blocking_write(ADDR_OFFSET, &cfg.pack().unwrap())
.unwrap();
cfg
}
};
let stick_state_fut = async {
let mut current_stick_state = StickState {
ax: 127,
ay: 127,
cx: 127,
cy: 127,
};
let mut raw_stick_values = RawStickValues::default();
let mut old_stick_pos = StickPositions::default();
let mut cstick_waveshaping_values = WaveshapingValues::default();
let mut controlstick_waveshaping_values = WaveshapingValues::default();
loop {
// current_stick_state = update_stick_states(&mut spi, &mut spi_acs, &mut spi_ccs, 1.0).await;
// STICK_SIGNAL.signal(current_stick_state.clone());
current_stick_state = update_stick_states(
&mut spi,
&mut spi_acs,
&mut spi_ccs,
&current_stick_state,
&controlstick_params,
&cstick_params,
&controller_config,
&filter_gains,
&mut controlstick_waveshaping_values,
&mut cstick_waveshaping_values,
&mut old_stick_pos,
&mut raw_stick_values,
)
.await;
STICK_SIGNAL.signal(current_stick_state.clone());
}
};

73
src/stick.rs
View file

@ -1,9 +1,9 @@
// vast majority of this is taken from Phob firmware
use core::f32::consts::PI;
use core::{f32::consts::PI, iter::Filter};
use defmt::Format;
use libm::{atan2f, fabs};
use libm::{atan2f, fabs, powf};
use crate::{
input::{ControllerConfig, Stick},
@ -16,6 +16,23 @@ const NUM_COEFFS: usize = FIT_ORDER + 1;
const NO_OF_NOTCHES: usize = 16;
const MAX_ORDER: usize = 20;
/// Filter gains for 800Hz, the ones for 1000Hz are provided by `get_norm_gains`
pub const FILTER_GAINS: FilterGains = FilterGains {
max_stick: 100.,
x_vel_decay: 0.1,
y_vel_decay: 0.1,
x_vel_pos_factor: 0.01,
y_vel_pos_factor: 0.01,
x_vel_damp: 0.125,
y_vel_damp: 0.125,
vel_thresh: 1.,
accel_thresh: 3.,
x_smoothing: 0.0,
y_smoothing: 0.0,
c_xsmoothing: 0.0,
c_ysmoothing: 0.0,
};
#[derive(Clone, Debug, Default, Format)]
pub struct StickParams {
// these are the linearization coefficients
@ -320,3 +337,55 @@ pub fn notch_remap(
(x_out, y_out)
}
fn vel_damp_from_snapback(snapback: i8) -> f32 {
match snapback {
a if a >= 0 => 0.125 * powf(2., (snapback - 4) as f32 / 3.0),
_ => 1. - 0.25 * powf(2., (snapback + 4) as f32 / 3.0),
}
}
/// Returns filter gains for 1000Hz polling rate
pub fn get_norm_gains(controller_config: &ControllerConfig) -> FilterGains {
let mut gains = FILTER_GAINS.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_smoothing = controller_config.x_smoothing as f32 / 10.;
gains.y_smoothing = controller_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.;
// The below is assuming the sticks to be polled at 1000Hz
let time_factor = 1.0 / 1.2;
let time_divisor = 1.2 / 1.0;
let vel_thresh = 1.0 / (gains.vel_thresh * time_factor);
let accel_thresh = 1.0 / (gains.accel_thresh * time_factor);
FilterGains {
max_stick: gains.max_stick * gains.max_stick,
x_vel_decay: gains.x_vel_decay * time_factor,
y_vel_decay: gains.y_vel_decay * time_factor,
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 {
a if a >= 0 => time_factor,
_ => 1.0,
},
y_vel_damp: gains.y_vel_damp
* match controller_config.y_snapback {
a if a >= 0 => time_factor,
_ => 1.0,
},
vel_thresh,
accel_thresh,
x_smoothing: powf(1.0 - gains.x_smoothing, time_divisor),
y_smoothing: powf(1.0 - gains.y_smoothing, time_divisor),
c_xsmoothing: powf(1.0 - gains.c_xsmoothing, time_divisor),
c_ysmoothing: powf(1.0 - gains.c_ysmoothing, time_divisor),
}
}