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

chore(project): implement more xy value pairs

Browse source
This commit is contained in:
Naxdy 2024-03-27 21:41:33 +01:00
parent eb0ba44b1c
commit 0f11c659c4
Signed by untrusted user: Naxdy
GPG key ID: CC15075846BCE91B
3 changed files with 140 additions and 125 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/config.rs
View file

@ -18,7 +18,7 @@ use crate::{
/// This needs to be incremented for ANY change to ControllerConfig /// This needs to be incremented for ANY change to ControllerConfig
/// else we risk loading uninitialized memory. /// else we risk loading uninitialized memory.
pub const CONTROLLER_CONFIG_REVISION: u8 = 2; pub const CONTROLLER_CONFIG_REVISION: u8 = 1;
pub const DEFAULT_NOTCH_STATUS: [NotchStatus; NO_OF_NOTCHES] = [ pub const DEFAULT_NOTCH_STATUS: [NotchStatus; NO_OF_NOTCHES] = [
NotchStatus::Cardinal, NotchStatus::Cardinal,

96
src/input.rs
View file

@ -1,4 +1,4 @@
use defmt::info; use defmt::{debug, info};
use embassy_futures::{join::join, yield_now}; use embassy_futures::{join::join, yield_now};
use embassy_rp::{ use embassy_rp::{
flash::{Async, Flash}, flash::{Async, Flash},
@ -18,6 +18,7 @@ use crate::{
config::ControllerConfig, config::ControllerConfig,
filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS}, filter::{run_waveshaping, FilterGains, KalmanState, WaveshapingValues, FILTER_GAINS},
gcc_hid::GcReport, gcc_hid::GcReport,
helpers::XyValuePair,
stick::{linearize, notch_remap, StickParams}, stick::{linearize, notch_remap, StickParams},
FLASH_SIZE, FLASH_SIZE,
}; };
@ -46,18 +47,12 @@ struct StickPositions {
#[derive(Clone, Debug, Default)] #[derive(Clone, Debug, Default)]
struct RawStickValues { struct RawStickValues {
ax_linearized: f32, a_linearized: XyValuePair<f32>,
ay_linearized: f32, c_linearized: XyValuePair<f32>,
cx_linearized: f32, a_raw: XyValuePair<f32>,
cy_linearized: f32, c_raw: XyValuePair<f32>,
ax_raw: f32, a_unfiltered: XyValuePair<f32>,
ay_raw: f32, c_unfiltered: XyValuePair<f32>,
cx_raw: f32,
cy_raw: f32,
ax_unfiltered: f32,
ay_unfiltered: f32,
cx_unfiltered: f32,
cy_unfiltered: f32,
} }
#[derive(PartialEq, Eq)] #[derive(PartialEq, Eq)]
@ -85,7 +80,7 @@ fn read_ext_adc<'a, Acs: Pin, Ccs: Pin, I: embassy_rp::spi::Instance, M: embassy
buf = [0b11110000; 3]; buf = [0b11110000; 3];
} }
if which_stick == Stick::CStick { if which_stick == Stick::ControlStick {
spi_acs.set_low(); spi_acs.set_low();
} else { } else {
spi_ccs.set_low(); spi_ccs.set_low();
@ -135,9 +130,7 @@ async fn update_stick_states<
// TODO: lower interval possible? // TODO: lower interval possible?
let end_time = Instant::now() + embassy_time::Duration::from_millis(1); let end_time = Instant::now() + embassy_time::Duration::from_micros(500);
let timer = Timer::at(end_time);
let mut loop_time = Duration::from_millis(0); let mut loop_time = Duration::from_millis(0);
while Instant::now() < end_time - loop_time { while Instant::now() < end_time - loop_time {
@ -173,36 +166,38 @@ async fn update_stick_states<
&mut spi_ccs, &mut spi_ccs,
) as u32; ) as u32;
loop_time = Instant::now() - loop_start;
// with this, we can poll the sticks at 1000Hz (ish), while updating // with this, we can poll the sticks at 1000Hz (ish), while updating
// the rest of the controller (the buttons) much faster, to ensure // the rest of the controller (the buttons) much faster, to ensure
// better input integrity for button inputs. // better input integrity for button inputs.
yield_now().await; yield_now().await;
loop_time = Instant::now() - loop_start;
} }
timer.await; let raw_controlstick = XyValuePair {
x: (ax_sum as f32) / (adc_count as f32) / 4096.0f32,
y: (ay_sum as f32) / (adc_count as f32) / 4096.0f32,
};
let raw_cstick = XyValuePair {
x: (cx_sum as f32) / (adc_count as f32) / 4096.0f32,
y: (cy_sum as f32) / (adc_count as f32) / 4096.0f32,
};
let raw_controlstick_x = (ax_sum as f32) / (adc_count as f32) / 4096.0f32; debug!("Raw Control Stick: {:?}", raw_controlstick);
let raw_controlstick_y = (ay_sum as f32) / (adc_count as f32) / 4096.0f32; debug!("Raw C Stick: {:?}", raw_cstick);
let raw_cstick_x = (cx_sum as f32) / (adc_count as f32) / 4096.0f32;
let raw_cstick_y = (cy_sum as f32) / (adc_count as f32) / 4096.0f32;
raw_stick_values.ax_raw = raw_controlstick_x; raw_stick_values.a_raw = raw_controlstick;
raw_stick_values.ay_raw = raw_controlstick_y; raw_stick_values.c_raw = raw_cstick;
raw_stick_values.cx_raw = raw_cstick_x;
raw_stick_values.cy_raw = raw_cstick_y;
let x_z = linearize(raw_controlstick_x, &controlstick_params.fit_coeffs_x); 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 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_cx = linearize(raw_cstick.x, &cstick_params.fit_coeffs.x);
let pos_cy = linearize(raw_cstick_y, &cstick_params.fit_coeffs_y); let pos_cy = linearize(raw_cstick.y, &cstick_params.fit_coeffs.y);
raw_stick_values.ax_linearized = x_z; raw_stick_values.a_linearized.x = x_z;
raw_stick_values.ay_linearized = y_z; raw_stick_values.a_linearized.y = y_z;
raw_stick_values.cx_linearized = pos_cx; raw_stick_values.c_linearized.x = pos_cx;
raw_stick_values.cy_linearized = pos_cy; raw_stick_values.c_linearized.y = pos_cy;
let (x_pos_filt, y_pos_filt) = let (x_pos_filt, y_pos_filt) =
kalman_state.run_kalman(x_z, y_z, &controller_config.astick_config, &filter_gains); kalman_state.run_kalman(x_z, y_z, &controller_config.astick_config, &filter_gains);
@ -262,15 +257,15 @@ async fn update_stick_states<
Stick::CStick, Stick::CStick,
); );
let (remapped_x_unfiltered, remapped_y_unfiltered) = notch_remap( let (remapped_x_unfiltered, remapped_y_unfiltered) = notch_remap(
raw_stick_values.ax_linearized, raw_stick_values.a_linearized.x,
raw_stick_values.ay_linearized, raw_stick_values.a_linearized.y,
controlstick_params, controlstick_params,
controller_config, controller_config,
Stick::ControlStick, Stick::ControlStick,
); );
let (remapped_cx_unfiltered, remapped_cy_unfiltered) = notch_remap( let (remapped_cx_unfiltered, remapped_cy_unfiltered) = notch_remap(
raw_stick_values.cx_linearized, raw_stick_values.c_linearized.x,
raw_stick_values.cy_linearized, raw_stick_values.c_linearized.y,
cstick_params, cstick_params,
controller_config, controller_config,
Stick::CStick, Stick::CStick,
@ -280,10 +275,10 @@ async fn update_stick_states<
remapped_y = fminf(125., fmaxf(-125., remapped_y)); remapped_y = fminf(125., fmaxf(-125., remapped_y));
remapped_cx = fminf(125., fmaxf(-125., remapped_cx)); remapped_cx = fminf(125., fmaxf(-125., remapped_cx));
remapped_cy = fminf(125., fmaxf(-125., remapped_cy)); remapped_cy = fminf(125., fmaxf(-125., remapped_cy));
raw_stick_values.ax_unfiltered = fminf(125., fmaxf(-125., remapped_x_unfiltered)); raw_stick_values.a_unfiltered.x = fminf(125., fmaxf(-125., remapped_x_unfiltered));
raw_stick_values.ay_unfiltered = fminf(125., fmaxf(-125., remapped_y_unfiltered)); raw_stick_values.a_unfiltered.y = fminf(125., fmaxf(-125., remapped_y_unfiltered));
raw_stick_values.cx_unfiltered = fminf(125., fmaxf(-125., remapped_cx_unfiltered)); raw_stick_values.c_unfiltered.x = fminf(125., fmaxf(-125., remapped_cx_unfiltered));
raw_stick_values.cy_unfiltered = fminf(125., fmaxf(-125., remapped_cy_unfiltered)); raw_stick_values.c_unfiltered.y = fminf(125., fmaxf(-125., remapped_cy_unfiltered));
let mut out_stick_state = current_stick_state.clone(); let mut out_stick_state = current_stick_state.clone();
@ -405,7 +400,11 @@ pub async fn input_loop(
let mut controlstick_waveshaping_values = WaveshapingValues::default(); let mut controlstick_waveshaping_values = WaveshapingValues::default();
let mut kalman_state = KalmanState::default(); let mut kalman_state = KalmanState::default();
let mut last_loop_time = Instant::now();
loop { loop {
let timer = Timer::after_millis(1);
current_stick_state = update_stick_states( current_stick_state = update_stick_states(
&mut spi, &mut spi,
&mut spi_acs, &mut spi_acs,
@ -423,6 +422,15 @@ pub async fn input_loop(
) )
.await; .await;
timer.await;
debug!(
"Loop took {} us",
(Instant::now() - last_loop_time).as_micros()
);
last_loop_time = Instant::now();
STICK_SIGNAL.signal(current_stick_state.clone()); STICK_SIGNAL.signal(current_stick_state.clone());
} }
}; };

167
src/stick.rs
View file

@ -7,7 +7,7 @@ use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf};
use crate::{ use crate::{
config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS}, config::{ControllerConfig, StickConfig, DEFAULT_NOTCH_STATUS},
helpers::ToRegularArray, helpers::{ToRegularArray, XyValuePair},
input::Stick, input::Stick,
}; };
@ -34,8 +34,7 @@ const NOTCH_ADJUSTMENT_ORDER: [usize; NO_OF_ADJ_NOTCHES] = [2, 6,
#[derive(Clone, Debug, Default, Format)] #[derive(Clone, Debug, Default, Format)]
pub struct StickParams { pub struct StickParams {
// these are the linearization coefficients // these are the linearization coefficients
pub fit_coeffs_x: [f32; NUM_COEFFS], pub fit_coeffs: XyValuePair<[f32; NUM_COEFFS]>,
pub fit_coeffs_y: [f32; NUM_COEFFS],
// these are the notch remap parameters // these are the notch remap parameters
pub affine_coeffs: [[f32; 4]; 16], // affine transformation coefficients for all regions of the stick pub affine_coeffs: [[f32; 4]; 16], // affine transformation coefficients for all regions of the stick
@ -59,8 +58,10 @@ impl StickParams {
); );
Self { Self {
fit_coeffs_x: linearized_cal.fit_coeffs_x.map(|e| e as f32), fit_coeffs: XyValuePair {
fit_coeffs_y: linearized_cal.fit_coeffs_y.map(|e| e as f32), x: linearized_cal.fit_coeffs.x.map(|e| e as f32),
y: linearized_cal.fit_coeffs.y.map(|e| e as f32),
},
affine_coeffs: notch_cal.affine_coeffs, affine_coeffs: notch_cal.affine_coeffs,
boundary_angles: notch_cal.boundary_angles, boundary_angles: notch_cal.boundary_angles,
} }
@ -77,20 +78,22 @@ pub enum NotchStatus {
#[derive(Clone, Debug)] #[derive(Clone, Debug)]
struct CleanedCalibrationPoints { struct CleanedCalibrationPoints {
pub cleaned_points_x: [f32; NO_OF_NOTCHES + 1], pub cleaned_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>,
pub cleaned_points_y: [f32; NO_OF_NOTCHES + 1], pub notch_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>,
pub notch_points_x: [f32; NO_OF_NOTCHES + 1],
pub notch_points_y: [f32; NO_OF_NOTCHES + 1],
pub notch_status: [NotchStatus; NO_OF_NOTCHES], pub notch_status: [NotchStatus; NO_OF_NOTCHES],
} }
impl Default for CleanedCalibrationPoints { impl Default for CleanedCalibrationPoints {
fn default() -> Self { fn default() -> Self {
Self { Self {
cleaned_points_x: [0f32; NO_OF_NOTCHES + 1], cleaned_points: XyValuePair {
cleaned_points_y: [0f32; NO_OF_NOTCHES + 1], x: [0f32; NO_OF_NOTCHES + 1],
notch_points_x: [0f32; NO_OF_NOTCHES + 1], y: [0f32; NO_OF_NOTCHES + 1],
notch_points_y: [0f32; NO_OF_NOTCHES + 1], },
notch_points: XyValuePair {
x: [0f32; NO_OF_NOTCHES + 1],
y: [0f32; NO_OF_NOTCHES + 1],
},
notch_status: DEFAULT_NOTCH_STATUS, notch_status: DEFAULT_NOTCH_STATUS,
} }
} }
@ -113,14 +116,14 @@ impl CleanedCalibrationPoints {
for i in 0..NO_OF_NOTCHES { for i in 0..NO_OF_NOTCHES {
// add the origin values to the first x,y point // add the origin values to the first x,y point
out.cleaned_points_x[0] += cal_points_x[i * 2]; out.cleaned_points.x[0] += cal_points_x[i * 2];
out.cleaned_points_y[0] += cal_points_y[i * 2]; out.cleaned_points.y[0] += cal_points_y[i * 2];
// copy the cal point into the cleaned list // copy the cal point into the cleaned list
out.cleaned_points_x[i + 1] = cal_points_x[i * 2 + 1]; out.cleaned_points.x[i + 1] = cal_points_x[i * 2 + 1];
out.cleaned_points_y[i + 1] = cal_points_y[i * 2 + 1]; out.cleaned_points.y[i + 1] = cal_points_y[i * 2 + 1];
(out.notch_points_x[i + 1], out.notch_points_y[i + 1]) = (out.notch_points.x[i + 1], out.notch_points.y[i + 1]) =
match calc_stick_values(notch_angles[i]) { match calc_stick_values(notch_angles[i]) {
(a, b) => (roundf(a), roundf(b)), (a, b) => (roundf(a), roundf(b)),
}; };
@ -157,22 +160,22 @@ impl CleanedCalibrationPoints {
let largest_y = y_by_size[y_by_size.len() - 1].0; let largest_y = y_by_size[y_by_size.len() - 1].0;
// TODO: make this whole thing a function? it looks very ugly // TODO: make this whole thing a function? it looks very ugly
out.cleaned_points_x[0] -= cal_points_x[smallest_x]; out.cleaned_points.x[0] -= cal_points_x[smallest_x];
out.cleaned_points_x[0] -= cal_points_x[small_x]; out.cleaned_points.x[0] -= cal_points_x[small_x];
out.cleaned_points_x[0] -= cal_points_x[large_x]; out.cleaned_points.x[0] -= cal_points_x[large_x];
out.cleaned_points_x[0] -= cal_points_x[largest_x]; out.cleaned_points.x[0] -= cal_points_x[largest_x];
out.cleaned_points_y[0] -= cal_points_y[smallest_y]; out.cleaned_points.y[0] -= cal_points_y[smallest_y];
out.cleaned_points_y[0] -= cal_points_y[small_y]; out.cleaned_points.y[0] -= cal_points_y[small_y];
out.cleaned_points_y[0] -= cal_points_y[large_y]; out.cleaned_points.y[0] -= cal_points_y[large_y];
out.cleaned_points_y[0] -= cal_points_y[largest_y]; out.cleaned_points.y[0] -= cal_points_y[largest_y];
out.cleaned_points_x[0] /= (NO_OF_NOTCHES - 4) as f32; out.cleaned_points.x[0] /= (NO_OF_NOTCHES - 4) as f32;
out.cleaned_points_y[0] /= (NO_OF_NOTCHES - 4) as f32; out.cleaned_points.y[0] /= (NO_OF_NOTCHES - 4) as f32;
for i in 0..NO_OF_NOTCHES { for i in 0..NO_OF_NOTCHES {
let delta_x = out.cleaned_points_x[i + 1] - out.cleaned_points_x[0]; let delta_x = out.cleaned_points.x[i + 1] - out.cleaned_points.x[0];
let delta_y = out.cleaned_points_y[i + 1] - out.cleaned_points_y[0]; let delta_y = out.cleaned_points.y[i + 1] - out.cleaned_points.y[0];
let mag = sqrtf(delta_x * delta_x + delta_y * delta_y); let mag = sqrtf(delta_x * delta_x + delta_y * delta_y);
// if the cleaned point was at the center and would be a firefox notch // if the cleaned point was at the center and would be a firefox notch
@ -181,15 +184,15 @@ impl CleanedCalibrationPoints {
let prev_index = ((i + NO_OF_NOTCHES - 1) % NO_OF_NOTCHES) + 1; let prev_index = ((i + NO_OF_NOTCHES - 1) % NO_OF_NOTCHES) + 1;
let next_index = ((i + 1) % NO_OF_NOTCHES) + 1; let next_index = ((i + 1) % NO_OF_NOTCHES) + 1;
out.cleaned_points_x[i + 1] = out.cleaned_points.x[i + 1] =
(out.cleaned_points_x[prev_index] + out.cleaned_points_x[next_index]) / 2.0; (out.cleaned_points.x[prev_index] + out.cleaned_points.x[next_index]) / 2.0;
out.cleaned_points_y[i + 1] = out.cleaned_points.y[i + 1] =
(out.cleaned_points_y[prev_index] + out.cleaned_points_y[next_index]) / 2.0; (out.cleaned_points.y[prev_index] + out.cleaned_points.y[next_index]) / 2.0;
out.notch_points_x[i + 1] = out.notch_points.x[i + 1] =
(out.notch_points_x[prev_index] + out.notch_points_x[next_index]) / 2.0; (out.notch_points.x[prev_index] + out.notch_points.x[next_index]) / 2.0;
out.notch_points_y[i + 1] = out.notch_points.y[i + 1] =
(out.notch_points_y[prev_index] + out.notch_points_y[next_index]) / 2.0; (out.notch_points.y[prev_index] + out.notch_points.y[next_index]) / 2.0;
debug!("Skipping notch {}", i + 1); debug!("Skipping notch {}", i + 1);
@ -204,10 +207,10 @@ impl CleanedCalibrationPoints {
for i in 0..=NO_OF_NOTCHES { for i in 0..=NO_OF_NOTCHES {
debug!( debug!(
"Cleaned: ({}, {}), Notch: ({}, {})", "Cleaned: ({}, {}), Notch: ({}, {})",
out.cleaned_points_x[i], out.cleaned_points.x[i],
out.cleaned_points_y[i], out.cleaned_points.y[i],
out.notch_points_x[i], out.notch_points.x[i],
out.notch_points_y[i], out.notch_points.y[i],
); );
} }
@ -219,11 +222,9 @@ impl CleanedCalibrationPoints {
#[derive(Clone, Debug, Default)] #[derive(Clone, Debug, Default)]
struct LinearizedCalibration { struct LinearizedCalibration {
pub fit_coeffs_x: [f64; NUM_COEFFS], pub fit_coeffs: XyValuePair<[f64; NUM_COEFFS]>,
pub fit_coeffs_y: [f64; NUM_COEFFS],
pub linearized_points_x: [f32; NO_OF_NOTCHES + 1], pub linearized_points: XyValuePair<[f32; NO_OF_NOTCHES + 1]>,
pub linearized_points_y: [f32; NO_OF_NOTCHES + 1],
} }
impl LinearizedCalibration { impl LinearizedCalibration {
@ -240,10 +241,12 @@ impl LinearizedCalibration {
let mut fit_points_y = [0f64; 5]; let mut fit_points_y = [0f64; 5];
let in_x = cleaned_calibration_points let in_x = cleaned_calibration_points
.cleaned_points_x .cleaned_points
.x
.map(|e| e as f64); .map(|e| e as f64);
let in_y = cleaned_calibration_points let in_y = cleaned_calibration_points
.cleaned_points_y .cleaned_points
.y
.map(|e| e as f64); .map(|e| e as f64);
fit_points_x[0] = in_x[8 + 1]; fit_points_x[0] = in_x[8 + 1];
@ -281,10 +284,14 @@ impl LinearizedCalibration {
} }
Self { Self {
fit_coeffs_x, fit_coeffs: XyValuePair {
fit_coeffs_y, x: fit_coeffs_x,
linearized_points_x, y: fit_coeffs_y,
linearized_points_y, },
linearized_points: XyValuePair {
x: linearized_points_x,
y: linearized_points_y,
},
} }
} }
} }
@ -307,40 +314,40 @@ impl NotchCalibration {
let mut points_out = [[0f32; 3]; 3]; let mut points_out = [[0f32; 3]; 3];
if i == NO_OF_NOTCHES { if i == NO_OF_NOTCHES {
points_in[0][0] = linearized_calibration.linearized_points_x[0]; points_in[0][0] = linearized_calibration.linearized_points.x[0];
points_in[0][1] = linearized_calibration.linearized_points_x[i]; points_in[0][1] = linearized_calibration.linearized_points.x[i];
points_in[0][2] = linearized_calibration.linearized_points_x[1]; points_in[0][2] = linearized_calibration.linearized_points.x[1];
points_in[1][0] = linearized_calibration.linearized_points_y[0]; points_in[1][0] = linearized_calibration.linearized_points.y[0];
points_in[1][1] = linearized_calibration.linearized_points_y[i]; points_in[1][1] = linearized_calibration.linearized_points.y[i];
points_in[1][2] = linearized_calibration.linearized_points_y[1]; points_in[1][2] = linearized_calibration.linearized_points.y[1];
points_in[2][0] = 1.; points_in[2][0] = 1.;
points_in[2][1] = 1.; points_in[2][1] = 1.;
points_in[2][2] = 1.; points_in[2][2] = 1.;
points_out[0][0] = cleaned_calibration_points.notch_points_x[0]; 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][1] = cleaned_calibration_points.notch_points.x[i];
points_out[0][2] = cleaned_calibration_points.notch_points_x[1]; 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][0] = cleaned_calibration_points.notch_points.y[0];
points_out[1][1] = cleaned_calibration_points.notch_points_y[i]; points_out[1][1] = cleaned_calibration_points.notch_points.y[i];
points_out[1][2] = cleaned_calibration_points.notch_points_y[1]; points_out[1][2] = cleaned_calibration_points.notch_points.y[1];
points_out[2][0] = 1.; points_out[2][0] = 1.;
points_out[2][1] = 1.; points_out[2][1] = 1.;
points_out[2][2] = 1.; points_out[2][2] = 1.;
} else { } else {
points_in[0][0] = linearized_calibration.linearized_points_x[0]; points_in[0][0] = linearized_calibration.linearized_points.x[0];
points_in[0][1] = linearized_calibration.linearized_points_x[i]; points_in[0][1] = linearized_calibration.linearized_points.x[i];
points_in[0][2] = linearized_calibration.linearized_points_x[i + 1]; points_in[0][2] = linearized_calibration.linearized_points.x[i + 1];
points_in[1][0] = linearized_calibration.linearized_points_y[0]; points_in[1][0] = linearized_calibration.linearized_points.y[0];
points_in[1][1] = linearized_calibration.linearized_points_y[i]; points_in[1][1] = linearized_calibration.linearized_points.y[i];
points_in[1][2] = linearized_calibration.linearized_points_y[i + 1]; points_in[1][2] = linearized_calibration.linearized_points.y[i + 1];
points_in[2][0] = 1.; points_in[2][0] = 1.;
points_in[2][1] = 1.; points_in[2][1] = 1.;
points_in[2][2] = 1.; points_in[2][2] = 1.;
points_out[0][0] = cleaned_calibration_points.notch_points_x[0]; 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][1] = cleaned_calibration_points.notch_points.x[i];
points_out[0][2] = cleaned_calibration_points.notch_points_x[i + 1]; 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][0] = cleaned_calibration_points.notch_points.y[0];
points_out[1][1] = cleaned_calibration_points.notch_points_y[i]; 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[1][2] = cleaned_calibration_points.notch_points.y[i + 1];
points_out[2][0] = 1.; points_out[2][0] = 1.;
points_out[2][1] = 1.; points_out[2][1] = 1.;
points_out[2][2] = 1.; points_out[2][2] = 1.;
@ -366,10 +373,10 @@ impl NotchCalibration {
); );
out.boundary_angles[i - 1] = match atan2f( out.boundary_angles[i - 1] = match atan2f(
linearized_calibration.linearized_points_y[i] linearized_calibration.linearized_points.y[i]
- linearized_calibration.linearized_points_y[0], - linearized_calibration.linearized_points.y[0],
linearized_calibration.linearized_points_x[i] linearized_calibration.linearized_points.x[i]
- linearized_calibration.linearized_points_x[0], - linearized_calibration.linearized_points.x[0],
) { ) {
a if a < out.boundary_angles[0] => a + 2. * PI, a if a < out.boundary_angles[0] => a + 2. * PI,
a => a, a => a,