From 35349dee49b35e435760248b0615392f84ebaadd Mon Sep 17 00:00:00 2001 From: Naxdy Date: Thu, 28 Mar 2024 22:43:44 +0100 Subject: [PATCH] shuffle some threads around, stricter timing for stick readings --- src/calibrate.rs | 1 + src/config.rs | 2 +- src/gcc_hid.rs | 9 +- src/input.rs | 208 +++++++++++++++++++++++++---------------------- src/main.rs | 68 ++++++++++------ src/stick.rs | 2 + 6 files changed, 160 insertions(+), 130 deletions(-) create mode 100644 src/calibrate.rs diff --git a/src/calibrate.rs b/src/calibrate.rs new file mode 100644 index 0000000..1068669 --- /dev/null +++ b/src/calibrate.rs @@ -0,0 +1 @@ +pub async fn calibration_loop() {} diff --git a/src/config.rs b/src/config.rs index 3a42c36..da95f57 100644 --- a/src/config.rs +++ b/src/config.rs @@ -18,7 +18,7 @@ use crate::{ /// 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 CONTROLLER_CONFIG_REVISION: u8 = 2; pub const DEFAULT_NOTCH_STATUS: [NotchStatus; NO_OF_NOTCHES] = [ NotchStatus::Cardinal, diff --git a/src/gcc_hid.rs b/src/gcc_hid.rs index 2718915..c41a114 100644 --- a/src/gcc_hid.rs +++ b/src/gcc_hid.rs @@ -16,7 +16,7 @@ use embassy_usb::{ }; use packed_struct::{derive::PackedStruct, PackedStruct}; -use crate::input::GCC_SIGNAL; +use crate::input::CHANNEL_GCC_STATE; #[rustfmt::skip] pub const GCC_REPORT_DESCRIPTOR: &[u8] = &[ @@ -234,7 +234,7 @@ impl Handler for MyDeviceHandler { } #[embassy_executor::task] -pub async fn usb_transfer_loop(driver: Driver<'static, USB>, raw_serial: [u8; 8]) { +pub async fn usb_transfer_task(driver: Driver<'static, USB>, raw_serial: [u8; 8]) { let mut serial_buffer = [0u8; 64]; let serial = format_no_std::show( @@ -312,13 +312,14 @@ pub async fn usb_transfer_loop(driver: Driver<'static, USB>, raw_serial: [u8; 8] }; let (mut reader, mut writer) = hid.split(); - debug!("In here"); let mut lasttime = Instant::now(); let in_fut = async { + let mut gcc_subscriber = CHANNEL_GCC_STATE.subscriber().unwrap(); + loop { - let state = GCC_SIGNAL.wait().await; + let state = gcc_subscriber.next_message_pure().await; let report = get_gcinput_hid_report(&state); match writer.write(&report).await { Ok(()) => { diff --git a/src/input.rs b/src/input.rs index 1ed7f2e..a78004e 100644 --- a/src/input.rs +++ b/src/input.rs @@ -10,7 +10,9 @@ use embassy_rp::{ pwm::Pwm, spi::Spi, }; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal}; +use embassy_sync::{ + blocking_mutex::raw::CriticalSectionRawMutex, pubsub::PubSubChannel, signal::Signal, +}; use embassy_time::{Duration, Instant, Timer}; use libm::{fmaxf, fminf}; @@ -23,9 +25,13 @@ use crate::{ FLASH_SIZE, }; -pub static GCC_SIGNAL: Signal = Signal::new(); +/// Used to send the button state to the usb task and the calibration task +pub static CHANNEL_GCC_STATE: PubSubChannel = + PubSubChannel::new(); +/// Used to send the stick state from the stick task to the main input task static STICK_SIGNAL: Signal = Signal::new(); + const STICK_HYST_VAL: f32 = 0.3; const FLOAT_ORIGIN: f32 = 127.5; @@ -67,6 +73,7 @@ pub enum StickAxis { YAxis, } +#[link_section = ".time_critical.read_ext_adc"] 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, @@ -101,6 +108,8 @@ fn read_ext_adc<'a, Acs: Pin, Ccs: Pin, I: embassy_rp::spi::Instance, M: embassy } /// Gets the average stick state over a 1ms interval in a non-blocking fashion. +/// Will wait until end_time is reached before continuing after reading the ADCs. +#[link_section = ".time_critical.update_stick_states"] async fn update_stick_states< 'a, Acs: Pin, @@ -128,9 +137,7 @@ async fn update_stick_states< let mut cx_sum = 0u32; let mut cy_sum = 0u32; - // TODO: lower interval possible? - - let end_time = Instant::now() + embassy_time::Duration::from_micros(500); + let end_time = Instant::now() + Duration::from_micros(300); // this seems kinda magic, and it is, but let mut loop_time = Duration::from_millis(0); while Instant::now() < end_time - loop_time { @@ -166,13 +173,11 @@ async fn update_stick_states< &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; loop_time = Instant::now() - loop_start; } + trace!("ADC Count: {}", adc_count); + 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, @@ -382,9 +387,10 @@ fn update_button_states< gcc_state.buttons_1.dpad_down = btn_ddown.is_low(); } +/// Task responsible for updating the button states. +/// Publishes the result to CHANNEL_GCC_STATE. #[embassy_executor::task] -pub async fn input_loop( - mut flash: Flash<'static, FLASH, Async, FLASH_SIZE>, +pub async fn update_button_state_task( btn_z: Input<'static, AnyPin>, btn_a: Input<'static, AnyPin>, btn_b: Input<'static, AnyPin>, @@ -397,12 +403,8 @@ pub async fn input_loop( btn_x: Input<'static, AnyPin>, btn_y: Input<'static, AnyPin>, btn_start: Input<'static, AnyPin>, - // 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, AnyPin>, - mut spi_ccs: Output<'static, AnyPin>, ) { + // upon loop entry, we check for the reset combo once if btn_a.is_low() && btn_x.is_low() && btn_y.is_low() { info!("Detected reset button press, booting into flash."); embassy_rp::rom_data::reset_to_usb_boot(0, 0); @@ -417,99 +419,109 @@ pub async fn input_loop( gcc_state.cstick_x = 127; gcc_state.cstick_y = 127; - let controller_config = ControllerConfig::from_flash_memory(&mut flash).unwrap(); + let gcc_publisher = CHANNEL_GCC_STATE.publisher().unwrap(); + 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, + ); + + // give other tasks a chance to do something + 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_publisher.publish_immediate(gcc_state); + } +} + +/// Task responsible for updating the stick states. +/// Publishes the result to STICK_SIGNAL. +#[embassy_executor::task] +pub async fn update_stick_states_task( + mut spi: Spi<'static, SPI0, embassy_rp::spi::Blocking>, + mut spi_acs: Output<'static, AnyPin>, + mut spi_ccs: Output<'static, AnyPin>, + controller_config: ControllerConfig, +) { let controlstick_params = StickParams::from_stick_config(&controller_config.astick_config); let cstick_params = StickParams::from_stick_config(&controller_config.cstick_config); let filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config); - let stick_state_fut = async { - let mut current_stick_state = StickState { - ax: 127, - ay: 127, - cx: 127, - cy: 127, - }; - let mut raw_stick_values = RawStickValues::default(); - let mut old_stick_pos = StickPositions::default(); - let mut cstick_waveshaping_values = WaveshapingValues::default(); - let mut controlstick_waveshaping_values = WaveshapingValues::default(); - let mut kalman_state = KalmanState::default(); - - let mut last_loop_time = Instant::now(); - - loop { - let timer = Timer::after_micros(1000); - - current_stick_state = update_stick_states( - &mut spi, - &mut spi_acs, - &mut spi_ccs, - ¤t_stick_state, - &controlstick_params, - &cstick_params, - &controller_config, - &filter_gains, - &mut controlstick_waveshaping_values, - &mut cstick_waveshaping_values, - &mut old_stick_pos, - &mut raw_stick_values, - &mut kalman_state, - ) - .await; - - timer.await; - - match (Instant::now() - last_loop_time).as_micros() { - a if a > 1100 => { - debug!("Loop took {} us", a); - } - _ => {} - }; - - last_loop_time = Instant::now(); - - STICK_SIGNAL.signal(current_stick_state.clone()); - } + let mut current_stick_state = StickState { + ax: 127, + ay: 127, + cx: 127, + cy: 127, }; + let mut raw_stick_values = RawStickValues::default(); + let mut old_stick_pos = StickPositions::default(); + let mut cstick_waveshaping_values = WaveshapingValues::default(); + let mut controlstick_waveshaping_values = WaveshapingValues::default(); + let mut kalman_state = KalmanState::default(); - let input_fut = async { - loop { - let timer = Timer::after_micros(500); + let mut last_loop_time = Instant::now(); - 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, - ); + debug!("Entering stick update loop."); - timer.await; + // the time at which the current loop iteration should end + let mut end_time = Instant::now() + Duration::from_micros(1000); - // 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 => (), + loop { + let timer = Timer::at(end_time); + + current_stick_state = update_stick_states( + &mut spi, + &mut spi_acs, + &mut spi_ccs, + ¤t_stick_state, + &controlstick_params, + &cstick_params, + &controller_config, + &filter_gains, + &mut controlstick_waveshaping_values, + &mut cstick_waveshaping_values, + &mut old_stick_pos, + &mut raw_stick_values, + &mut kalman_state, + ) + .await; + + timer.await; + end_time += Duration::from_micros(1000); + + match Instant::now() { + n => { + match (n - last_loop_time).as_micros() { + a if a > 1100 => debug!("Loop took {} us", a), + _ => {} + }; + last_loop_time = n; } + }; - GCC_SIGNAL.signal(gcc_state); - } - }; - - join(input_fut, stick_state_fut).await; + STICK_SIGNAL.signal(current_stick_state.clone()); + } } diff --git a/src/main.rs b/src/main.rs index 0316552..e49a382 100644 --- a/src/main.rs +++ b/src/main.rs @@ -4,6 +4,7 @@ #![no_std] #![no_main] +mod calibrate; mod config; mod filter; mod gcc_hid; @@ -11,23 +12,27 @@ mod helpers; mod input; mod stick; +use calibrate::calibration_loop; +use config::ControllerConfig; use defmt::{debug, info}; use embassy_executor::Executor; +use embassy_futures::join::join; use embassy_rp::{ bind_interrupts, flash::{Async, Flash}, gpio::{self, AnyPin, Input}, multicore::{spawn_core1, Stack}, - peripherals::USB, + peripherals::{SPI0, USB}, pwm::Pwm, spi::{self, Spi}, usb::{Driver, InterruptHandler}, }; -use gcc_hid::usb_transfer_loop; +use gcc_hid::usb_transfer_task; use gpio::{Level, Output}; -use input::input_loop; +use input::{update_button_state_task, update_stick_states_task}; use static_cell::StaticCell; + use {defmt_rtt as _, panic_probe as _}; static mut CORE1_STACK: Stack<4096> = Stack::new(); @@ -49,21 +54,17 @@ fn main() -> ! { let driver = Driver::new(p.USB, Irqs); + // reading and writing from flash has to be done on the main thread, else funny things happen. + let mut flash = Flash::<_, Async, FLASH_SIZE>::new(p.FLASH, p.DMA_CH0); let mut uid = [0u8; 8]; flash.blocking_unique_id(&mut uid).unwrap(); + let controller_config = ControllerConfig::from_flash_memory(&mut flash).unwrap(); + debug!("Read unique id: {:02X}", uid); - spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || { - let executor1 = EXECUTOR1.init(Executor::new()); - executor1.run(|spawner| spawner.spawn(usb_transfer_loop(driver, uid)).unwrap()); - }); - - let executor0 = EXECUTOR0.init(Executor::new()); - info!("Initialized."); - let mosi = p.PIN_7; let miso = p.PIN_4; let spi_clk = p.PIN_6; @@ -79,6 +80,33 @@ fn main() -> ! { let spi_acs = Output::new(AnyPin::from(p_acs), Level::High); // active low let spi_ccs = Output::new(AnyPin::from(p_ccs), Level::High); // active low + spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || { + let executor1 = EXECUTOR1.init(Executor::new()); + debug!("Mana"); + executor1.run(|spawner| { + spawner.spawn(usb_transfer_task(driver, uid)).unwrap(); + spawner + .spawn(update_button_state_task( + Input::new(AnyPin::from(p.PIN_20), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_17), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_16), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_11), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_9), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_10), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_8), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_22), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_21), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_18), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_19), gpio::Pull::Up), + Input::new(AnyPin::from(p.PIN_5), gpio::Pull::Up), + )) + .unwrap() + }); + }); + + let executor0 = EXECUTOR0.init(Executor::new()); + info!("Initialized."); + let mut pwm_config: embassy_rp::pwm::Config = Default::default(); pwm_config.top = 255; pwm_config.enable = true; @@ -92,25 +120,11 @@ fn main() -> ! { executor0.run(|spawner| { spawner - .spawn(input_loop( - flash, - Input::new(AnyPin::from(p.PIN_20), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_17), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_16), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_11), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_9), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_10), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_8), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_22), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_21), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_18), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_19), gpio::Pull::Up), - Input::new(AnyPin::from(p.PIN_5), gpio::Pull::Up), - // pwm_rumble, - // pwm_brake, + .spawn(update_stick_states_task( spi, spi_acs, spi_ccs, + controller_config, )) .unwrap() }); diff --git a/src/stick.rs b/src/stick.rs index de56e1d..f3307e8 100644 --- a/src/stick.rs +++ b/src/stick.rs @@ -570,6 +570,7 @@ fn calc_stick_values(angle: f32) -> (f32, f32) { (x, y) } +#[link_section = ".time_critical.linearize"] pub fn linearize(point: f32, coefficients: &[f32; 4]) -> f32 { coefficients[0] * (point * point * point) + coefficients[1] * (point * point) @@ -577,6 +578,7 @@ pub fn linearize(point: f32, coefficients: &[f32; 4]) -> f32 { + coefficients[3] } +#[link_section = ".time_critical.notch_remap"] pub fn notch_remap( x_in: f32, y_in: f32,