quiK/NaxGCC-FW
1
0
Fork 0
forked from NaxdyOrg/NaxGCC-FW
Code Pull requests Activity

feat(config, stick): implement more calibration logic

Browse source
This commit is contained in:
Naxdy 2024-03-29 19:54:52 +01:00
parent 5408d37560
commit 2d7239c04a
Signed by: Naxdy
GPG key ID: CC15075846BCE91B
6 changed files with 489 additions and 121 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
Cargo.toml
View file

@ -62,6 +62,7 @@ debug = 2
debug-assertions = true
incremental = false
opt-level = 3
lto = "fat"
overflow-checks = true
# cargo build/run --release

253
src/config.rs
View file

@ -4,7 +4,7 @@
*/
use core::f32::consts::PI;
use defmt::{info, warn, Format};
use defmt::{error, info, warn, Format};
use embassy_rp::{
flash::{Async, Flash, ERASE_SIZE},
peripherals::FLASH,
@ -12,16 +12,38 @@ use embassy_rp::{
use packed_struct::{derive::PackedStruct, PackedStruct};
use crate::{
helpers::{PackedFloat, ToPackedFloatArray},
stick::{NotchStatus, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES},
helpers::{PackedFloat, ToPackedFloatArray, XyValuePair},
input::{read_ext_adc, Stick, StickAxis, SPI_ACS_SHARED, SPI_CCS_SHARED, SPI_SHARED},
stick::{NotchStatus, NO_OF_ADJ_NOTCHES, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES},
ADDR_OFFSET, FLASH_SIZE,
};
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pubsub::Subscriber};
use embassy_sync::{
blocking_mutex::raw::{CriticalSectionRawMutex, RawMutex},
pubsub::Subscriber,
signal::Signal,
};
use embassy_time::Timer;
use crate::{gcc_hid::GcReport, input::CHANNEL_GCC_STATE};
/// Whether we are currently calibrating the sticks. Updates are dispatched when the status changes.
/// Initial status is assumed to be false.
pub static SIGNAL_IS_CALIBRATING: Signal<CriticalSectionRawMutex, bool> = Signal::new();
/// Dispatched when we want to override the GCC state for a short amount of time.
pub static SIGNAL_OVERRIDE_GCC_STATE: Signal<CriticalSectionRawMutex, OverrideGcReportInstruction> =
Signal::new();
/// Struct used for overriding the GCC state for a given amount of
/// time, useful for providing feedback to the user e.g. if we just entered
/// a certain mode.
#[derive(Default, Debug, Clone, Format)]
pub struct OverrideGcReportInstruction {
pub report: GcReport,
pub duration_ms: u64,
}
const CONFIG_MODE_ENTRY_COMBO: [AwaitableButtons; 4] = [
AwaitableButtons::Start,
AwaitableButtons::A,
@ -29,6 +51,20 @@ const CONFIG_MODE_ENTRY_COMBO: [AwaitableButtons; 4] = [
AwaitableButtons::Y,
];
const LSTICK_CALIBRATION_COMBO: [AwaitableButtons; 4] = [
AwaitableButtons::A,
AwaitableButtons::X,
AwaitableButtons::Y,
AwaitableButtons::L,
];
const RSTICK_CALIBRATION_COMBO: [AwaitableButtons; 4] = [
AwaitableButtons::A,
AwaitableButtons::X,
AwaitableButtons::Y,
AwaitableButtons::R,
];
/// This doesn't need to be super fast, since it's only used
/// in config mode.
const BUTTON_POLL_INTERVAL_MILLIS: u64 = 20;
@ -96,7 +132,7 @@ const DEFAULT_CAL_POINTS_Y: [f32; NO_OF_CALIBRATION_POINTS] = [
0.3000802760,0.3008482317
];
const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [
pub const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [
0.,
PI / 8.0,
PI * 2. / 8.,
@ -150,9 +186,9 @@ pub struct StickConfig {
#[packed_field(size_bits = "8")]
pub y_smoothing: u8,
#[packed_field(element_size_bytes = "4")]
pub temp_cal_points_x: [PackedFloat; 32],
pub cal_points_x: [PackedFloat; 32],
#[packed_field(element_size_bytes = "4")]
pub temp_cal_points_y: [PackedFloat; 32],
pub cal_points_y: [PackedFloat; 32],
#[packed_field(element_size_bytes = "4")]
pub angles: [PackedFloat; 16],
}
@ -168,8 +204,8 @@ impl Default for StickConfig {
y_smoothing: 0,
cardinal_snapping: 0,
analog_scaler: 100,
temp_cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
temp_cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
angles: *DEFAULT_ANGLES.to_packed_float_array(),
}
}
@ -255,16 +291,17 @@ trait WaitForButtonPress {
) -> usize;
}
impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress
for Subscriber<'a, CriticalSectionRawMutex, GcReport, I, J, K>
impl<'a, T: RawMutex, const I: usize, const J: usize, const K: usize> WaitForButtonPress
for Subscriber<'a, T, GcReport, I, J, K>
{
async fn wait_for_button_press(&mut self, button_to_wait_for: &AwaitableButtons) {
loop {
if match self.next_message_pure().await {
report => is_awaitable_button_pressed(&report, button_to_wait_for),
} {
let report = self.next_message_pure().await;
if is_awaitable_button_pressed(&report, button_to_wait_for) {
break;
}
Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
}
}
@ -274,13 +311,15 @@ impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress
buttons_to_wait_for: &[AwaitableButtons; N],
) {
loop {
if match self.next_message_pure().await {
report => buttons_to_wait_for
.iter()
.all(|button| is_awaitable_button_pressed(&report, button)),
} {
let report = self.next_message_pure().await;
if buttons_to_wait_for
.iter()
.all(|button| is_awaitable_button_pressed(&report, button))
{
break;
}
Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
}
}
@ -290,15 +329,14 @@ impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress
buttons_to_wait_for: &[AwaitableButtons; N],
) -> AwaitableButtons {
loop {
match self.next_message_pure().await {
report => {
for button in buttons_to_wait_for {
if is_awaitable_button_pressed(&report, button) {
return *button;
}
}
let report = self.next_message_pure().await;
for button in buttons_to_wait_for {
if is_awaitable_button_pressed(&report, button) {
return *button;
}
}
Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
}
}
@ -308,18 +346,17 @@ impl<'a, const I: usize, const J: usize, const K: usize> WaitForButtonPress
buttons_to_wait_for: &[[AwaitableButtons; N]; M],
) -> usize {
loop {
match self.next_message_pure().await {
report => {
for (i, buttons) in buttons_to_wait_for.iter().enumerate() {
if buttons
.iter()
.all(|button| is_awaitable_button_pressed(&report, button))
{
return i;
}
}
let report = self.next_message_pure().await;
for (i, buttons) in buttons_to_wait_for.iter().enumerate() {
if buttons
.iter()
.all(|button| is_awaitable_button_pressed(&report, button))
{
return i;
}
}
Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
}
}
@ -336,18 +373,156 @@ fn is_awaitable_button_pressed(report: &GcReport, button_to_wait_for: &Awaitable
AwaitableButtons::Left => report.buttons_1.dpad_left,
AwaitableButtons::Right => report.buttons_1.dpad_right,
AwaitableButtons::Start => report.buttons_2.button_start,
AwaitableButtons::L => report.buttons_2.button_l,
AwaitableButtons::R => report.buttons_2.button_r,
AwaitableButtons::L => report.buttons_2.button_l || report.trigger_l > 10,
AwaitableButtons::R => report.buttons_2.button_r || report.trigger_r > 10,
}
}
#[derive(Debug, Format)]
struct StickCalibrationProcess<'a> {
which_stick: Stick,
calibration_step: u8,
gcc_config: &'a mut ControllerConfig,
cal_points: [XyValuePair<f32>; NO_OF_CALIBRATION_POINTS],
}
impl<'a> StickCalibrationProcess<'a> {
pub fn new(gcc_config: &'a mut ControllerConfig, which_stick: Stick) -> Self {
Self {
which_stick,
calibration_step: 0,
gcc_config,
cal_points: [XyValuePair::default(); NO_OF_CALIBRATION_POINTS],
}
}
async fn calibration_advance(&mut self) {
let mut spi_unlocked = SPI_SHARED.lock().await;
let mut spi_acs_unlocked = SPI_ACS_SHARED.lock().await;
let mut spi_ccs_unlocked = SPI_CCS_SHARED.lock().await;
let spi = spi_unlocked.as_mut().unwrap();
let spi_acs = spi_acs_unlocked.as_mut().unwrap();
let spi_ccs = spi_ccs_unlocked.as_mut().unwrap();
if self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 {
let mut x: f32 = 0.;
let mut y: f32 = 0.;
for _ in 0..128 {
x += read_ext_adc(self.which_stick, StickAxis::XAxis, spi, spi_acs, spi_ccs) as f32
/ 4096.0;
y += read_ext_adc(self.which_stick, StickAxis::YAxis, spi, spi_acs, spi_ccs) as f32
/ 4096.0;
}
x /= 128.;
y /= 128.;
self.cal_points[self.calibration_step as usize] = XyValuePair { x, y };
}
self.calibration_step += 1;
// TODO: phob does something related to undo here
if self.calibration_step == NO_OF_CALIBRATION_POINTS as u8 {
// TODO
}
if self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8 {
let stick_config = match self.which_stick {
Stick::ControlStick => &mut self.gcc_config.astick_config,
Stick::CStick => &mut self.gcc_config.cstick_config,
};
stick_config.cal_points_x = self.cal_points.map(|p| p.x.into());
stick_config.cal_points_y = self.cal_points.map(|p| p.y.into());
}
}
pub async fn calibrate_stick(&mut self) {
todo!()
}
}
async fn configuration_main_loop<
'a,
M: RawMutex,
const C: usize,
const S: usize,
const P: usize,
>(
current_config: &ControllerConfig,
mut flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
gcc_subscriber: &mut Subscriber<'a, M, GcReport, C, S, P>,
) {
let mut final_config = current_config.clone();
let config_options = [LSTICK_CALIBRATION_COMBO, RSTICK_CALIBRATION_COMBO];
'main: loop {
match gcc_subscriber
.wait_and_filter_simultaneous_button_presses(&config_options)
.await
{
selection => match selection {
1 => {
StickCalibrationProcess::new(&mut final_config, Stick::ControlStick)
.calibrate_stick()
.await
}
2 => {
StickCalibrationProcess::new(&mut final_config, Stick::CStick)
.calibrate_stick()
.await
}
_ => {
error!("Invalid selection: {}", selection);
break 'main;
}
},
};
final_config.write_to_flash(&mut flash).unwrap();
}
}
#[embassy_executor::task]
pub async fn config_task() {
pub async fn config_task(
current_config: ControllerConfig,
mut flash: Flash<'static, FLASH, Async, FLASH_SIZE>,
) {
let mut gcc_subscriber = CHANNEL_GCC_STATE.subscriber().unwrap();
loop {
gcc_subscriber
.wait_for_simultaneous_button_presses(&CONFIG_MODE_ENTRY_COMBO)
.await;
info!("Entering config mode.");
SIGNAL_OVERRIDE_GCC_STATE.signal(OverrideGcReportInstruction {
report: match GcReport::default() {
mut a => {
a.trigger_r = 255;
a.trigger_l = 255;
a.buttons_2.button_l = true;
a.buttons_2.button_r = true;
a.buttons_1.button_x = true;
a.buttons_1.button_y = true;
a.buttons_1.button_a = true;
a.stick_x = 127;
a.stick_y = 127;
a.cstick_x = 127;
a.cstick_y = 127;
a
}
},
duration_ms: 1000,
});
configuration_main_loop(&current_config, &mut flash, &mut gcc_subscriber).await;
info!("Exiting config mode.");
}
}

6
src/helpers.rs
View file

@ -47,6 +47,12 @@ impl Deref for PackedFloat {
}
}
impl From<f32> for PackedFloat {
fn from(f: f32) -> Self {
Self(f)
}
}
#[derive(Debug, Clone, Format, Default, Copy)]
pub struct XyValuePair<T> {
pub x: T,

128
src/input.rs
View file

@ -5,19 +5,22 @@ use embassy_rp::{
gpio::{AnyPin, 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,
PIN_24, PIN_5, PIN_8, PIN_9, PWM_CH4, PWM_CH6, SPI0,
PIN_24, PIN_5, PIN_8, PIN_9, PWM_CH4, PWM_CH6, SPI0, SPI1,
},
pwm::Pwm,
spi::Spi,
spi::{Blocking, Spi},
};
use embassy_sync::{
blocking_mutex::raw::CriticalSectionRawMutex, pubsub::PubSubChannel, signal::Signal,
blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex, ThreadModeRawMutex},
mutex::Mutex,
pubsub::PubSubChannel,
signal::Signal,
};
use embassy_time::{Duration, Instant, Timer};
use libm::{fmaxf, fminf};
use crate::{
config::ControllerConfig,
config::{ControllerConfig, SIGNAL_IS_CALIBRATING, SIGNAL_OVERRIDE_GCC_STATE},
filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS},
gcc_hid::GcReport,
helpers::XyValuePair,
@ -30,7 +33,17 @@ pub static CHANNEL_GCC_STATE: PubSubChannel<CriticalSectionRawMutex, GcReport, 1
PubSubChannel::new();
/// Used to send the stick state from the stick task to the main input task
static STICK_SIGNAL: Signal<CriticalSectionRawMutex, StickState> = Signal::new();
static SIGNAL_STICK_STATE: Signal<CriticalSectionRawMutex, StickState> = Signal::new();
/// Used to send the raw stick values for the calibration task
static SIGNAL_RAW_STICK_VALUES: Signal<CriticalSectionRawMutex, RawStickValues> = Signal::new();
pub static SPI_SHARED: Mutex<ThreadModeRawMutex, Option<Spi<'static, SPI0, Blocking>>> =
Mutex::new(None);
pub static SPI_ACS_SHARED: Mutex<ThreadModeRawMutex, Option<Output<'static, AnyPin>>> =
Mutex::new(None);
pub static SPI_CCS_SHARED: Mutex<ThreadModeRawMutex, Option<Output<'static, AnyPin>>> =
Mutex::new(None);
const STICK_HYST_VAL: f32 = 0.3;
const FLOAT_ORIGIN: f32 = 127.5;
@ -61,7 +74,7 @@ struct RawStickValues {
c_unfiltered: XyValuePair<f32>,
}
#[derive(PartialEq, Eq)]
#[derive(PartialEq, Eq, Debug, Clone, Format, Copy)]
pub enum Stick {
ControlStick,
CStick,
@ -74,7 +87,13 @@ pub enum StickAxis {
}
#[link_section = ".time_critical.read_ext_adc"]
fn read_ext_adc<'a, Acs: Pin, Ccs: Pin, I: embassy_rp::spi::Instance, M: embassy_rp::spi::Mode>(
pub fn read_ext_adc<
'a,
Acs: Pin,
Ccs: Pin,
I: embassy_rp::spi::Instance,
M: embassy_rp::spi::Mode,
>(
which_stick: Stick,
which_axis: StickAxis,
spi: &mut Spi<'a, I, M>,
@ -110,16 +129,7 @@ fn read_ext_adc<'a, Acs: Pin, Ccs: Pin, I: embassy_rp::spi::Instance, M: embassy
/// Gets the average stick state over a 1ms interval in a non-blocking fashion.
/// Will wait until end_time is reached before continuing after reading the ADCs.
#[link_section = ".time_critical.update_stick_states"]
async fn update_stick_states<
'a,
Acs: Pin,
Ccs: Pin,
I: embassy_rp::spi::Instance,
M: embassy_rp::spi::Mode,
>(
mut spi: &mut Spi<'a, I, M>,
mut spi_acs: &mut Output<'a, Acs>,
mut spi_ccs: &mut Output<'a, Ccs>,
async fn update_stick_states(
current_stick_state: &StickState,
controlstick_params: &StickParams,
cstick_params: &StickParams,
@ -130,6 +140,7 @@ async fn update_stick_states<
old_stick_pos: &mut StickPositions,
raw_stick_values: &mut RawStickValues,
kalman_state: &mut KalmanState,
is_calibrating: bool,
) -> StickState {
let mut adc_count = 0u32;
let mut ax_sum = 0u32;
@ -139,39 +150,23 @@ async fn update_stick_states<
let end_time = Instant::now() + Duration::from_micros(300); // this seems kinda magic, and it is, but
let mut spi_unlocked = SPI_SHARED.lock().await;
let mut spi_acs_unlocked = SPI_ACS_SHARED.lock().await;
let mut spi_ccs_unlocked = SPI_CCS_SHARED.lock().await;
let spi = spi_unlocked.as_mut().unwrap();
let spi_acs = spi_acs_unlocked.as_mut().unwrap();
let spi_ccs = spi_ccs_unlocked.as_mut().unwrap();
// "do-while at home"
while {
let loop_start = Instant::now();
adc_count += 1;
ax_sum += read_ext_adc(
Stick::ControlStick,
StickAxis::XAxis,
&mut spi,
&mut spi_acs,
&mut spi_ccs,
) as u32;
ay_sum += read_ext_adc(
Stick::ControlStick,
StickAxis::YAxis,
&mut spi,
&mut spi_acs,
&mut spi_ccs,
) as u32;
cx_sum += read_ext_adc(
Stick::CStick,
StickAxis::XAxis,
&mut spi,
&mut spi_acs,
&mut spi_ccs,
) as u32;
cy_sum += read_ext_adc(
Stick::CStick,
StickAxis::YAxis,
&mut spi,
&mut spi_acs,
&mut spi_ccs,
) as u32;
ax_sum += read_ext_adc(Stick::ControlStick, StickAxis::XAxis, spi, spi_acs, spi_ccs) as u32;
ay_sum += read_ext_adc(Stick::ControlStick, StickAxis::YAxis, spi, spi_acs, spi_ccs) as u32;
cx_sum += read_ext_adc(Stick::CStick, StickAxis::XAxis, spi, spi_acs, spi_ccs) as u32;
cy_sum += read_ext_adc(Stick::CStick, StickAxis::YAxis, spi, spi_acs, spi_ccs) as u32;
let loop_end = Instant::now();
loop_end < end_time - (loop_end - loop_start)
@ -265,7 +260,7 @@ async fn update_stick_states<
controlstick_params,
controller_config,
Stick::ControlStick,
false,
is_calibrating,
) {
(x, y) => XyValuePair { x, y },
};
@ -275,7 +270,7 @@ async fn update_stick_states<
cstick_params,
controller_config,
Stick::CStick,
false,
is_calibrating,
) {
(x, y) => XyValuePair { x, y },
};
@ -285,7 +280,7 @@ async fn update_stick_states<
controlstick_params,
controller_config,
Stick::ControlStick,
false,
is_calibrating,
) {
(x, y) => XyValuePair { x, y },
};
@ -295,7 +290,7 @@ async fn update_stick_states<
cstick_params,
controller_config,
Stick::CStick,
false,
is_calibrating,
) {
(x, y) => XyValuePair { x, y },
};
@ -440,16 +435,19 @@ pub async fn update_button_state_task(
);
// not every loop pass is going to update the stick state
match STICK_SIGNAL.try_take() {
Some(stick_state) => {
gcc_state.stick_x = stick_state.ax;
gcc_state.stick_y = stick_state.ay;
gcc_state.cstick_x = stick_state.cx;
gcc_state.cstick_y = stick_state.cy;
}
None => (),
if let Some(stick_state) = SIGNAL_STICK_STATE.try_take() {
gcc_state.stick_x = stick_state.ax;
gcc_state.stick_y = stick_state.ay;
gcc_state.cstick_x = stick_state.cx;
gcc_state.cstick_y = stick_state.cy;
}
// check for a gcc state override (usually coming from the config task)
if let Some(override_gcc_state) = SIGNAL_OVERRIDE_GCC_STATE.try_take() {
gcc_publisher.publish_immediate(override_gcc_state.report);
Timer::after_millis(override_gcc_state.duration_ms).await;
};
gcc_publisher.publish_immediate(gcc_state);
// give other tasks a chance to do something
@ -459,6 +457,8 @@ pub async fn update_button_state_task(
/// Task responsible for updating the stick states.
/// Publishes the result to STICK_SIGNAL.
///
/// Has to run on core0 because it makes use of SPI0.
#[embassy_executor::task]
pub async fn update_stick_states_task(
mut spi: Spi<'static, SPI0, embassy_rp::spi::Blocking>,
@ -490,13 +490,12 @@ pub async fn update_stick_states_task(
// the time at which the current loop iteration should end
let mut end_time = Instant::now() + Duration::from_micros(1000);
let mut is_calibrating = false;
loop {
let timer = Timer::at(end_time);
current_stick_state = update_stick_states(
&mut spi,
&mut spi_acs,
&mut spi_ccs,
&current_stick_state,
&controlstick_params,
&cstick_params,
@ -507,22 +506,27 @@ pub async fn update_stick_states_task(
&mut old_stick_pos,
&mut raw_stick_values,
&mut kalman_state,
is_calibrating,
)
.await;
timer.await;
end_time += Duration::from_micros(1000);
if let Some(new_calibrating) = SIGNAL_IS_CALIBRATING.try_take() {
is_calibrating = new_calibrating;
}
match Instant::now() {
n => {
match (n - last_loop_time).as_micros() {
a if a > 1100 => debug!("Loop took {} us", a),
a if a > 1999 => debug!("Loop took {} us", a),
_ => {}
};
last_loop_time = n;
}
};
STICK_SIGNAL.signal(current_stick_state.clone());
SIGNAL_STICK_STATE.signal(current_stick_state.clone());
}
}

9
src/main.rs
View file

@ -118,7 +118,14 @@ fn main() -> ! {
// pwm_brake.set_counter(255);
executor0.run(|spawner| {
spawner.spawn(config_task()).unwrap();
// Config task has to run on core0 because it reads and writes to flash.
spawner
.spawn(config_task(controller_config.clone(), flash))
.unwrap();
// Stick loop has to run on core0 because it makes use of SPI0.
// Perhaps in the future we can rewire the board to have it make use of SPI1 instead.
// This way it could be the sole task running on core1, and everything else could happen on core0.
spawner
.spawn(update_stick_states_task(
spi,

213
src/stick.rs
View file

@ -6,7 +6,7 @@ use defmt::{debug, Format};
use libm::{atan2f, cosf, fabs, fabsf, roundf, sinf, sqrtf};
use crate::{
config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS},
config::{ControllerConfig, StickConfig, DEFAULT_ANGLES, DEFAULT_NOTCH_STATUS},
helpers::{ToRegularArray, XyValuePair},
input::Stick,
};
@ -15,7 +15,7 @@ use crate::{
const FIT_ORDER: usize = 3;
const NUM_COEFFS: usize = FIT_ORDER + 1;
pub const NO_OF_NOTCHES: usize = 16;
const NO_OF_ADJ_NOTCHES: usize = 12;
pub const NO_OF_ADJ_NOTCHES: usize = 12;
pub const NO_OF_CALIBRATION_POINTS: usize = 32;
const MAX_ORDER: usize = 20;
@ -45,9 +45,9 @@ impl StickParams {
/// Generate StickParams structs for the sticks, returned as a tuple of (analog_stick, c_stick)
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(),
stick_config.cal_points_x.to_regular_array(),
stick_config.cal_points_y.to_regular_array(),
stick_config.angles.to_regular_array(),
);
let linearized_cal = LinearizedCalibration::from_calibration_points(&cleaned_cal_points);
@ -68,7 +68,7 @@ impl StickParams {
}
}
#[derive(Clone, Debug, Format, Copy)]
#[derive(Clone, Debug, Format, Copy, Eq, PartialEq)]
pub enum NotchStatus {
TertInactive,
TertActive,
@ -387,6 +387,190 @@ impl NotchCalibration {
}
}
struct AppliedCalibration {
stick_params: StickParams,
cleaned_calibration: CleanedCalibrationPoints,
notch_angles: [f32; NO_OF_NOTCHES],
measured_notch_angles: [f32; NO_OF_NOTCHES],
}
impl AppliedCalibration {
pub fn from_points(
cal_points_x: &[f32; NO_OF_CALIBRATION_POINTS],
cal_points_y: &[f32; NO_OF_CALIBRATION_POINTS],
stick_config: &StickConfig,
which_stick: Stick,
) -> Self {
let mut stick_params = StickParams::from_stick_config(stick_config);
let angles = stick_config.angles;
let (stripped_cal_points_x, stripped_cal_points_y) =
strip_cal_points(cal_points_x, cal_points_y);
let stripped_cleaned_calibration = CleanedCalibrationPoints::from_temp_calibration_points(
&stripped_cal_points_x,
&stripped_cal_points_y,
&DEFAULT_ANGLES,
);
let linearized_calibration =
LinearizedCalibration::from_calibration_points(&stripped_cleaned_calibration);
stick_params.fit_coeffs = XyValuePair {
x: linearized_calibration.fit_coeffs.x.map(|e| e as f32),
y: linearized_calibration.fit_coeffs.y.map(|e| e as f32),
};
let notch_calibration = NotchCalibration::from_cleaned_and_linearized_calibration(
&stripped_cleaned_calibration,
&linearized_calibration,
);
stick_params.affine_coeffs = notch_calibration.affine_coeffs;
stick_params.boundary_angles = notch_calibration.boundary_angles;
let original_cleaned_calibration = CleanedCalibrationPoints::from_temp_calibration_points(
cal_points_x,
cal_points_y,
&DEFAULT_ANGLES,
);
let (transformed_cal_points_x, transformed_cal_points_y) = transform_cal_points(
&original_cleaned_calibration.cleaned_points.x,
&original_cleaned_calibration.cleaned_points.y,
&stick_params,
stick_config,
);
let measured_notch_angles =
compute_stick_angles(&transformed_cal_points_x, &transformed_cal_points_y);
let cleaned_with_measured_notch_angles =
CleanedCalibrationPoints::from_temp_calibration_points(
cal_points_x,
cal_points_y,
&measured_notch_angles,
);
let cleaned_notch_angles = clean_notches(
&measured_notch_angles,
&cleaned_with_measured_notch_angles.notch_status,
);
let cleaned_full = CleanedCalibrationPoints::from_temp_calibration_points(
cal_points_x,
cal_points_y,
&cleaned_notch_angles,
);
let linearized_full = LinearizedCalibration::from_calibration_points(&cleaned_full);
stick_params.fit_coeffs = XyValuePair {
x: linearized_full.fit_coeffs.x.map(|e| e as f32),
y: linearized_full.fit_coeffs.y.map(|e| e as f32),
};
let notch_calibrate_full = NotchCalibration::from_cleaned_and_linearized_calibration(
&cleaned_full,
&linearized_full,
);
stick_params.affine_coeffs = notch_calibrate_full.affine_coeffs;
stick_params.boundary_angles = notch_calibrate_full.boundary_angles;
Self {
stick_params,
measured_notch_angles,
notch_angles: cleaned_notch_angles,
cleaned_calibration: cleaned_full,
}
}
}
/// Sets notches to measured values if absent.
fn clean_notches(
measured_notch_angles: &[f32; NO_OF_NOTCHES],
notch_status: &[NotchStatus; NO_OF_NOTCHES],
) -> [f32; NO_OF_NOTCHES] {
let mut out = [0f32; NO_OF_NOTCHES];
for i in 0..NO_OF_NOTCHES {
if notch_status[i] == NotchStatus::TertInactive {
out[i] = measured_notch_angles[i];
}
}
out
}
fn angle_on_sphere(x: f32, y: f32) -> f32 {
let xx = sinf(x * MAX_STICK_ANGLE / 100.) * cosf(y * MAX_STICK_ANGLE / 100.);
let yy = cosf(x * MAX_STICK_ANGLE / 100.) * sinf(y * MAX_STICK_ANGLE / 100.);
match atan2f(yy, xx) {
a if a < 0. => a + 2. * PI,
a => a,
}
}
fn compute_stick_angles(
x_in: &[f32; NO_OF_NOTCHES + 1],
y_in: &[f32; NO_OF_NOTCHES + 1],
) -> [f32; NO_OF_NOTCHES] {
let mut angles = [0f32; NO_OF_NOTCHES];
for i in 0..NO_OF_NOTCHES {
if i % 2 == 0 {
angles[i] = DEFAULT_ANGLES[i];
} else {
angles[i] = angle_on_sphere(x_in[i], y_in[i]);
}
}
angles
}
fn transform_cal_points(
cal_points_x: &[f32; NO_OF_NOTCHES + 1],
cal_points_y: &[f32; NO_OF_NOTCHES + 1],
stick_params: &StickParams,
stick_config: &StickConfig,
) -> ([f32; NO_OF_NOTCHES + 1], [f32; NO_OF_NOTCHES + 1]) {
let mut transformed_points_x = [0f32; NO_OF_NOTCHES + 1];
let mut transformed_points_y = [0f32; NO_OF_NOTCHES + 1];
for i in 0..NO_OF_NOTCHES + 1 {
let x = linearize(cal_points_x[i], &stick_params.fit_coeffs.x);
let y = linearize(cal_points_y[i], &stick_params.fit_coeffs.y);
let (out_x, out_y) = notch_remap(x, y, stick_params, stick_config, true);
transformed_points_x[i] = out_x;
transformed_points_y[i] = out_y;
}
(transformed_points_x, transformed_points_y)
}
/// Removes the notches from un-cleaned cal points
/// so we can get the original values of the notches after the affine transform.
fn strip_cal_points(
cal_points_x: &[f32; NO_OF_CALIBRATION_POINTS],
cal_points_y: &[f32; NO_OF_CALIBRATION_POINTS],
) -> (
[f32; NO_OF_CALIBRATION_POINTS],
[f32; NO_OF_CALIBRATION_POINTS],
) {
let mut stripped_points_x = [0f32; NO_OF_CALIBRATION_POINTS];
let mut stripped_points_y = [0f32; NO_OF_CALIBRATION_POINTS];
for i in 0..NO_OF_CALIBRATION_POINTS {
(stripped_points_x[i], stripped_points_y[i]) = if (i + 1) % 4 == 0 {
(cal_points_x[0], cal_points_y[0])
} else {
(cal_points_x[i], cal_points_y[i])
}
}
(stripped_points_x, stripped_points_y)
}
fn inverse(in_mat: &[[f32; 3]; 3]) -> [[f32; 3]; 3] {
let mut out_mat = [[0f32; 3]; 3];
@ -561,7 +745,7 @@ fn fit_curve<const N: usize, const NCOEFFS: usize>(
/// Compute the stick x/y coordinates from a given angle.
/// The stick moves spherically, so it requires 3D trigonometry.
fn calc_stick_values(angle: f32) -> (f32, f32) {
pub fn calc_stick_values(angle: f32) -> (f32, f32) {
let x =
100. * atan2f(sinf(MAX_STICK_ANGLE) * cosf(angle), cosf(MAX_STICK_ANGLE)) / MAX_STICK_ANGLE;
let y =
@ -571,7 +755,7 @@ fn calc_stick_values(angle: f32) -> (f32, f32) {
}
#[link_section = ".time_critical.linearize"]
pub fn linearize(point: f32, coefficients: &[f32; 4]) -> f32 {
pub fn linearize(point: f32, coefficients: &[f32; NUM_COEFFS]) -> f32 {
coefficients[0] * (point * point * point)
+ coefficients[1] * (point * point)
+ coefficients[2] * point
@ -583,8 +767,7 @@ pub fn notch_remap(
x_in: f32,
y_in: f32,
stick_params: &StickParams,
controller_config: &ControllerConfig,
which_stick: Stick,
stick_config: &StickConfig,
is_calibrating: bool,
) -> (f32, f32) {
//determine the angle between the x unit vector and the current position vector
@ -606,10 +789,7 @@ pub fn notch_remap(
NO_OF_NOTCHES - 1
};
let stick_scale = match which_stick {
Stick::ControlStick => controller_config.astick_config.analog_scaler as f32 / 100.,
Stick::CStick => controller_config.cstick_config.analog_scaler as f32 / 100.,
};
let stick_scale = stick_config.analog_scaler as f32 / 100.;
let mut x_out = stick_scale
* (stick_params.affine_coeffs[region][0] * x_in
@ -619,11 +799,6 @@ pub fn notch_remap(
+ stick_params.affine_coeffs[region][3] * y_in);
if !is_calibrating {
let stick_config = match which_stick {
Stick::ControlStick => &controller_config.astick_config,
Stick::CStick => &controller_config.cstick_config,
};
if stick_config.cardinal_snapping > 0 {
if fabsf(x_out) < stick_config.cardinal_snapping as f32 + 0.5 && fabsf(y_out) >= 79.5 {
x_out = 0.;