forked from NaxdyOrg/NaxGCC-FW
feat(input, stick): implement controller config revisioning & norm gains
This commit is contained in:
parent
483a51c265
commit
fe4afce386
GPG key ID:
CC15075846BCE91B
3 changed files with 165 additions and 16 deletions
|
|
@ -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
105
src/input.rs
|
|
@ -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,
|
||||
¤t_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
73
src/stick.rs
|
|
@ -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),
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue