diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index bdd661a..1b5a7fb 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -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( 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, } }