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

shuffle some threads around, stricter timing for stick readings

Browse source
This commit is contained in:
Naxdy 2024-03-28 22:43:44 +01:00
parent d4c88a4a60
commit 35349dee49
Signed by: Naxdy
GPG key ID: CC15075846BCE91B
6 changed files with 160 additions and 130 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
src/calibrate.rs Normal file
View file

@ -0,0 +1 @@
pub async fn calibration_loop() {}

2
src/config.rs
View file

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

9
src/gcc_hid.rs
View file

@ -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(()) => {

208
src/input.rs
View file

@ -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<CriticalSectionRawMutex, GcReport> = Signal::new();
/// Used to send the button state to the usb task and the calibration task
pub static CHANNEL_GCC_STATE: PubSubChannel<CriticalSectionRawMutex, GcReport, 1, 2, 1> =
PubSubChannel::new();
/// Used to send the stick state from the stick task to the main input task
static STICK_SIGNAL: Signal<CriticalSectionRawMutex, StickState> = 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,
&current_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,
&current_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());
}
}

68
src/main.rs
View file

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

2
src/stick.rs
View file

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