Added expo and direction compensation to the virtual axis

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_ry: u16 = AXIS_CENTER;
-    let mut virtual_rz: i16 = 0;
+    let mut virtual_rz: u16 = AXIS_CENTER;
-    let mut virtual_step: i16 = 512;
     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 {
+            let virtual_step: u16 = 5;
-                virtual_ry = virtual_ry.saturating_add(100);
+            // 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 {
+            } else if buttons[0].pressed
-                virtual_ry = virtual_ry.saturating_sub(100);
+                && !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 {
+            } else if (virtual_ry != AXIS_CENTER && !buttons[1].pressed && !buttons[0].pressed)
-                // Optional: decay to center
+                || (buttons[1].pressed && buttons[0].pressed)
-                virtual_ry = 0;
+            {
+                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 {
+            // Compensate value when changing direction
-                virtual_rz = virtual_rz.saturating_add(100);
+            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 {
+            } else if buttons[26].pressed
-                virtual_rz = virtual_rz.saturating_sub(100);
+                && !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 {
+            } else if (virtual_rz != AXIS_CENTER && !buttons[25].pressed && !buttons[26].pressed)
-                // Optional: decay to center
+                || (buttons[25].pressed && buttons[26].pressed)
-                virtual_rz = 0;
+            {
+                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,
+                calculate_axis_value(
-                &virtual_rz,
+                    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_ry: u16,
-    virtual_rz: &i16,
+    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 ry: i16 = axis_12bit_to_i16(virtual_ry);
-    let rz: i16 = *virtual_rz;
+    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,
     }
 }