cm/cmdr-joystick
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added expo and direction compensation to the virtual axis

Browse Source
This commit is contained in:
Christoffer Martinsson 2025-05-29 12:12:11 +02:00
parent 0ab690dae1
commit 6243f552c3
1 changed files with 75 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
rp2040/src/main.rs
View File

@ -251,9 +251,8 @@ fn main() -> ! {
let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS];
let mut buttons: [Button; NUMBER_OF_BUTTONS + 2] = [Button::default(); NUMBER_OF_BUTTONS + 2];
let mut virtual_ry: i16 = 0;
let mut virtual_rz: i16 = 0;
let mut virtual_step: i16 = 512;
let mut virtual_ry: u16 = AXIS_CENTER;
let mut virtual_rz: u16 = AXIS_CENTER;
let mut gimbal_mode: u8;
// HW Button index map:
@ -337,6 +336,7 @@ fn main() -> ! {
// Table for gimbal expo curve lookup insded of doing floating point math for every analog read
let expo_lut: [u16; ADC_MAX as usize + 1] = generate_expo_lut(0.3);
let expo_lut_virtual: [u16; ADC_MAX as usize + 1] = generate_expo_lut(0.6);
// Create dynamic smoother array for gimbal axis
let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [
@ -564,27 +564,59 @@ fn main() -> ! {
}
// Update Virtual RY
if buttons[1].pressed && !buttons[0].pressed {
virtual_ry = virtual_ry.saturating_add(100);
let virtual_step: u16 = 5;
// Compensate value when changing direction
if buttons[1].pressed && !buttons[0].pressed && virtual_ry < AXIS_CENTER {
virtual_ry = AXIS_CENTER + (AXIS_CENTER - virtual_ry);
} else if buttons[0].pressed && !buttons[1].pressed && virtual_ry > AXIS_CENTER {
virtual_ry = AXIS_CENTER - (virtual_ry - AXIS_CENTER);
}
// Move virtual axis
if buttons[1].pressed && !buttons[0].pressed && virtual_ry < ADC_MAX - virtual_step {
virtual_ry = virtual_ry + virtual_step;
usb_activity = true;
} else if buttons[0].pressed && !buttons[1].pressed {
virtual_ry = virtual_ry.saturating_sub(100);
} else if buttons[0].pressed
&& !buttons[1].pressed
&& virtual_ry > ADC_MIN + virtual_step
{
virtual_ry = virtual_ry - virtual_step;
usb_activity = true;
} else if virtual_ry != 0 && !buttons[1].pressed && !buttons[0].pressed {
// Optional: decay to center
virtual_ry = 0;
} else if (virtual_ry != AXIS_CENTER && !buttons[1].pressed && !buttons[0].pressed)
|| (buttons[1].pressed && buttons[0].pressed)
{
if virtual_ry < AXIS_CENTER + virtual_step {
virtual_ry = virtual_ry + virtual_step;
} else if virtual_ry > AXIS_CENTER - virtual_step {
virtual_ry = virtual_ry - virtual_step;
}
usb_activity = true;
}
// Update Virtual RZ
if buttons[25].pressed && !buttons[26].pressed {
virtual_rz = virtual_rz.saturating_add(100);
// Compensate value when changing direction
if buttons[25].pressed && !buttons[26].pressed && virtual_rz < AXIS_CENTER {
virtual_rz = AXIS_CENTER + (AXIS_CENTER - virtual_rz);
} else if buttons[26].pressed && !buttons[25].pressed && virtual_rz > AXIS_CENTER {
virtual_rz = AXIS_CENTER - (virtual_rz - AXIS_CENTER);
}
// Move virtual axis
if buttons[25].pressed && !buttons[26].pressed && virtual_rz < ADC_MAX - virtual_step {
virtual_rz = virtual_rz + virtual_step;
usb_activity = true;
} else if buttons[26].pressed && !buttons[25].pressed {
virtual_rz = virtual_rz.saturating_sub(100);
} else if buttons[26].pressed
&& !buttons[25].pressed
&& virtual_rz > ADC_MIN + virtual_step
{
virtual_rz = virtual_rz - virtual_step;
usb_activity = true;
} else if virtual_rz != 0 && !buttons[25].pressed && !buttons[26].pressed {
// Optional: decay to center
virtual_rz = 0;
} else if (virtual_rz != AXIS_CENTER && !buttons[25].pressed && !buttons[26].pressed)
|| (buttons[25].pressed && buttons[26].pressed)
{
if virtual_rz < AXIS_CENTER + virtual_step {
virtual_rz = virtual_rz + virtual_step;
} else if virtual_rz > AXIS_CENTER - virtual_step {
virtual_rz = virtual_rz - virtual_step;
}
usb_activity = true;
}
@ -636,8 +668,24 @@ fn main() -> ! {
match usb_hid_joystick.device().write_report(&get_joystick_report(
&mut buttons,
&mut axis,
&virtual_ry,
&virtual_rz,
calculate_axis_value(
virtual_ry,
ADC_MIN,
ADC_MAX,
AXIS_CENTER,
(0, 0, 0),
true,
&expo_lut_virtual,
),
calculate_axis_value(
virtual_rz,
ADC_MIN,
ADC_MAX,
AXIS_CENTER,
(0, 0, 0),
true,
&expo_lut_virtual,
),
)) {
Err(UsbHidError::WouldBlock) => {}
Ok(_) => {}
@ -693,15 +741,17 @@ fn update_status_led<P, SM, I>(
fn get_joystick_report(
matrix_keys: &mut [Button; NUMBER_OF_BUTTONS + 2],
axis: &mut [GimbalAxis; 4],
virtual_ry: &i16,
virtual_rz: &i16,
virtual_ry: u16,
virtual_rz: u16,
) -> JoystickReport {
let x: i16 = axis_12bit_to_i16(axis[GIMBAL_AXIS_LEFT_X].value);
let y: i16 = axis_12bit_to_i16(ADC_MAX - axis[GIMBAL_AXIS_LEFT_Y].value);
let z: i16 = axis_12bit_to_i16(axis[GIMBAL_AXIS_RIGHT_X].value);
let rx: i16 = axis_12bit_to_i16(ADC_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value);
let ry: i16 = *virtual_ry;
let rz: i16 = *virtual_rz;
let ry: i16 = axis_12bit_to_i16(virtual_ry);
let rz: i16 = axis_12bit_to_i16(virtual_rz);
let slider: i16 = 0;
let dial: i16 = 0;
// Update button state for joystick buttons
let mut buttons: u32 = 0;
@ -723,6 +773,8 @@ fn get_joystick_report(
rx,
ry,
rz,
slider,
dial,
buttons,
}
}