/**
* Storage for controller configuration, including helper functions & types, as well as sane defaults.
* Also includes necessary logic for configuring the controller & calibrating the sticks.
*/
use core::{cmp::min, f32::consts::PI};
use defmt::{debug, error, info, warn, Format};
use embassy_futures::yield_now;
use embassy_rp::{
flash::{Async, Flash, ERASE_SIZE},
peripherals::FLASH,
};
use packed_struct::{derive::PackedStruct, PackedStruct};
use crate::{
helpers::{PackedFloat, ToPackedFloatArray, ToRegularArray, XyValuePair},
input::{
read_ext_adc, Stick, StickAxis, StickState, FLOAT_ORIGIN, SPI_ACS_SHARED, SPI_CCS_SHARED,
SPI_SHARED,
},
stick::{
calc_stick_values, legalize_notches, AppliedCalibration, CleanedCalibrationPoints,
LinearizedCalibration, NotchCalibration, NotchStatus, CALIBRATION_ORDER,
NOTCH_ADJUSTMENT_ORDER, NO_OF_ADJ_NOTCHES, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES,
},
ADDR_OFFSET, FLASH_SIZE,
};
use embassy_sync::{
blocking_mutex::raw::{CriticalSectionRawMutex, RawMutex, ThreadModeRawMutex},
pubsub::{PubSubBehavior, Subscriber},
signal::Signal,
};
use embassy_time::{Duration, Ticker, Timer};
use crate::{gcc_hid::GcReport, input::CHANNEL_GCC_STATE};
/// Whether we are currently calibrating the sticks. Updates are dispatched when the status changes.
/// Initial status is assumed to be false.
pub static SIGNAL_IS_CALIBRATING: Signal<ThreadModeRawMutex, bool> = Signal::new();
/// Config change signalled to the stick task.
pub static SIGNAL_CONFIG_CHANGE: Signal<ThreadModeRawMutex, ControllerConfig> = Signal::new();
/// Signal used to override the stick state in order to display desired stick positions during calibration.
pub static SIGNAL_OVERRIDE_STICK_STATE: Signal<
CriticalSectionRawMutex,
Option<OverrideStickState>,
> = Signal::new();
/// Dispatched when we want to override the GCC state for a short amount of time.
pub static SIGNAL_OVERRIDE_GCC_STATE: Signal<CriticalSectionRawMutex, OverrideGcReportInstruction> =
Signal::new();
/// Struct used for overriding the GCC state for a given amount of
/// time, useful for providing feedback to the user e.g. if we just entered
/// a certain mode.
#[derive(Default, Debug, Clone, Format)]
pub struct OverrideGcReportInstruction {
pub report: GcReport,
pub duration_ms: u64,
}
const CONFIG_MODE_ENTRY_COMBO: [AwaitableButtons; 4] = [
AwaitableButtons::Start,
AwaitableButtons::A,
AwaitableButtons::X,
AwaitableButtons::Y,
];
const LSTICK_CALIBRATION_COMBO: [AwaitableButtons; 2] = [AwaitableButtons::A, AwaitableButtons::X];
const RSTICK_CALIBRATION_COMBO: [AwaitableButtons; 2] = [AwaitableButtons::A, AwaitableButtons::Y];
/// This doesn't need to be super fast, since it's only used
/// in config mode.
const BUTTON_POLL_INTERVAL_MILLIS: u64 = 20;
/// This needs to be incremented for ANY change to ControllerConfig
/// else we risk loading uninitialized memory.
pub const CONTROLLER_CONFIG_REVISION: u8 = 1;
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
];
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.,
];
#[derive(Clone, Format)]
pub struct OverrideStickState {
pub x: u8,
pub y: u8,
pub which_stick: Stick,
}
#[derive(Clone, Copy, Debug, Format)]
enum AwaitableButtons {
A,
B,
X,
Y,
Up,
Down,
Left,
Right,
Start,
L,
R,
}
#[derive(Clone, Copy, Debug, Format)]
enum NotchAdjustmentType {
Clockwise,
CounterClockwise,
Reset,
None,
}
#[derive(Debug, Clone, Format, PackedStruct)]
#[packed_struct(endian = "msb")]
pub struct StickConfig {
#[packed_field(size_bits = "8")]
pub x_waveshaping: u8,
#[packed_field(size_bits = "8")]
pub y_waveshaping: u8,
#[packed_field(size_bits = "8")]
pub analog_scaler: u8,
#[packed_field(size_bits = "8")]
pub x_snapback: i8, // not used for CStick
#[packed_field(size_bits = "8")]
pub y_snapback: i8, // not used for CStick
#[packed_field(size_bits = "8")]
pub cardinal_snapping: i8, // not used for CStick
#[packed_field(size_bits = "8")]
pub x_smoothing: u8,
#[packed_field(size_bits = "8")]
pub y_smoothing: u8,
#[packed_field(element_size_bytes = "4")]
pub cal_points_x: [PackedFloat; 32],
#[packed_field(element_size_bytes = "4")]
pub cal_points_y: [PackedFloat; 32],
#[packed_field(element_size_bytes = "4")]
pub angles: [PackedFloat; 16],
}
impl Default for StickConfig {
fn default() -> Self {
Self {
x_waveshaping: 0,
y_waveshaping: 0,
x_snapback: 4,
y_snapback: 4,
x_smoothing: 0,
y_smoothing: 0,
cardinal_snapping: 6,
analog_scaler: 100,
cal_points_x: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
cal_points_y: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
angles: *DEFAULT_ANGLES.to_packed_float_array(),
}
}
}
#[derive(Debug, Clone, Format, PackedStruct)]
#[packed_struct(endian = "msb")]
pub struct ControllerConfig {
#[packed_field(size_bits = "8")]
pub config_revision: u8,
/// Toggle for input consistency mode. If true, the controller
/// will trick the Switch into updating the state every 8.33ms
/// instead of every 8ms. The tradeoff is a slight increase in
/// input lag.
#[packed_field(size_bits = "8")]
pub input_consistency_mode: bool,
#[packed_field(size_bytes = "328")]
pub astick_config: StickConfig,
#[packed_field(size_bytes = "328")]
pub cstick_config: StickConfig,
}
impl Default for ControllerConfig {
fn default() -> Self {
Self {
config_revision: CONTROLLER_CONFIG_REVISION,
input_consistency_mode: true,
astick_config: StickConfig::default(),
cstick_config: StickConfig::default(),
}
}
}
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; 658]; // 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(())
}
}
trait WaitForButtonPress {
/// Wait for a single button press.
async fn wait_for_button_press(&mut self, button_to_wait_for: &AwaitableButtons);
/// Wait for a single button release.
async fn wait_for_button_release(&mut self, button_to_wait_for: &AwaitableButtons);
/// Wait for multiple buttons to be pressed simultaneously.
async fn wait_for_simultaneous_button_presses<const N: usize>(
&mut self,
buttons_to_wait_for: &[AwaitableButtons; N],
);
/// Wait for a single button press of specified buttons, and return the button that was pressed.
async fn wait_and_filter_button_press<const N: usize>(
&mut self,
buttons_to_wait_for: &[AwaitableButtons; N],
) -> AwaitableButtons;
/// See if one of the buttons in buttons_to_look_out_for is pressed, and return the pressed button, otherwise None.
fn filter_button_press_if_present<const N: usize>(
&mut self,
buttons_to_look_out_for: &[AwaitableButtons; N],
) -> Option<AwaitableButtons>;
/// Wait for multiple possible button combinations to be pressed simultaneously, and return the index of the combination that was pressed.
async fn wait_and_filter_simultaneous_button_presses<const N: usize, const M: usize>(
&mut self,
buttons_to_wait_for: &[[AwaitableButtons; N]; M],
) -> usize;
}
impl<'a, T: RawMutex, const I: usize, const J: usize, const K: usize> WaitForButtonPress
for Subscriber<'a, T, GcReport, I, J, K>
{
async fn wait_for_button_press(&mut self, button_to_wait_for: &AwaitableButtons) {
loop {
let report = self.next_message_pure().await;
if is_awaitable_button_pressed(&report, button_to_wait_for) {
break;
}
Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
}
}
async fn wait_for_button_release(&mut self, button_to_wait_for: &AwaitableButtons) {
loop {
let report = self.next_message_pure().await;
if !is_awaitable_button_pressed(&report, button_to_wait_for) {
break;
}
Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
}
}
async fn wait_for_simultaneous_button_presses<const N: usize>(
&mut self,
buttons_to_wait_for: &[AwaitableButtons; N],
) {
loop {
let report = self.next_message_pure().await;
if buttons_to_wait_for
.iter()
.all(|button| is_awaitable_button_pressed(&report, button))
{
break;
}
Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
}
}
async fn wait_and_filter_button_press<const N: usize>(
&mut self,
buttons_to_wait_for: &[AwaitableButtons; N],
) -> AwaitableButtons {
loop {
let report = self.next_message_pure().await;
for button in buttons_to_wait_for {
if is_awaitable_button_pressed(&report, button) {
return *button;
}
}
Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
}
}
async fn wait_and_filter_simultaneous_button_presses<const N: usize, const M: usize>(
&mut self,
buttons_to_wait_for: &[[AwaitableButtons; N]; M],
) -> usize {
loop {
let report = self.next_message_pure().await;
for (i, buttons) in buttons_to_wait_for.iter().enumerate() {
if buttons
.iter()
.all(|button| is_awaitable_button_pressed(&report, button))
{
return i;
}
}
Timer::after_millis(BUTTON_POLL_INTERVAL_MILLIS).await;
}
}
fn filter_button_press_if_present<const N: usize>(
&mut self,
buttons_to_look_out_for: &[AwaitableButtons; N],
) -> Option<AwaitableButtons> {
let report = self.try_next_message_pure();
if let Some(report) = report {
for button in buttons_to_look_out_for {
if is_awaitable_button_pressed(&report, button) {
return Some(*button);
}
}
}
return None;
}
}
fn is_awaitable_button_pressed(report: &GcReport, button_to_wait_for: &AwaitableButtons) -> bool {
match button_to_wait_for {
AwaitableButtons::A => report.buttons_1.button_a,
AwaitableButtons::B => report.buttons_1.button_b,
AwaitableButtons::X => report.buttons_1.button_x,
AwaitableButtons::Y => report.buttons_1.button_y,
AwaitableButtons::Up => report.buttons_1.dpad_up,
AwaitableButtons::Down => report.buttons_1.dpad_down,
AwaitableButtons::Left => report.buttons_1.dpad_left,
AwaitableButtons::Right => report.buttons_1.dpad_right,
AwaitableButtons::Start => report.buttons_2.button_start,
AwaitableButtons::L => report.buttons_2.button_l || report.trigger_l > 10,
AwaitableButtons::R => report.buttons_2.button_r || report.trigger_r > 10,
}
}
#[derive(Debug, Format)]
struct StickCalibrationProcess<'a> {
which_stick: Stick,
calibration_step: u8,
gcc_config: &'a mut ControllerConfig,
cal_points: [XyValuePair<f32>; NO_OF_CALIBRATION_POINTS],
applied_calibration: AppliedCalibration,
}
impl<'a> StickCalibrationProcess<'a> {
pub fn new(gcc_config: &'a mut ControllerConfig, which_stick: Stick) -> Self {
Self {
which_stick,
calibration_step: 0,
gcc_config,
cal_points: [XyValuePair::default(); NO_OF_CALIBRATION_POINTS],
applied_calibration: AppliedCalibration::default(),
}
}
fn adjust_notch(&mut self, notch_adjustment_type: NotchAdjustmentType) {
let stick_config = match self.which_stick {
Stick::ControlStick => &mut self.gcc_config.astick_config,
Stick::CStick => &mut self.gcc_config.cstick_config,
};
let notch_idx =
NOTCH_ADJUSTMENT_ORDER[self.calibration_step as usize - NO_OF_CALIBRATION_POINTS];
if self.applied_calibration.cleaned_calibration.notch_status[notch_idx]
== NotchStatus::TertInactive
{}
// assumes a tick rate of 1ms
match notch_adjustment_type {
NotchAdjustmentType::Clockwise => {
stick_config.angles[notch_idx] -= 0.0075;
}
NotchAdjustmentType::CounterClockwise => {
stick_config.angles[notch_idx] += 0.0075;
}
NotchAdjustmentType::Reset => {
stick_config.angles[notch_idx] =
PackedFloat(self.applied_calibration.measured_notch_angles[notch_idx]);
}
NotchAdjustmentType::None => {}
}
match notch_adjustment_type {
NotchAdjustmentType::Clockwise
| NotchAdjustmentType::CounterClockwise
| NotchAdjustmentType::Reset => {
let cleaned_calibration_points =
CleanedCalibrationPoints::from_temp_calibration_points(
&self.cal_points.map(|e| e.x),
&self.cal_points.map(|e| e.y),
&self.applied_calibration.measured_notch_angles,
);
let linearized_calibration =
LinearizedCalibration::from_calibration_points(&cleaned_calibration_points);
self.applied_calibration.stick_params.fit_coeffs = XyValuePair {
x: linearized_calibration.fit_coeffs.x.map(|e| e as f32),
y: linearized_calibration.fit_coeffs.y.map(|e| e as f32),
};
let notch_calibration = NotchCalibration::from_cleaned_and_linearized_calibration(
&cleaned_calibration_points,
&linearized_calibration,
);
self.applied_calibration.stick_params.affine_coeffs =
notch_calibration.affine_coeffs;
self.applied_calibration.stick_params.boundary_angles =
notch_calibration.boundary_angles;
stick_config.angles = *legalize_notches(
self.calibration_step as usize,
&self.applied_calibration.measured_notch_angles,
&stick_config.angles.to_regular_array(),
)
.to_packed_float_array();
SIGNAL_CONFIG_CHANGE.signal(self.gcc_config.clone());
}
NotchAdjustmentType::None => {}
}
}
async fn calibration_advance(&mut self) -> bool {
info!(
"Running calibration advance on stick {} at step {}",
self.which_stick, self.calibration_step
);
let stick_config = match self.which_stick {
Stick::ControlStick => &mut self.gcc_config.astick_config,
Stick::CStick => &mut self.gcc_config.cstick_config,
};
let mut spi_unlocked = SPI_SHARED.lock().await;
let mut spi_acs_unlocked = SPI_ACS_SHARED.lock().await;
let mut spi_ccs_unlocked = SPI_CCS_SHARED.lock().await;
let spi = spi_unlocked.as_mut().unwrap();
let spi_acs = spi_acs_unlocked.as_mut().unwrap();
let spi_ccs = spi_ccs_unlocked.as_mut().unwrap();
if self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 {
let mut x: f32 = 0.;
let mut y: f32 = 0.;
for _ in 0..128 {
x += read_ext_adc(self.which_stick, StickAxis::XAxis, spi, spi_acs, spi_ccs) as f32
/ 4096.0;
y += read_ext_adc(self.which_stick, StickAxis::YAxis, spi, spi_acs, spi_ccs) as f32
/ 4096.0;
}
x /= 128.;
y /= 128.;
let idx = CALIBRATION_ORDER[self.calibration_step as usize];
self.cal_points[idx] = XyValuePair { x, y };
}
self.calibration_step += 1;
// TODO: phob does something related to undo here
if self.calibration_step == NO_OF_CALIBRATION_POINTS as u8 {
stick_config.angles = *legalize_notches(
self.calibration_step as usize,
&self.applied_calibration.measured_notch_angles,
&self.applied_calibration.notch_angles,
)
.to_packed_float_array();
self.applied_calibration = AppliedCalibration::from_points(
&self.cal_points.map(|e| e.x),
&self.cal_points.map(|e| e.y),
&stick_config,
self.which_stick,
);
}
if self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8 {
let mut notch_idx = NOTCH_ADJUSTMENT_ORDER[min(
self.calibration_step - NO_OF_CALIBRATION_POINTS as u8,
NO_OF_ADJ_NOTCHES as u8 - 1,
) as usize];
while self.applied_calibration.cleaned_calibration.notch_status[notch_idx]
== NotchStatus::TertInactive
&& self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8
{
stick_config.angles = *legalize_notches(
self.calibration_step as usize,
&self.applied_calibration.measured_notch_angles,
&stick_config.angles.to_regular_array(),
)
.to_packed_float_array();
self.calibration_step += 1;
notch_idx = NOTCH_ADJUSTMENT_ORDER[min(
self.calibration_step - NO_OF_CALIBRATION_POINTS as u8,
NO_OF_ADJ_NOTCHES as u8 - 1,
) as usize];
}
}
if self.calibration_step >= NO_OF_CALIBRATION_POINTS as u8 + NO_OF_ADJ_NOTCHES as u8 {
stick_config.cal_points_x = self.cal_points.map(|p| p.x.into());
stick_config.cal_points_y = self.cal_points.map(|p| p.y.into());
SIGNAL_CONFIG_CHANGE.signal(self.gcc_config.clone());
info!("Finished calibrating stick {}", self.which_stick);
return true;
}
return false;
}
pub async fn calibrate_stick(&mut self) {
info!("Beginning stick calibration for {}", self.which_stick);
let mut gcc_subscriber = CHANNEL_GCC_STATE.subscriber().unwrap();
SIGNAL_IS_CALIBRATING.signal(true);
while {
if self.calibration_step < NO_OF_CALIBRATION_POINTS as u8 {
// Calibration phase
let (x, y) = get_stick_display_coords(self.calibration_step as usize);
debug!(
"Raw display coords for step {}: {}, {}",
self.calibration_step, x, y
);
SIGNAL_OVERRIDE_STICK_STATE.signal(Some(OverrideStickState {
x: x as u8,
y: y as u8,
which_stick: match self.which_stick {
Stick::ControlStick => Stick::CStick,
Stick::CStick => Stick::ControlStick,
},
}));
gcc_subscriber
.wait_for_button_release(&AwaitableButtons::A)
.await;
// Prevent accidental double presses
Timer::after_millis(100).await;
gcc_subscriber
.wait_for_button_press(&AwaitableButtons::A)
.await;
} else {
// Notch adjustment phase
gcc_subscriber
.wait_for_button_release(&AwaitableButtons::A)
.await;
Timer::after_millis(100).await;
let mut ticker = Ticker::every(Duration::from_millis(20));
let notch_idx = NOTCH_ADJUSTMENT_ORDER
[self.calibration_step as usize - NO_OF_CALIBRATION_POINTS];
let (init_x, init_y) =
calc_stick_values(self.applied_calibration.measured_notch_angles[notch_idx]);
SIGNAL_OVERRIDE_STICK_STATE.signal(Some(OverrideStickState {
x: (init_x + FLOAT_ORIGIN) as u8,
y: (init_y + FLOAT_ORIGIN) as u8,
which_stick: match self.which_stick {
Stick::ControlStick => Stick::CStick,
Stick::CStick => Stick::ControlStick,
},
}));
'adjust: loop {
let btn_result = gcc_subscriber.filter_button_press_if_present(&[
AwaitableButtons::A,
AwaitableButtons::B,
AwaitableButtons::X,
AwaitableButtons::Y,
]);
match btn_result {
Some(btn) => match btn {
AwaitableButtons::A => {
debug!("Btn A release pressed");
break 'adjust;
}
AwaitableButtons::B => self.adjust_notch(NotchAdjustmentType::Reset),
AwaitableButtons::X => {
self.adjust_notch(NotchAdjustmentType::Clockwise)
}
AwaitableButtons::Y => {
self.adjust_notch(NotchAdjustmentType::CounterClockwise)
}
_ => self.adjust_notch(NotchAdjustmentType::None),
},
None => self.adjust_notch(NotchAdjustmentType::None),
};
ticker.next().await;
yield_now().await;
}
};
!self.calibration_advance().await
} {}
SIGNAL_IS_CALIBRATING.signal(false);
SIGNAL_OVERRIDE_STICK_STATE.signal(None);
}
}
fn get_stick_display_coords(current_step: usize) -> (f32, f32) {
let idx = CALIBRATION_ORDER[current_step];
if idx % 2 != 0 {
let notch_idx = idx / 2;
match calc_stick_values(DEFAULT_ANGLES[notch_idx]) {
(x, y) => (x + FLOAT_ORIGIN, y + FLOAT_ORIGIN),
}
} else {
(127.5, 127.5)
}
}
async fn configuration_main_loop<
'a,
M: RawMutex,
const C: usize,
const S: usize,
const P: usize,
>(
current_config: &ControllerConfig,
mut flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
gcc_subscriber: &mut Subscriber<'a, M, GcReport, C, S, P>,
) {
let mut final_config = current_config.clone();
let config_options = [LSTICK_CALIBRATION_COMBO, RSTICK_CALIBRATION_COMBO];
'main: loop {
match gcc_subscriber
.wait_and_filter_simultaneous_button_presses(&config_options)
.await
{
selection => match selection {
0 => {
StickCalibrationProcess::new(&mut final_config, Stick::ControlStick)
.calibrate_stick()
.await;
}
1 => {
StickCalibrationProcess::new(&mut final_config, Stick::CStick)
.calibrate_stick()
.await;
}
s => {
error!("Invalid selection in config loop: {}", s);
continue;
}
},
};
final_config.write_to_flash(&mut flash).unwrap();
break 'main;
}
info!("Exiting config main loop.");
}
#[embassy_executor::task]
pub async fn config_task(
current_config: ControllerConfig,
mut flash: Flash<'static, FLASH, Async, FLASH_SIZE>,
) {
let mut gcc_subscriber = CHANNEL_GCC_STATE.subscriber().unwrap();
info!("Config task is running.");
loop {
gcc_subscriber
.wait_for_simultaneous_button_presses(&CONFIG_MODE_ENTRY_COMBO)
.await;
info!("Entering config mode.");
SIGNAL_OVERRIDE_GCC_STATE.signal(OverrideGcReportInstruction {
report: match GcReport::default() {
mut a => {
a.trigger_r = 255;
a.trigger_l = 255;
a.buttons_2.button_l = true;
a.buttons_2.button_r = true;
a.buttons_1.button_x = true;
a.buttons_1.button_y = true;
a.buttons_1.button_a = true;
a.stick_x = 127;
a.stick_y = 127;
a.cstick_x = 127;
a.cstick_y = 127;
a
}
},
duration_ms: 1000,
});
// Wait for the user to release the buttons
Timer::after_millis(1500).await;
configuration_main_loop(¤t_config, &mut flash, &mut gcc_subscriber).await;
info!("Exiting config mode.");
}
}