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

chore: refactor and finish stick cal

Browse source
This commit is contained in:
Naxdy 2024-03-27 19:34:14 +01:00
parent 40f734f25d
commit 5ab51f5670
Signed by: Naxdy
GPG key ID: CC15075846BCE91B
6 changed files with 411 additions and 239 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

204
src/config.rs Normal file
View 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(())
}
}

4
src/filter.rs
View file

@ -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);

25
src/gcc_hid.rs
View file

@ -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
View file

@ -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 {

1
src/main.rs
View file

@ -4,6 +4,7 @@
#![no_std]
#![no_main]
mod config;
mod filter;
mod gcc_hid;
mod input;

263
src/stick.rs
View file

@ -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 {