diff --git a/src/config.rs b/src/config.rs new file mode 100644 index 0000000..01a1676 --- /dev/null +++ b/src/config.rs @@ -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 { + let mut controller_config_packed: ::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(()) + } +} diff --git a/src/filter.rs b/src/filter.rs index e83f10a..20ee7b8 100644 --- a/src/filter.rs +++ b/src/filter.rs @@ -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); diff --git a/src/gcc_hid.rs b/src/gcc_hid.rs index 4a89147..cdd3966 100644 --- a/src/gcc_hid.rs +++ b/src/gcc_hid.rs @@ -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,27 +237,23 @@ 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( - &mut serial_buffer, - format_args!( - "{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}", - raw_serial[0], - raw_serial[1], - raw_serial[2], - raw_serial[3], - raw_serial[4], - raw_serial[5], - raw_serial[6], - raw_serial[7] - ), - ) - .unwrap(); + let serial = format_no_std::show( + &mut serial_buffer, + format_args!( + "{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}", + raw_serial[0], + raw_serial[1], + raw_serial[2], + raw_serial[3], + raw_serial[4], + raw_serial[5], + raw_serial[6], + raw_serial[7] + ), + ) + .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); diff --git a/src/input.rs b/src/input.rs index 1f7ed27..de142b9 100644 --- a/src/input.rs +++ b/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 = Signal::new(); @@ -34,113 +30,6 @@ static STICK_SIGNAL: Signal = 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 { - let mut controller_config_packed: ::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 { diff --git a/src/main.rs b/src/main.rs index c98491d..e4501ce 100644 --- a/src/main.rs +++ b/src/main.rs @@ -4,6 +4,7 @@ #![no_std] #![no_main] +mod config; mod filter; mod gcc_hid; mod input; diff --git a/src/stick.rs b/src/stick.rs index fc69d5e..a7d04c9 100644 --- a/src/stick.rs +++ b/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 {