forked from NaxdyOrg/NaxGCC-FW
begin implementing stick shenanigans
This commit is contained in:
parent
cbaa4f4ca9
commit
5244f1a75e
5 changed files with 535 additions and 22 deletions
7
Cargo.lock
generated
7
Cargo.lock
generated
|
|
@ -828,6 +828,12 @@ version = "0.2.153"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "9c198f91728a82281a64e1f4f9eeb25d82cb32a5de251c6bd1b5154d63a8e7bd"
|
||||
|
||||
[[package]]
|
||||
name = "libm"
|
||||
version = "0.2.8"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058"
|
||||
|
||||
[[package]]
|
||||
name = "libredox"
|
||||
version = "0.0.1"
|
||||
|
|
@ -885,6 +891,7 @@ dependencies = [
|
|||
"fixed",
|
||||
"fixed-macro",
|
||||
"format_no_std",
|
||||
"libm",
|
||||
"packed_struct",
|
||||
"panic-probe",
|
||||
"portable-atomic",
|
||||
|
|
|
|||
|
|
@ -42,6 +42,7 @@ fixed = "1.23.1"
|
|||
fixed-macro = "1.2"
|
||||
static_cell = "2"
|
||||
portable-atomic = { version = "1.5", features = ["critical-section"] }
|
||||
libm = { version = "0.2.8" }
|
||||
|
||||
#cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] }
|
||||
cortex-m = { version = "0.7.6", features = ["inline-asm"] }
|
||||
|
|
|
|||
233
src/input.rs
233
src/input.rs
|
|
@ -1,21 +1,53 @@
|
|||
use defmt::{debug, info};
|
||||
use core::task::Poll;
|
||||
|
||||
use defmt::{debug, Format};
|
||||
use embassy_futures::{join::join, yield_now};
|
||||
use embassy_rp::{
|
||||
clocks::RoscRng,
|
||||
flash::{Async, Flash, ERASE_SIZE},
|
||||
flash::{Async, Flash},
|
||||
gpio::{Input, Output, Pin},
|
||||
peripherals::{
|
||||
DMA_CH0, FLASH, PIN_10, PIN_11, PIN_15, PIN_16, PIN_17, PIN_18, PIN_19, PIN_20, PIN_21,
|
||||
PIN_22, PIN_23, PIN_24, PIN_25, PIN_29, PIN_5, PIN_8, PIN_9, SPI0,
|
||||
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,
|
||||
},
|
||||
pwm::Pwm,
|
||||
spi::Spi,
|
||||
};
|
||||
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
|
||||
use rand::RngCore;
|
||||
use embassy_time::{Instant, Timer};
|
||||
use packed_struct::derive::PackedStruct;
|
||||
|
||||
use crate::{gcc_hid::GcReport, ADDR_OFFSET, FLASH_SIZE};
|
||||
use crate::{
|
||||
gcc_hid::GcReport,
|
||||
stick::{linearize, run_kalman, FilterGains, StickParams},
|
||||
PackedFloat, ADDR_OFFSET, FLASH_SIZE,
|
||||
};
|
||||
|
||||
pub static GCC_SIGNAL: Signal<CriticalSectionRawMutex, GcReport> = Signal::new();
|
||||
|
||||
static STICK_SIGNAL: Signal<CriticalSectionRawMutex, StickState> = Signal::new();
|
||||
|
||||
#[derive(Debug, Clone, Default, Format, PackedStruct)]
|
||||
#[packed_struct(endian = "msb")]
|
||||
pub struct ControllerConfig {
|
||||
#[packed_field(size_bits = "8")]
|
||||
pub config_version: u8,
|
||||
#[packed_field(size_bits = "32")]
|
||||
pub ax_waveshaping: PackedFloat,
|
||||
#[packed_field(size_bits = "32")]
|
||||
pub ay_waveshaping: PackedFloat,
|
||||
#[packed_field(size_bits = "32")]
|
||||
pub cx_waveshaping: PackedFloat,
|
||||
#[packed_field(size_bits = "32")]
|
||||
pub cy_waveshaping: PackedFloat,
|
||||
}
|
||||
|
||||
struct StickState {
|
||||
ax: u8,
|
||||
ay: u8,
|
||||
cx: u8,
|
||||
cy: u8,
|
||||
}
|
||||
|
||||
#[derive(PartialEq, Eq)]
|
||||
enum Stick {
|
||||
ControlStick,
|
||||
|
|
@ -28,10 +60,10 @@ enum StickAxis {
|
|||
YAxis,
|
||||
}
|
||||
|
||||
fn read_ext_adc<'a, Acs: Pin, Ccs: Pin>(
|
||||
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, SPI0, embassy_rp::spi::Blocking>,
|
||||
spi: &mut Spi<'a, I, M>,
|
||||
spi_acs: &mut Output<'a, Acs>,
|
||||
spi_ccs: &mut Output<'a, Ccs>,
|
||||
) -> u16 {
|
||||
|
|
@ -61,6 +93,134 @@ fn read_ext_adc<'a, Acs: Pin, Ccs: Pin>(
|
|||
return temp_value;
|
||||
}
|
||||
|
||||
/// Gets the average stick state over a 1ms interval in a non-blocking fashion.
|
||||
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>,
|
||||
adc_scale: f32,
|
||||
controlstick_params: &StickParams,
|
||||
cstick_params: &StickParams,
|
||||
controller_config: &ControllerConfig,
|
||||
filter_gains: &FilterGains,
|
||||
) {
|
||||
let mut adc_count = 0u32;
|
||||
let mut ax_sum = 0u32;
|
||||
let mut ay_sum = 0u32;
|
||||
let mut cx_sum = 0u32;
|
||||
let mut cy_sum = 0u32;
|
||||
|
||||
// TODO: lower interval possible?
|
||||
let mut timer = Timer::at(Instant::now() + embassy_time::Duration::from_millis(1));
|
||||
|
||||
while embassy_futures::poll_once(&mut timer) != Poll::Ready(()) {
|
||||
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;
|
||||
|
||||
// with this, we can poll the sticks at 1000Hz (ish), while updating
|
||||
// the rest of the controller (the buttons) much faster, to ensure
|
||||
// better input integrity for button inputs.
|
||||
yield_now().await;
|
||||
}
|
||||
|
||||
timer.await;
|
||||
|
||||
let raw_controlstick_x = (ax_sum as f32) / (adc_count as f32) / 4096.0f32 * adc_scale;
|
||||
let raw_controlstick_y = (ay_sum as f32) / (adc_count as f32) / 4096.0f32 * adc_scale;
|
||||
let raw_cstick_x = (cx_sum as f32) / (adc_count as f32) / 4096.0f32 * adc_scale;
|
||||
let raw_cstick_y = (cy_sum as f32) / (adc_count as f32) / 4096.0f32 * adc_scale;
|
||||
|
||||
let x_z = linearize(raw_controlstick_x, &controlstick_params.fit_coeffs_x);
|
||||
let y_z = linearize(raw_controlstick_y, &controlstick_params.fit_coeffs_y);
|
||||
|
||||
let pos_cx = linearize(raw_cstick_x, &cstick_params.fit_coeffs_x);
|
||||
let pos_cy = linearize(raw_cstick_y, &cstick_params.fit_coeffs_y);
|
||||
|
||||
let (x_pos_filt, y_pos_filt) = run_kalman(x_z, y_z, controller_config, filter_gains);
|
||||
|
||||
STICK_SIGNAL.signal(StickState {
|
||||
ax: 127,
|
||||
ay: 127,
|
||||
cx: 127,
|
||||
cy: 127,
|
||||
})
|
||||
}
|
||||
|
||||
fn update_button_states<
|
||||
A: Pin,
|
||||
B: Pin,
|
||||
X: Pin,
|
||||
Y: Pin,
|
||||
Start: Pin,
|
||||
L: Pin,
|
||||
R: Pin,
|
||||
Z: Pin,
|
||||
DLeft: Pin,
|
||||
DRight: Pin,
|
||||
DUp: Pin,
|
||||
DDown: Pin,
|
||||
>(
|
||||
gcc_state: &mut GcReport,
|
||||
btn_a: &Input<'_, A>,
|
||||
btn_b: &Input<'_, B>,
|
||||
btn_x: &Input<'_, X>,
|
||||
btn_y: &Input<'_, Y>,
|
||||
btn_start: &Input<'_, Start>,
|
||||
btn_l: &Input<'_, L>,
|
||||
btn_r: &Input<'_, R>,
|
||||
btn_z: &Input<'_, Z>,
|
||||
btn_dleft: &Input<'_, DLeft>,
|
||||
btn_dright: &Input<'_, DRight>,
|
||||
btn_dup: &Input<'_, DUp>,
|
||||
btn_ddown: &Input<'_, DDown>,
|
||||
) {
|
||||
gcc_state.buttons_1.button_a = btn_a.is_low();
|
||||
gcc_state.buttons_1.button_b = btn_b.is_low();
|
||||
gcc_state.buttons_1.button_x = btn_x.is_low();
|
||||
gcc_state.buttons_1.button_y = btn_y.is_low();
|
||||
gcc_state.buttons_2.button_z = btn_z.is_low();
|
||||
gcc_state.buttons_2.button_start = btn_start.is_low();
|
||||
gcc_state.buttons_2.button_l = btn_l.is_low();
|
||||
gcc_state.buttons_2.button_r = btn_r.is_low();
|
||||
gcc_state.buttons_1.dpad_left = btn_dleft.is_low();
|
||||
gcc_state.buttons_1.dpad_right = btn_dright.is_low();
|
||||
gcc_state.buttons_1.dpad_up = btn_dup.is_low();
|
||||
gcc_state.buttons_1.dpad_down = btn_ddown.is_low();
|
||||
}
|
||||
|
||||
#[embassy_executor::task]
|
||||
pub async fn input_loop(
|
||||
mut flash: Flash<'static, FLASH, Async, FLASH_SIZE>,
|
||||
|
|
@ -76,26 +236,67 @@ pub async fn input_loop(
|
|||
btn_x: Input<'static, PIN_18>,
|
||||
btn_y: Input<'static, PIN_19>,
|
||||
btn_start: Input<'static, PIN_5>,
|
||||
btn_rumble: Input<'static, PIN_25>,
|
||||
btn_brake: Input<'static, PIN_29>,
|
||||
pwm_rumble: Pwm<'static, PWM_CH4>,
|
||||
pwm_brake: Pwm<'static, PWM_CH6>,
|
||||
mut spi: Spi<'static, SPI0, embassy_rp::spi::Blocking>,
|
||||
mut spi_acs: Output<'static, PIN_24>,
|
||||
mut spi_ccs: Output<'static, PIN_23>,
|
||||
) -> ! {
|
||||
) {
|
||||
let mut gcc_state = GcReport::default();
|
||||
|
||||
let mut rng = RoscRng;
|
||||
// Set the stick states to the center
|
||||
gcc_state.stick_x = 127;
|
||||
gcc_state.stick_y = 127;
|
||||
gcc_state.cstick_x = 127;
|
||||
gcc_state.cstick_y = 127;
|
||||
|
||||
let mut uid = [0u8; 1];
|
||||
flash.blocking_read(ADDR_OFFSET, &mut uid).unwrap();
|
||||
|
||||
debug!("Read from flash: {:02X}", uid);
|
||||
|
||||
// TODO: load controller config here
|
||||
|
||||
let stick_state_fut = async {
|
||||
loop {
|
||||
gcc_state.buttons_1.button_a = btn_z.is_low();
|
||||
gcc_state.stick_x = rng.next_u32() as u8;
|
||||
gcc_state.stick_y = rng.next_u32() as u8;
|
||||
// update_stick_states(&mut spi, &mut spi_acs, &mut spi_ccs, 1.0).await;
|
||||
}
|
||||
};
|
||||
|
||||
let input_fut = async {
|
||||
loop {
|
||||
update_button_states(
|
||||
&mut gcc_state,
|
||||
&btn_a,
|
||||
&btn_b,
|
||||
&btn_x,
|
||||
&btn_y,
|
||||
&btn_start,
|
||||
&btn_l,
|
||||
&btn_r,
|
||||
&btn_z,
|
||||
&btn_dleft,
|
||||
&btn_dright,
|
||||
&btn_dup,
|
||||
&btn_ddown,
|
||||
);
|
||||
|
||||
yield_now().await;
|
||||
|
||||
// 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 => (),
|
||||
}
|
||||
|
||||
GCC_SIGNAL.signal(gcc_state);
|
||||
}
|
||||
};
|
||||
|
||||
join(input_fut, stick_state_fut).await;
|
||||
}
|
||||
|
|
|
|||
47
src/main.rs
47
src/main.rs
|
|
@ -6,8 +6,11 @@
|
|||
#![no_main]
|
||||
mod gcc_hid;
|
||||
mod input;
|
||||
mod stick;
|
||||
|
||||
use defmt::{debug, info};
|
||||
use core::ops::Deref;
|
||||
|
||||
use defmt::{debug, info, Format};
|
||||
use embassy_executor::Executor;
|
||||
use embassy_rp::{
|
||||
bind_interrupts,
|
||||
|
|
@ -15,12 +18,14 @@ use embassy_rp::{
|
|||
gpio::{self, Input},
|
||||
multicore::{spawn_core1, Stack},
|
||||
peripherals::USB,
|
||||
pwm::Pwm,
|
||||
spi::{self, Spi},
|
||||
usb::{Driver, InterruptHandler},
|
||||
};
|
||||
use gcc_hid::usb_transfer_loop;
|
||||
use gpio::{Level, Output};
|
||||
use input::input_loop;
|
||||
use packed_struct::PackedStruct;
|
||||
use static_cell::StaticCell;
|
||||
use {defmt_rtt as _, panic_probe as _};
|
||||
|
||||
|
|
@ -31,6 +36,31 @@ static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
|
|||
const FLASH_SIZE: usize = 2 * 1024 * 1024;
|
||||
const ADDR_OFFSET: u32 = 0x100000;
|
||||
|
||||
/// wrapper type because packed_struct doesn't implement float
|
||||
/// packing by default for some reason
|
||||
#[derive(Debug, Format, Clone, Default)]
|
||||
pub struct PackedFloat(f32);
|
||||
|
||||
impl PackedStruct for PackedFloat {
|
||||
type ByteArray = [u8; 4];
|
||||
|
||||
fn pack(&self) -> packed_struct::PackingResult<Self::ByteArray> {
|
||||
Ok(self.to_be_bytes())
|
||||
}
|
||||
|
||||
fn unpack(src: &Self::ByteArray) -> packed_struct::PackingResult<Self> {
|
||||
Ok(Self(f32::from_be_bytes(*src)))
|
||||
}
|
||||
}
|
||||
|
||||
impl Deref for PackedFloat {
|
||||
type Target = f32;
|
||||
|
||||
fn deref(&self) -> &Self::Target {
|
||||
&self.0
|
||||
}
|
||||
}
|
||||
|
||||
bind_interrupts!(struct Irqs {
|
||||
USBCTRL_IRQ => InterruptHandler<USB>;
|
||||
});
|
||||
|
|
@ -79,6 +109,17 @@ fn main() -> ! {
|
|||
let spi_acs = Output::new(p_acs, Level::High); // active low
|
||||
let spi_ccs = Output::new(p_ccs, Level::High); // active low
|
||||
|
||||
let mut pwm_config: embassy_rp::pwm::Config = Default::default();
|
||||
pwm_config.top = 255;
|
||||
pwm_config.enable = true;
|
||||
pwm_config.compare_b = 255;
|
||||
|
||||
let pwm_rumble = Pwm::new_output_b(p.PWM_CH4, p.PIN_25, pwm_config.clone());
|
||||
let pwm_brake = Pwm::new_output_b(p.PWM_CH6, p.PIN_29, pwm_config.clone());
|
||||
|
||||
pwm_rumble.set_counter(255);
|
||||
pwm_brake.set_counter(0);
|
||||
|
||||
executor0.run(|spawner| {
|
||||
spawner
|
||||
.spawn(input_loop(
|
||||
|
|
@ -95,8 +136,8 @@ fn main() -> ! {
|
|||
Input::new(p.PIN_18, gpio::Pull::Up),
|
||||
Input::new(p.PIN_19, gpio::Pull::Up),
|
||||
Input::new(p.PIN_5, gpio::Pull::Up),
|
||||
Input::new(p.PIN_25, gpio::Pull::Up),
|
||||
Input::new(p.PIN_29, gpio::Pull::Up),
|
||||
pwm_rumble,
|
||||
pwm_brake,
|
||||
spi,
|
||||
spi_acs,
|
||||
spi_ccs,
|
||||
|
|
|
|||
263
src/stick.rs
Normal file
263
src/stick.rs
Normal file
|
|
@ -0,0 +1,263 @@
|
|||
// vast majority of this is taken from Phob firmware
|
||||
|
||||
use defmt::Format;
|
||||
use libm::fabs;
|
||||
|
||||
use crate::input::ControllerConfig;
|
||||
|
||||
/// fit order for the linearization
|
||||
const FIT_ORDER: usize = 3;
|
||||
const N_COEFFS: usize = FIT_ORDER + 1;
|
||||
const NO_OF_NOTCHES: usize = 16;
|
||||
const MAX_ORDER: usize = 20;
|
||||
|
||||
#[derive(Clone, Debug, Default, Format)]
|
||||
pub struct StickParams {
|
||||
// these are the linearization coefficients
|
||||
pub fit_coeffs_x: [f32; N_COEFFS],
|
||||
pub fit_coeffs_y: [f32; N_COEFFS],
|
||||
|
||||
// these are the notch remap parameters
|
||||
pub affine_coeffs_x: [[f32; 16]; 4], // affine transformation coefficients for all regions of the stick
|
||||
pub boundary_angles_x: [f32; 4], // angles at the boundaries between regions of the stick (in the plane)
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Default, Format)]
|
||||
pub struct FilterGains {
|
||||
/// What's the max stick distance from the center
|
||||
pub max_stick: f32,
|
||||
/// filtered velocity terms
|
||||
/// how fast the filtered velocity falls off in the absence of stick movement.
|
||||
/// Probably don't touch this.
|
||||
pub x_vel_decay: f32, //0.1 default for 1.2ms timesteps, larger for bigger timesteps
|
||||
pub y_vel_decay: f32,
|
||||
/// how much the current position disagreement impacts the filtered velocity.
|
||||
/// Probably don't touch this.
|
||||
pub x_vel_pos_factor: f32, //0.01 default for 1.2ms timesteps, larger for bigger timesteps
|
||||
pub y_vel_pos_factor: f32,
|
||||
/// how much to ignore filtered velocity when computing the new stick position.
|
||||
/// DO CHANGE THIS
|
||||
/// Higher gives shorter rise times and slower fall times (more pode, less snapback)
|
||||
pub x_vel_damp: f32, //0.125 default for 1.2ms timesteps, smaller for bigger timesteps
|
||||
pub y_vel_damp: f32,
|
||||
/// speed and accel thresholds below which we try to follow the stick better
|
||||
/// These may need tweaking according to how noisy the signal is
|
||||
/// If it's noisier, we may need to add additional filtering
|
||||
/// If the timesteps are *really small* then it may need to be increased to get
|
||||
/// above the noise floor. Or some combination of filtering and playing with
|
||||
/// the thresholds.
|
||||
pub vel_thresh: f32, //1 default for 1.2ms timesteps, larger for bigger timesteps
|
||||
pub accel_thresh: f32, //5 default for 1.2ms timesteps, larger for bigger timesteps
|
||||
/// This just applies a low-pass filter.
|
||||
/// The purpose is to provide delay for single-axis ledgedashes.
|
||||
/// Must be between 0 and 1. Larger = more smoothing and delay.
|
||||
pub x_smoothing: f32,
|
||||
pub y_smoothing: f32,
|
||||
/// Same thing but for C-stick
|
||||
pub c_xsmoothing: f32,
|
||||
pub c_ysmoothing: f32,
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Default)]
|
||||
struct LinearizeCalibrationOutput {
|
||||
pub fit_coeffs_x: [f64; N_COEFFS],
|
||||
pub fit_coeffs_y: [f64; N_COEFFS],
|
||||
|
||||
pub out_x: [f32; NO_OF_NOTCHES],
|
||||
pub out_y: [f32; NO_OF_NOTCHES],
|
||||
}
|
||||
|
||||
pub fn run_kalman(
|
||||
x_z: f32,
|
||||
y_z: f32,
|
||||
controller_config: &ControllerConfig,
|
||||
filter_gains: &FilterGains,
|
||||
) -> (f32, f32) {
|
||||
todo!()
|
||||
}
|
||||
|
||||
fn curve_fit_power(base: f64, exponent: u32) -> f64 {
|
||||
if exponent == 0 {
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
let mut val = base;
|
||||
for _ in 1..exponent {
|
||||
val *= base;
|
||||
}
|
||||
|
||||
val
|
||||
}
|
||||
|
||||
fn sub_col<const N: usize>(
|
||||
matrix: &[[f64; N]; N],
|
||||
t: &[f64; MAX_ORDER],
|
||||
col: usize,
|
||||
n: usize,
|
||||
) -> [[f64; N]; N] {
|
||||
let mut m = *matrix;
|
||||
for i in 0..n {
|
||||
m[i][col] = t[i];
|
||||
}
|
||||
m
|
||||
}
|
||||
|
||||
fn det<const N: usize>(matrix: &[[f64; N]; N]) -> f64 {
|
||||
let sign = trianglize(matrix);
|
||||
if sign == 0 {
|
||||
return 0.;
|
||||
}
|
||||
let mut p = 1f64;
|
||||
for i in 0..N {
|
||||
p *= matrix[i][i];
|
||||
}
|
||||
p * (sign as f64)
|
||||
}
|
||||
|
||||
fn trianglize<const N: usize>(matrix: &[[f64; N]; N]) -> i32 {
|
||||
let mut sign = 1;
|
||||
let mut matrix = *matrix;
|
||||
|
||||
for i in 0..N {
|
||||
let mut max = 0;
|
||||
for row in i..N {
|
||||
if fabs(matrix[row][i]) > fabs(matrix[max][i]) {
|
||||
max = row;
|
||||
}
|
||||
}
|
||||
if max > 0 {
|
||||
sign = -sign;
|
||||
let tmp = matrix[i];
|
||||
matrix[i] = matrix[max];
|
||||
matrix[max] = tmp;
|
||||
}
|
||||
if matrix[i][i] == 0. {
|
||||
return 0;
|
||||
}
|
||||
for row in i + 1..N {
|
||||
let factor = matrix[row][i] / matrix[i][i];
|
||||
if factor == 0. {
|
||||
continue;
|
||||
}
|
||||
for col in i..N {
|
||||
matrix[row][col] -= factor * matrix[i][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
sign
|
||||
}
|
||||
|
||||
fn fit_curve<const N: usize, const NCoeffs: usize>(
|
||||
order: i32,
|
||||
px: &[f64; N],
|
||||
py: &[f64; N],
|
||||
) -> [f64; NCoeffs] {
|
||||
let mut coeffs = [0f64; NCoeffs];
|
||||
|
||||
if NCoeffs != (order + 1) as usize {
|
||||
panic!(
|
||||
"Invalid coefficients length, expected {}, but got {}",
|
||||
order + 1,
|
||||
NCoeffs
|
||||
);
|
||||
}
|
||||
|
||||
if NCoeffs > MAX_ORDER || NCoeffs < 2 {
|
||||
panic!("Matrix size out of bounds");
|
||||
}
|
||||
|
||||
if N < 1 {
|
||||
panic!("Not enough points to fit");
|
||||
}
|
||||
|
||||
let mut t = [0f64; MAX_ORDER];
|
||||
let mut s = [0f64; MAX_ORDER * 2 + 1];
|
||||
|
||||
for i in 0..N {
|
||||
let x = px[i];
|
||||
let y = py[i];
|
||||
for j in 0..NCoeffs * 2 - 1 {
|
||||
s[j] += curve_fit_power(x, j as u32);
|
||||
}
|
||||
for j in 0..NCoeffs {
|
||||
t[j] += y * curve_fit_power(x, j as u32);
|
||||
}
|
||||
}
|
||||
|
||||
//Master matrix LHS of linear equation
|
||||
let mut matrix = [[0f64; NCoeffs]; NCoeffs];
|
||||
|
||||
for i in 0..NCoeffs {
|
||||
for j in 0..NCoeffs {
|
||||
matrix[i][j] = s[i + j];
|
||||
}
|
||||
}
|
||||
|
||||
let denom = det(&matrix);
|
||||
|
||||
for i in 0..NCoeffs {
|
||||
coeffs[NCoeffs - i - 1] = det(&sub_col(&matrix, &t, i, NCoeffs)) / denom;
|
||||
}
|
||||
|
||||
coeffs
|
||||
}
|
||||
|
||||
pub fn linearize(point: f32, coefficients: &[f32; 4]) -> f32 {
|
||||
coefficients[0] * (point * point * point)
|
||||
+ coefficients[1] * (point * point)
|
||||
+ coefficients[2] * point
|
||||
+ coefficients[3]
|
||||
}
|
||||
|
||||
///
|
||||
/// Generate a fit to linearize the stick response.
|
||||
///
|
||||
/// Inputs:
|
||||
/// cleaned points X and Y, (must be 17 points for each of these, the first being the center, the others starting at 3 oclock and going around counterclockwise)
|
||||
///
|
||||
/// Outputs:
|
||||
/// linearization fit coefficients for X and Y
|
||||
pub fn linearize_calibration(in_x: [f32; 17], in_y: [f32; 17]) -> LinearizeCalibrationOutput {
|
||||
let mut fit_points_x = [0f64; 5];
|
||||
let mut fit_points_y = [0f64; 5];
|
||||
|
||||
fit_points_x[0] = in_x[8 + 1] as f64;
|
||||
fit_points_x[1] = (in_x[6 + 1] as f64 + in_x[10 + 1] as f64) / 2.0f64;
|
||||
fit_points_x[2] = in_x[0] as f64;
|
||||
fit_points_x[3] = (in_x[2 + 1] as f64 + in_x[14 + 1] as f64) / 2.0f64;
|
||||
fit_points_x[4] = in_x[0 + 1] as f64;
|
||||
|
||||
fit_points_y[0] = in_y[12 + 1] as f64;
|
||||
fit_points_y[1] = (in_y[10 + 1] as f64 + in_y[14 + 1] as f64) / 2.0f64;
|
||||
fit_points_y[2] = in_y[0] as f64;
|
||||
fit_points_y[3] = (in_y[6 + 1] as f64 + in_y[2 + 1] as f64) / 2.0f64;
|
||||
fit_points_y[4] = in_y[4 + 1] as f64;
|
||||
|
||||
let x_output: [f64; 5] = [27.5, 53.2537879754, 127.5, 201.7462120246, 227.5];
|
||||
let y_output: [f64; 5] = [27.5, 53.2537879754, 127.5, 201.7462120246, 227.5];
|
||||
|
||||
let mut fit_coeffs_x = fit_curve::<5, N_COEFFS>(FIT_ORDER as i32, &fit_points_x, &x_output);
|
||||
let mut fit_coeffs_y = fit_curve::<5, N_COEFFS>(FIT_ORDER as i32, &fit_points_y, &y_output);
|
||||
|
||||
let x_zero_error = linearize(fit_points_x[2] as f32, &fit_coeffs_x.map(|e| e as f32));
|
||||
let y_zero_error = linearize(fit_points_y[2] as f32, &fit_coeffs_y.map(|e| e as f32));
|
||||
|
||||
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];
|
||||
|
||||
for i in 0..=NO_OF_NOTCHES {
|
||||
out_x[i] = linearize(in_x[i], &fit_coeffs_x.map(|e| e as f32));
|
||||
out_y[i] = linearize(in_y[i], &fit_coeffs_y.map(|e| e as f32));
|
||||
}
|
||||
|
||||
LinearizeCalibrationOutput {
|
||||
fit_coeffs_x,
|
||||
fit_coeffs_y,
|
||||
out_x,
|
||||
out_y,
|
||||
}
|
||||
}
|
||||
Loading…
Reference in a new issue