diff --git a/layout.drawio b/layout.drawio
index 062b926..4c6c3a0 100644
--- a/layout.drawio
+++ b/layout.drawio
@@ -213,17 +213,17 @@
-
+
-
+
-
+
@@ -235,10 +235,10 @@
-
+
-
+
@@ -291,7 +291,7 @@
-
+
@@ -354,7 +354,7 @@
-
+
@@ -370,18 +370,18 @@
-
+
-
+
-
+
-
+
diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs
index 799b39c..bdd661a 100644
--- a/rp2040/src/main.rs
+++ b/rp2040/src/main.rs
@@ -251,6 +251,9 @@ 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 gimbal_mode: u8;
// HW Button index map:
@@ -281,7 +284,7 @@ fn main() -> ! {
// ---------------------------------------------------------------
// USB HID joystick map :
// ---------------------------------------------------------------
- // | B4 L| B3 U| B7 U| | B26 | | B8 U| B1 U| B2 L|
+ // | Ry- L| Ry+ U| Rz- U| | B26 | | Rz+ U| B1 U| B2 L|
// ---------------------------------------------------------------
// | | B5 | B14 | B9 | | B10 | B15 | B6 | |
// | |
@@ -560,6 +563,31 @@ fn main() -> ! {
axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold;
}
+ // Update Virtual RY
+ if buttons[1].pressed && !buttons[0].pressed {
+ virtual_ry = virtual_ry.saturating_add(100);
+ usb_activity = true;
+ } else if buttons[0].pressed && !buttons[1].pressed {
+ virtual_ry = virtual_ry.saturating_sub(100);
+ usb_activity = true;
+ } else if virtual_ry != 0 && !buttons[1].pressed && !buttons[0].pressed {
+ // Optional: decay to center
+ virtual_ry = 0;
+ usb_activity = true;
+ }
+ // Update Virtual RZ
+ if buttons[25].pressed && !buttons[26].pressed {
+ virtual_rz = virtual_rz.saturating_add(100);
+ usb_activity = true;
+ } else if buttons[26].pressed && !buttons[25].pressed {
+ virtual_rz = virtual_rz.saturating_sub(100);
+ usb_activity = true;
+ } else if virtual_rz != 0 && !buttons[25].pressed && !buttons[26].pressed {
+ // Optional: decay to center
+ virtual_rz = 0;
+ usb_activity = true;
+ }
+
// Generate led activity when gimbal is moved from idle position
for item in axis.iter_mut() {
if item.value != item.previous_value {
@@ -605,10 +633,12 @@ fn main() -> ! {
// Dont send USB HID joystick report if there is no activity
// This is to avoid preventing the computer from going to sleep
if usb_update_count_down.wait().is_ok() && usb_activity {
- match usb_hid_joystick
- .device()
- .write_report(&get_joystick_report(&mut buttons, &mut axis))
- {
+ match usb_hid_joystick.device().write_report(&get_joystick_report(
+ &mut buttons,
+ &mut axis,
+ &virtual_ry,
+ &virtual_rz,
+ )) {
Err(UsbHidError::WouldBlock) => {}
Ok(_) => {}
Err(e) => {
@@ -663,11 +693,15 @@ 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,
) -> 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;
// Update button state for joystick buttons
let mut buttons: u32 = 0;
@@ -687,50 +721,12 @@ fn get_joystick_report(
y,
z,
rx,
+ ry,
+ rz,
buttons,
}
}
-/// Format hat value from 5 switches to USB HID coded value and button state
-///
-/// # Arguments
-/// * `input` - Hat value coded as
-/// bit 2-5: direction (U L R D)
-/// bit 1: button state
-/// 0 = not pressed
-/// 1 = pressed
-fn format_hat_value(input: u8) -> (u8, u8) {
- const HAT_CENTER: u8 = 8; //8 or 15 (OS-dependent; usually 8)
- const HAT_UP: u8 = 0;
- const HAT_UP_RIGHT: u8 = 1;
- const HAT_RIGHT: u8 = 2;
- const HAT_DOWN_RIGHT: u8 = 3;
- const HAT_DOWN: u8 = 4;
- const HAT_DOWN_LEFT: u8 = 5;
- const HAT_LEFT: u8 = 6;
- const HAT_UP_LEFT: u8 = 7;
-
- let direction: u8 = match input & 0xFE {
- 2 => HAT_UP,
- 4 => HAT_RIGHT,
- 6 => HAT_UP_RIGHT,
- 8 => HAT_DOWN,
- 12 => HAT_DOWN_RIGHT,
- 16 => HAT_LEFT,
- 24 => HAT_DOWN_LEFT,
- 18 => HAT_UP_LEFT,
- _ => HAT_CENTER,
- };
-
- // Alpine hat switch button filter
- let mut button_state: u8 = 0;
- if input & 0x01 == 0x01 && direction == HAT_CENTER {
- button_state = 1;
- }
-
- (direction, button_state)
-}
-
/// Calculate value for joystick axis
///
/// # Arguments
diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs
index 216f5ce..aac6b94 100644
--- a/rp2040/src/usb_joystick_device.rs
+++ b/rp2040/src/usb_joystick_device.rs
@@ -67,15 +67,17 @@ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[
0x09, 0x04, // Usage (Joystick)
0xA1, 0x01, // Collection (Application)
- // 4 signed 16-bit axes: X, Y, Z, Rx
+ // 6 signed 16-bit axes: X, Y, Z, Rx, Ry, Rz
0x09, 0x30, // Usage (X)
0x09, 0x31, // Usage (Y)
0x09, 0x32, // Usage (Z)
0x09, 0x33, // Usage (Rx)
+ 0x09, 0x34, // Usage (Ry)
+ 0x09, 0x35, // Usage (Rz)
0x16, 0x00, 0x80, // Logical Minimum (-32768)
0x26, 0xFF, 0x7F, // Logical Maximum (32767)
0x75, 0x10, // Report Size (16)
- 0x95, 0x04, // Report Count (4)
+ 0x95, 0x06, // Report Count (6)
0x81, 0x02, // Input (Data,Var,Abs)
// 26 Buttons (1-bit each)
@@ -102,6 +104,8 @@ pub struct JoystickReport {
pub y: i16, // 16bit
pub z: i16, // 16bit
pub rx: i16, // 16bit
+ pub ry: i16, // 16bit
+ pub rz: i16, // 16bit
pub buttons: u32, // 32bit
}
@@ -111,7 +115,7 @@ pub struct Joystick<'a, B: UsbBus> {
impl Joystick<'_, B> {
pub fn write_report(&mut self, report: &JoystickReport) -> Result<(), UsbHidError> {
- let mut data: [u8; 12] = [0; 12];
+ let mut data: [u8; 16] = [0; 16];
// Did not make the packed struct work, so doing it manually
data[0] = report.x as u8;
@@ -122,10 +126,14 @@ impl Joystick<'_, B> {
data[5] = (report.z >> 8) as u8;
data[6] = report.rx as u8;
data[7] = (report.rx >> 8) as u8;
- data[8] = report.buttons as u8;
- data[9] = (report.buttons >> 8) as u8;
- data[10] = (report.buttons >> 16) as u8;
- data[11] = (report.buttons >> 24) as u8;
+ data[8] = report.ry as u8;
+ data[9] = (report.ry >> 8) as u8;
+ data[10] = report.rz as u8;
+ data[11] = (report.rz >> 8) as u8;
+ data[12] = report.buttons as u8;
+ data[13] = (report.buttons >> 8) as u8;
+ data[14] = (report.buttons >> 16) as u8;
+ data[15] = (report.buttons >> 24) as u8;
self.interface
.write_report(&data)