From 64ddd03ad08476fa4a04d5315197abc4e3b67b46 Mon Sep 17 00:00:00 2001
From: Christoffer Martinsson <cm@cmtec.se>
Date: Sun, 6 Aug 2023 16:39:05 +0200
Subject: [PATCH] Fixed some bugs

---
 rp2040/src/main.rs | 43 ++++++++++++++++++++++++++++++-------------
 1 file changed, 30 insertions(+), 13 deletions(-)

diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs
index df9d5dd..c131ffe 100644
--- a/rp2040/src/main.rs
+++ b/rp2040/src/main.rs
@@ -140,10 +140,10 @@ fn main() -> ! {
     // Configure ADC input pins
     // Have not figured out hov to store the adc pins in an array yet
     // TODO: Find a way to store adc pins in an array
-    let mut adc_pin_left_x = pins.gp26.into_floating_input();
-    let mut adc_pin_left_y = pins.gp27.into_floating_input();
-    let mut adc_pin_right_x = pins.gp28.into_floating_input();
-    let mut adc_pin_right_y = pins.gp29.into_floating_input();
+    let mut adc_pin_left_x = pins.gp29.into_floating_input();
+    let mut adc_pin_left_y = pins.gp28.into_floating_input();
+    let mut adc_pin_right_x = pins.gp27.into_floating_input();
+    let mut adc_pin_right_y = pins.gp26.into_floating_input();
 
     // Setting up array with pins connected to button rows
     let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>; BUTTON_ROWS] = &[
@@ -259,6 +259,22 @@ fn main() -> ! {
                 buttons[index].pressed = *key;
             }
 
+            if button_matrix.buttons_pressed()[0]
+                && button_matrix.buttons_pressed()[1]
+                && button_matrix.buttons_pressed()[5]
+                && button_matrix.buttons_pressed()[6]
+                && button_matrix.buttons_pressed()[8]
+                && button_matrix.buttons_pressed()[9]
+            {
+                status_led.update(StatusMode::Bootloader);
+                let gpio_activity_pin_mask: u32 = 0;
+                let disable_interface_mask: u32 = 0;
+                rp2040_hal::rom_data::reset_to_usb_boot(
+                    gpio_activity_pin_mask,
+                    disable_interface_mask,
+                );
+            }
+
             match usb_hid_joystick.device().write_report(&get_joystick_report(
                 &mut buttons,
                 &mut axis,
@@ -354,9 +370,9 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 {
 
     if fn_l_active && fn_r_active {
         mode = 3;
-    } else if fn_l_active {
-        mode = 2;
     } else if fn_r_active {
+        mode = 2;
+    } else if fn_l_active {
         mode = 1;
     }
 
@@ -417,13 +433,14 @@ fn get_joystick_report(
     } else if mode & 0x10 == 0x10
         && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 1 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 3)
     {
-        rz = remap(
-            axis[GIMBAL_AXIS_LEFT_Y].value,
-            AXIS_MIN,
-            AXIS_MAX,
-            AXIS_CENTER,
-            AXIS_MIN,
-        );
+        rz = AXIS_MAX
+            - remap(
+                axis[GIMBAL_AXIS_LEFT_Y].value,
+                AXIS_MIN,
+                AXIS_MAX,
+                AXIS_CENTER,
+                AXIS_MAX,
+            );
     }
 
     // Right Alt mode active (bit 5)