forked from NaxdyOrg/NaxGCC-FW
chore: refactor and finish stick cal
This commit is contained in:
parent
40f734f25d
commit
5ab51f5670
GPG key ID:
CC15075846BCE91B
6 changed files with 411 additions and 239 deletions
204
src/config.rs
Normal file
204
src/config.rs
Normal file
|
|
@ -0,0 +1,204 @@
|
|||
/**
|
||||
* Storage for controller configuration, including helper functions & types, as well as sane defaults.
|
||||
*/
|
||||
use core::f32::consts::PI;
|
||||
|
||||
use defmt::{info, warn, Format};
|
||||
use embassy_rp::{
|
||||
flash::{Async, Flash, ERASE_SIZE},
|
||||
peripherals::FLASH,
|
||||
};
|
||||
use packed_struct::{derive::PackedStruct, PackedStruct};
|
||||
|
||||
use crate::{
|
||||
packed_float::{PackedFloat, ToPackedFloatArray},
|
||||
stick::{NotchStatus, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES},
|
||||
ADDR_OFFSET, FLASH_SIZE,
|
||||
};
|
||||
|
||||
/// This needs to be incremented for ANY change to ControllerConfig
|
||||
/// else we risk loading uninitialized memory.
|
||||
pub const CONTROLLER_CONFIG_REVISION: u8 = 2;
|
||||
|
||||
pub const DEFAULT_NOTCH_STATUS: [NotchStatus; NO_OF_NOTCHES] = [
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
];
|
||||
|
||||
#[rustfmt::skip]
|
||||
const DEFAULT_CAL_POINTS_X: [f32; NO_OF_CALIBRATION_POINTS] = [
|
||||
0.3010610568,0.3603937084,// right
|
||||
0.3010903951,0.3000194135,
|
||||
0.3005567843,0.3471911134,// up right
|
||||
0.3006904343,0.3009976295,
|
||||
0.3000800899,0.300985051,// up
|
||||
0.3001020858,0.300852804,
|
||||
0.3008746305,0.2548450139,// up left
|
||||
0.3001434092,0.3012600593,
|
||||
0.3011594091,0.2400535218,// left
|
||||
0.3014621077,0.3011248469,
|
||||
0.3010860944,0.2552106305,// down left
|
||||
0.3002197989,0.3001679513,
|
||||
0.3004438517,0.300486505,// down
|
||||
0.3002766984,0.3012828579,
|
||||
0.3014959877,0.346512936,// down right
|
||||
0.3013398149,0.3007809916
|
||||
];
|
||||
|
||||
#[rustfmt::skip]
|
||||
const DEFAULT_CAL_POINTS_Y: [f32; NO_OF_CALIBRATION_POINTS] = [
|
||||
0.300092277, 0.3003803475,// right
|
||||
0.3002205792,0.301004752,
|
||||
0.3001241394,0.3464200104,// up right
|
||||
0.3001331245,0.3011881186,
|
||||
0.3010685972,0.3606900641,// up
|
||||
0.3001520488,0.3010662947,
|
||||
0.3008837105,0.3461478452,// up left
|
||||
0.3011732026,0.3007367683,
|
||||
0.3011345742,0.3000566197,// left
|
||||
0.3006843288,0.3009673425,
|
||||
0.3011228978,0.2547579852,// down left
|
||||
0.3011177285,0.301264851,
|
||||
0.3002376991,0.2403885431,// down
|
||||
0.3006540818,0.3010588401,
|
||||
0.3011093054,0.2555000655,// down right
|
||||
0.3000802760,0.3008482317
|
||||
];
|
||||
|
||||
const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [
|
||||
0.,
|
||||
PI / 8.0,
|
||||
PI * 2. / 8.,
|
||||
PI * 3. / 8.,
|
||||
PI * 4. / 8.,
|
||||
PI * 5. / 8.,
|
||||
PI * 6. / 8.,
|
||||
PI * 7. / 8.,
|
||||
PI * 8. / 8.,
|
||||
PI * 9. / 8.,
|
||||
PI * 10. / 8.,
|
||||
PI * 11. / 8.,
|
||||
PI * 12. / 8.,
|
||||
PI * 13. / 8.,
|
||||
PI * 14. / 8.,
|
||||
PI * 15. / 8.,
|
||||
];
|
||||
|
||||
#[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")]
|
||||
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],
|
||||
}
|
||||
|
||||
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,
|
||||
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(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
flash.blocking_read(ADDR_OFFSET, &mut controller_config_packed)?;
|
||||
|
||||
match ControllerConfig::unpack(&controller_config_packed).unwrap() {
|
||||
a if a.config_revision == CONTROLLER_CONFIG_REVISION => {
|
||||
info!("Controller config loaded from flash: {}", a);
|
||||
Ok(a)
|
||||
}
|
||||
a => {
|
||||
warn!("Outdated controller config detected ({:02X}), or controller config was never present, using default.", a.config_revision);
|
||||
let cfg = ControllerConfig::default();
|
||||
info!("Going to save default controller config.");
|
||||
cfg.write_to_flash(&mut flash)?;
|
||||
Ok(cfg)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn write_to_flash(
|
||||
&self,
|
||||
flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
|
||||
) -> Result<(), embassy_rp::flash::Error> {
|
||||
info!("Writing controller config to flash.");
|
||||
flash.blocking_erase(ADDR_OFFSET, ADDR_OFFSET + ERASE_SIZE as u32)?;
|
||||
flash.blocking_write(ADDR_OFFSET, &self.pack().unwrap())?;
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
|
@ -1,7 +1,7 @@
|
|||
use defmt::Format;
|
||||
use libm::{fminf, powf};
|
||||
|
||||
use crate::input::{ControllerConfig, Stick};
|
||||
use crate::config::ControllerConfig;
|
||||
|
||||
/// Filter gains for 800Hz, the ones for 1000Hz are provided by `get_norm_gains`
|
||||
pub const FILTER_GAINS: FilterGains = FilterGains {
|
||||
|
|
@ -85,7 +85,7 @@ pub struct FilterGains {
|
|||
|
||||
impl FilterGains {
|
||||
/// Returns filter gains for 1000Hz polling rate
|
||||
pub fn normalize_gains(&self, controller_config: &ControllerConfig) -> Self {
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -1,21 +1,20 @@
|
|||
/**
|
||||
* Communication with the console / PC over USB HID.
|
||||
* Includes the HID report descriptor, and the GcReport struct.
|
||||
*/
|
||||
use core::default::Default;
|
||||
|
||||
use defmt::{debug, error, info, trace, unwrap, warn, Debug2Format, Format};
|
||||
use embassy_futures::{
|
||||
join::join,
|
||||
select::{self, select, Either},
|
||||
};
|
||||
use defmt::{debug, info, trace, warn, Format};
|
||||
use embassy_futures::join::join;
|
||||
use embassy_rp::{peripherals::USB, usb::Driver};
|
||||
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
|
||||
use embassy_sync::signal::Signal;
|
||||
use embassy_time::{Duration, Instant};
|
||||
|
||||
use embassy_time::Instant;
|
||||
use embassy_usb::{
|
||||
class::hid::{HidReaderWriter, ReportId, RequestHandler, State},
|
||||
control::OutResponse,
|
||||
Builder, Handler,
|
||||
};
|
||||
use packed_struct::{derive::PackedStruct, PackedStruct};
|
||||
use portable_atomic::Ordering;
|
||||
|
||||
use crate::input::GCC_SIGNAL;
|
||||
|
||||
|
|
@ -238,8 +237,7 @@ impl Handler for MyDeviceHandler {
|
|||
pub async fn usb_transfer_loop(driver: Driver<'static, USB>, raw_serial: [u8; 8]) {
|
||||
let mut serial_buffer = [0u8; 64];
|
||||
|
||||
let serial = {
|
||||
let s = format_no_std::show(
|
||||
let serial = format_no_std::show(
|
||||
&mut serial_buffer,
|
||||
format_args!(
|
||||
"{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}",
|
||||
|
|
@ -255,10 +253,7 @@ pub async fn usb_transfer_loop(driver: Driver<'static, USB>, raw_serial: [u8; 8]
|
|||
)
|
||||
.unwrap();
|
||||
|
||||
info!("Detected flash with unique serial number {}", s);
|
||||
|
||||
s
|
||||
};
|
||||
info!("Detected flash with unique serial number {}", serial);
|
||||
|
||||
trace!("Start of config");
|
||||
let mut usb_config = embassy_usb::Config::new(0x057e, 0x0337);
|
||||
|
|
|
|||
125
src/input.rs
125
src/input.rs
|
|
@ -1,9 +1,9 @@
|
|||
use core::task::Poll;
|
||||
|
||||
use defmt::{debug, info, warn, Format};
|
||||
use defmt::info;
|
||||
use embassy_futures::{join::join, yield_now};
|
||||
use embassy_rp::{
|
||||
flash::{Async, Flash, ERASE_SIZE},
|
||||
flash::{Async, Flash},
|
||||
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,
|
||||
|
|
@ -14,18 +14,14 @@ 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, PackedStruct};
|
||||
use libm::{fmaxf, fminf};
|
||||
|
||||
use crate::{
|
||||
config::ControllerConfig,
|
||||
filter::{run_kalman, run_waveshaping, FilterGains, WaveshapingValues, FILTER_GAINS},
|
||||
gcc_hid::GcReport,
|
||||
packed_float::{PackedFloat, ToPackedFloatArray},
|
||||
stick::{
|
||||
linearize, notch_remap, StickParams, DEFAULT_ANGLES, DEFAULT_CAL_POINTS_X,
|
||||
DEFAULT_CAL_POINTS_Y, NO_OF_NOTCHES,
|
||||
},
|
||||
ADDR_OFFSET, FLASH_SIZE,
|
||||
stick::{linearize, notch_remap, StickParams},
|
||||
FLASH_SIZE,
|
||||
};
|
||||
|
||||
pub static GCC_SIGNAL: Signal<CriticalSectionRawMutex, GcReport> = Signal::new();
|
||||
|
|
@ -34,113 +30,6 @@ static STICK_SIGNAL: Signal<CriticalSectionRawMutex, StickState> = Signal::new()
|
|||
const STICK_HYST_VAL: f32 = 0.3;
|
||||
const FLOAT_ORIGIN: f32 = 127.5;
|
||||
|
||||
pub const CONTROLLER_CONFIG_REVISION: u8 = 2;
|
||||
|
||||
#[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")]
|
||||
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],
|
||||
}
|
||||
|
||||
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,
|
||||
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(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
flash.blocking_read(ADDR_OFFSET, &mut controller_config_packed)?;
|
||||
|
||||
match ControllerConfig::unpack(&controller_config_packed).unwrap() {
|
||||
a if a.config_revision == CONTROLLER_CONFIG_REVISION => {
|
||||
info!("Controller config loaded from flash: {}", a);
|
||||
Ok(a)
|
||||
}
|
||||
a => {
|
||||
warn!("Outdated controller config detected ({:02X}), or controller config was never present, using default.", a.config_revision);
|
||||
let cfg = ControllerConfig::default();
|
||||
info!("Going to save default controller config.");
|
||||
cfg.write_to_flash(&mut flash)?;
|
||||
Ok(cfg)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn write_to_flash(
|
||||
&self,
|
||||
flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
|
||||
) -> Result<(), embassy_rp::flash::Error> {
|
||||
info!("Writing controller config to flash.");
|
||||
flash.blocking_erase(ADDR_OFFSET, ADDR_OFFSET + ERASE_SIZE as u32)?;
|
||||
flash.blocking_write(ADDR_OFFSET, &self.pack().unwrap())?;
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Default)]
|
||||
struct StickState {
|
||||
ax: u8,
|
||||
|
|
@ -493,7 +382,7 @@ pub async fn input_loop(
|
|||
let (controlstick_params, cstick_params) =
|
||||
StickParams::from_controller_config(&controller_config);
|
||||
|
||||
let filter_gains = FILTER_GAINS.normalize_gains(&controller_config);
|
||||
let filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config);
|
||||
|
||||
let stick_state_fut = async {
|
||||
let mut current_stick_state = StickState {
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
|
||||
#![no_std]
|
||||
#![no_main]
|
||||
mod config;
|
||||
mod filter;
|
||||
mod gcc_hid;
|
||||
mod input;
|
||||
|
|
|
|||
263
src/stick.rs
263
src/stick.rs
|
|
@ -6,7 +6,8 @@ use defmt::{debug, Format};
|
|||
use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf};
|
||||
|
||||
use crate::{
|
||||
input::{ControllerConfig, Stick},
|
||||
config::{ControllerConfig, DEFAULT_NOTCH_STATUS},
|
||||
input::Stick,
|
||||
packed_float::ToRegularArray,
|
||||
};
|
||||
|
||||
|
|
@ -21,84 +22,6 @@ const MAX_ORDER: usize = 20;
|
|||
/// 28 degrees; this is the max angular deflection of the stick.
|
||||
const MAX_STICK_ANGLE: f32 = 0.4886921906;
|
||||
|
||||
const DEFAULT_NOTCH_STATUS: [NotchStatus; NO_OF_NOTCHES] = [
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Cardinal,
|
||||
NotchStatus::TertActive,
|
||||
NotchStatus::Secondary,
|
||||
NotchStatus::TertActive,
|
||||
];
|
||||
|
||||
#[rustfmt::skip]
|
||||
pub const DEFAULT_CAL_POINTS_X: [f32; NO_OF_CALIBRATION_POINTS] = [
|
||||
0.3010610568,0.3603937084,// right
|
||||
0.3010903951,0.3000194135,
|
||||
0.3005567843,0.3471911134,// up right
|
||||
0.3006904343,0.3009976295,
|
||||
0.3000800899,0.300985051,// up
|
||||
0.3001020858,0.300852804,
|
||||
0.3008746305,0.2548450139,// up left
|
||||
0.3001434092,0.3012600593,
|
||||
0.3011594091,0.2400535218,// left
|
||||
0.3014621077,0.3011248469,
|
||||
0.3010860944,0.2552106305,// down left
|
||||
0.3002197989,0.3001679513,
|
||||
0.3004438517,0.300486505,// down
|
||||
0.3002766984,0.3012828579,
|
||||
0.3014959877,0.346512936,// down right
|
||||
0.3013398149,0.3007809916
|
||||
];
|
||||
|
||||
#[rustfmt::skip]
|
||||
pub const DEFAULT_CAL_POINTS_Y: [f32; NO_OF_CALIBRATION_POINTS] = [
|
||||
0.300092277, 0.3003803475,// right
|
||||
0.3002205792,0.301004752,
|
||||
0.3001241394,0.3464200104,// up right
|
||||
0.3001331245,0.3011881186,
|
||||
0.3010685972,0.3606900641,// up
|
||||
0.3001520488,0.3010662947,
|
||||
0.3008837105,0.3461478452,// up left
|
||||
0.3011732026,0.3007367683,
|
||||
0.3011345742,0.3000566197,// left
|
||||
0.3006843288,0.3009673425,
|
||||
0.3011228978,0.2547579852,// down left
|
||||
0.3011177285,0.301264851,
|
||||
0.3002376991,0.2403885431,// down
|
||||
0.3006540818,0.3010588401,
|
||||
0.3011093054,0.2555000655,// down right
|
||||
0.3000802760,0.3008482317
|
||||
];
|
||||
|
||||
pub const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [
|
||||
0.,
|
||||
PI / 8.0,
|
||||
PI * 2. / 8.,
|
||||
PI * 3. / 8.,
|
||||
PI * 4. / 8.,
|
||||
PI * 5. / 8.,
|
||||
PI * 6. / 8.,
|
||||
PI * 7. / 8.,
|
||||
PI * 8. / 8.,
|
||||
PI * 9. / 8.,
|
||||
PI * 10. / 8.,
|
||||
PI * 11. / 8.,
|
||||
PI * 12. / 8.,
|
||||
PI * 13. / 8.,
|
||||
PI * 14. / 8.,
|
||||
PI * 15. / 8.,
|
||||
];
|
||||
|
||||
#[rustfmt::skip]
|
||||
// right notch 1 up right notch 2 up notch 3 up left notch 4 left notch 5 down left notch 6 down notch 7 down right notch 8
|
||||
// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
|
||||
|
|
@ -134,12 +57,40 @@ impl StickParams {
|
|||
&controller_config.c_angles.to_regular_array(),
|
||||
);
|
||||
|
||||
todo!()
|
||||
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)
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Format, Copy)]
|
||||
enum NotchStatus {
|
||||
pub enum NotchStatus {
|
||||
TertInactive,
|
||||
TertActive,
|
||||
Secondary,
|
||||
|
|
@ -293,8 +244,8 @@ struct LinearizedCalibration {
|
|||
pub fit_coeffs_x: [f64; NUM_COEFFS],
|
||||
pub fit_coeffs_y: [f64; NUM_COEFFS],
|
||||
|
||||
pub out_x: [f32; NO_OF_NOTCHES],
|
||||
pub out_y: [f32; NO_OF_NOTCHES],
|
||||
pub linearized_points_x: [f32; NO_OF_NOTCHES + 1],
|
||||
pub linearized_points_y: [f32; NO_OF_NOTCHES + 1],
|
||||
}
|
||||
|
||||
impl LinearizedCalibration {
|
||||
|
|
@ -306,10 +257,17 @@ impl LinearizedCalibration {
|
|||
///
|
||||
/// Outputs:
|
||||
/// linearization fit coefficients for X and Y
|
||||
pub fn from_calibration_points(in_x: &[f64; 17], in_y: &[f64; 17]) -> Self {
|
||||
pub fn from_calibration_points(cleaned_calibration_points: &CleanedCalibrationPoints) -> Self {
|
||||
let mut fit_points_x = [0f64; 5];
|
||||
let mut fit_points_y = [0f64; 5];
|
||||
|
||||
let in_x = cleaned_calibration_points
|
||||
.cleaned_points_x
|
||||
.map(|e| e as f64);
|
||||
let in_y = cleaned_calibration_points
|
||||
.cleaned_points_y
|
||||
.map(|e| e as f64);
|
||||
|
||||
fit_points_x[0] = in_x[8 + 1];
|
||||
fit_points_x[1] = (in_x[6 + 1] + in_x[10 + 1]) / 2.0f64;
|
||||
fit_points_x[2] = in_x[0];
|
||||
|
|
@ -336,23 +294,148 @@ impl LinearizedCalibration {
|
|||
fit_coeffs_x[3] = fit_coeffs_x[3] - x_zero_error as f64;
|
||||
fit_coeffs_y[3] = fit_coeffs_y[3] - y_zero_error as f64;
|
||||
|
||||
let mut out_x = [0f32; NO_OF_NOTCHES];
|
||||
let mut out_y = [0f32; NO_OF_NOTCHES];
|
||||
let mut linearized_points_x = [0f32; NO_OF_NOTCHES + 1];
|
||||
let mut linearized_points_y = [0f32; NO_OF_NOTCHES + 1];
|
||||
|
||||
for i in 0..=NO_OF_NOTCHES {
|
||||
out_x[i] = linearize(in_x[i] as f32, &fit_coeffs_x.map(|e| e as f32));
|
||||
out_y[i] = linearize(in_y[i] as f32, &fit_coeffs_y.map(|e| e as f32));
|
||||
linearized_points_x[i] = linearize(in_x[i] as f32, &fit_coeffs_x.map(|e| e as f32));
|
||||
linearized_points_y[i] = linearize(in_y[i] as f32, &fit_coeffs_y.map(|e| e as f32));
|
||||
}
|
||||
|
||||
Self {
|
||||
fit_coeffs_x,
|
||||
fit_coeffs_y,
|
||||
out_x,
|
||||
out_y,
|
||||
linearized_points_x,
|
||||
linearized_points_y,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Default)]
|
||||
struct NotchCalibration {
|
||||
affine_coeffs: [[f32; 16]; 4],
|
||||
boundary_angles: [f32; 4],
|
||||
}
|
||||
|
||||
impl NotchCalibration {
|
||||
fn from_cleaned_and_linearized_calibration(
|
||||
cleaned_calibration_points: &CleanedCalibrationPoints,
|
||||
linearized_calibration: &LinearizedCalibration,
|
||||
) -> Self {
|
||||
let mut out = Self::default();
|
||||
|
||||
for i in 1..=NO_OF_NOTCHES {
|
||||
let mut points_in = [[0f32; 3]; 3];
|
||||
let mut points_out = [[0f32; 3]; 3];
|
||||
|
||||
if i == NO_OF_NOTCHES {
|
||||
points_in[0][0] = linearized_calibration.linearized_points_x[0];
|
||||
points_in[0][1] = linearized_calibration.linearized_points_x[i];
|
||||
points_in[0][2] = linearized_calibration.linearized_points_x[1];
|
||||
points_in[1][0] = linearized_calibration.linearized_points_y[0];
|
||||
points_in[1][1] = linearized_calibration.linearized_points_y[i];
|
||||
points_in[1][2] = linearized_calibration.linearized_points_y[1];
|
||||
points_in[2][0] = 1.;
|
||||
points_in[2][1] = 1.;
|
||||
points_in[2][2] = 1.;
|
||||
points_out[0][0] = cleaned_calibration_points.notch_points_x[0];
|
||||
points_out[0][1] = cleaned_calibration_points.notch_points_x[i];
|
||||
points_out[0][2] = cleaned_calibration_points.notch_points_x[1];
|
||||
points_out[1][0] = cleaned_calibration_points.notch_points_y[0];
|
||||
points_out[1][1] = cleaned_calibration_points.notch_points_y[i];
|
||||
points_out[1][2] = cleaned_calibration_points.notch_points_y[1];
|
||||
points_out[2][0] = 1.;
|
||||
points_out[2][1] = 1.;
|
||||
points_out[2][2] = 1.;
|
||||
} else {
|
||||
points_in[0][0] = linearized_calibration.linearized_points_x[0];
|
||||
points_in[0][1] = linearized_calibration.linearized_points_x[i];
|
||||
points_in[0][2] = linearized_calibration.linearized_points_x[i + 1];
|
||||
points_in[1][0] = linearized_calibration.linearized_points_y[0];
|
||||
points_in[1][1] = linearized_calibration.linearized_points_y[i];
|
||||
points_in[1][2] = linearized_calibration.linearized_points_y[i + 1];
|
||||
points_in[2][0] = 1.;
|
||||
points_in[2][1] = 1.;
|
||||
points_in[2][2] = 1.;
|
||||
points_out[0][0] = cleaned_calibration_points.notch_points_x[0];
|
||||
points_out[0][1] = cleaned_calibration_points.notch_points_x[i];
|
||||
points_out[0][2] = cleaned_calibration_points.notch_points_x[i + 1];
|
||||
points_out[1][0] = cleaned_calibration_points.notch_points_y[0];
|
||||
points_out[1][1] = cleaned_calibration_points.notch_points_y[i];
|
||||
points_out[1][2] = cleaned_calibration_points.notch_points_y[i + 1];
|
||||
points_out[2][0] = 1.;
|
||||
points_out[2][1] = 1.;
|
||||
points_out[2][2] = 1.;
|
||||
}
|
||||
debug!("In points: {:?}", points_in);
|
||||
debug!("Out points: {:?}", points_out);
|
||||
|
||||
let temp = inverse(&points_in);
|
||||
|
||||
let a = matrix_mult(&points_out, &temp);
|
||||
|
||||
debug!("The transform matrix is: {:?}", a);
|
||||
|
||||
for j in 0..2 {
|
||||
for k in 0..2 {
|
||||
out.affine_coeffs[i - 1][j * 2 + k] = a[j][k];
|
||||
}
|
||||
}
|
||||
|
||||
debug!(
|
||||
"Transform coefficients for this region are: {:?}",
|
||||
out.affine_coeffs[i - 1]
|
||||
);
|
||||
|
||||
out.boundary_angles[i - 1] = match atan2f(
|
||||
cleaned_calibration_points.cleaned_points_y[i]
|
||||
- cleaned_calibration_points.cleaned_points_y[0],
|
||||
cleaned_calibration_points.cleaned_points_x[i]
|
||||
- cleaned_calibration_points.cleaned_points_x[0],
|
||||
) {
|
||||
a if a < out.boundary_angles[0] => a + 2. * PI,
|
||||
a => a,
|
||||
};
|
||||
}
|
||||
|
||||
out
|
||||
}
|
||||
}
|
||||
|
||||
fn inverse(in_mat: &[[f32; 3]; 3]) -> [[f32; 3]; 3] {
|
||||
let mut out_mat = [[0f32; 3]; 3];
|
||||
|
||||
let det = in_mat[0][0] * (in_mat[1][1] * in_mat[2][2] - in_mat[1][2] * in_mat[2][1])
|
||||
- in_mat[0][1] * (in_mat[1][0] * in_mat[2][2] - in_mat[1][2] * in_mat[2][0])
|
||||
+ in_mat[0][2] * (in_mat[1][0] * in_mat[2][1] - in_mat[1][1] * in_mat[2][0]);
|
||||
|
||||
out_mat[0][0] = (in_mat[1][1] * in_mat[2][2] - in_mat[1][2] * in_mat[2][1]) / det;
|
||||
out_mat[0][1] = (in_mat[0][2] * in_mat[2][1] - in_mat[0][1] * in_mat[2][2]) / det;
|
||||
out_mat[0][2] = (in_mat[0][1] * in_mat[1][2] - in_mat[0][2] * in_mat[1][1]) / det;
|
||||
out_mat[1][0] = (in_mat[1][2] * in_mat[2][0] - in_mat[1][0] * in_mat[2][2]) / det;
|
||||
out_mat[1][1] = (in_mat[0][0] * in_mat[2][2] - in_mat[0][2] * in_mat[2][0]) / det;
|
||||
out_mat[1][2] = (in_mat[0][2] * in_mat[1][0] - in_mat[0][0] * in_mat[1][2]) / det;
|
||||
out_mat[2][0] = (in_mat[1][0] * in_mat[2][1] - in_mat[1][1] * in_mat[2][0]) / det;
|
||||
out_mat[2][1] = (in_mat[0][1] * in_mat[2][0] - in_mat[0][0] * in_mat[2][1]) / det;
|
||||
out_mat[2][2] = (in_mat[0][0] * in_mat[1][1] - in_mat[0][1] * in_mat[1][0]) / det;
|
||||
|
||||
out_mat
|
||||
}
|
||||
|
||||
fn matrix_mult(a: &[[f32; 3]; 3], b: &[[f32; 3]; 3]) -> [[f32; 3]; 3] {
|
||||
let mut out = [[0f32; 3]; 3];
|
||||
|
||||
for i in 0..3 {
|
||||
for j in 0..3 {
|
||||
for k in 0..3 {
|
||||
out[i][j] += a[i][k] * b[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
out
|
||||
}
|
||||
|
||||
/// Calculate the power of a number
|
||||
fn curve_fit_power(base: f64, exponent: u32) -> f64 {
|
||||
if exponent == 0 {
|
||||
|
|
|
|||
Loading…
Reference in a new issue