diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs
index 1cbc06a..dbe7a07 100644
--- a/rp2040/src/main.rs
+++ b/rp2040/src/main.rs
@@ -356,15 +356,15 @@ fn main() -> ! {
     // Set up usb button layout
     buttons[0].usb_button = 1;
     buttons[1].usb_button = 2;
+    buttons[1].usb_button_sec_enable = true;
+    buttons[1].usb_button_sec = 21;
+    buttons[1].usb_button_sec_trigger_index = 0;
     buttons[2].usb_button = 3;
     buttons[3].usb_button = 4;
     buttons[3].usb_button_sec_enable = true;
-    buttons[3].usb_button_sec = 21;
-    buttons[3].usb_button_sec_trigger_index = 12;
+    buttons[3].usb_button_sec = 22;
+    buttons[3].usb_button_sec_trigger_index = 0;
     buttons[4].usb_button = 5;
-    buttons[4].usb_button_sec_enable = true;
-    buttons[4].usb_button_sec = 22;
-    buttons[4].usb_button_sec_trigger_index = 12;
     buttons[5].usb_button = 6;
     buttons[6].usb_button = 7;
     buttons[7].usb_button = 8;
@@ -373,18 +373,26 @@ fn main() -> ! {
     buttons[8].usb_button = 9;
     buttons[9].usb_button = 10;
     buttons[10].usb_button = 11;
+    buttons[10].usb_button_sec_enable = true;
+    buttons[10].usb_button_sec = 23;
+    buttons[10].usb_button_sec_trigger_index = 0;
     buttons[11].usb_button = 12;
+    buttons[11].usb_button_sec_enable = true;
+    buttons[11].usb_button_sec = 24;
+    buttons[11].usb_button_sec_trigger_index = 0;
     buttons[12].usb_button = 13;
     buttons[12].usb_button_sec = 17;
     buttons[12].usb_button_toggle_enable = true;
     buttons[13].usb_button = 14;
-    buttons[13].usb_button_sec_enable = true;
-    buttons[13].usb_button_sec = 23;
-    buttons[13].usb_button_sec_trigger_index = 12;
     buttons[14].usb_button = 15;
-    buttons[14].usb_button_sec_enable = true;
-    buttons[14].usb_button_sec = 24;
-    buttons[14].usb_button_sec_trigger_index = 12;
+    buttons[15].usb_button = 18;
+    buttons[15].usb_button_sec_enable = true;
+    buttons[15].usb_button_sec = 20;
+    buttons[15].usb_button_sec_trigger_index = 0;
+    buttons[20].usb_button = 19;
+    buttons[20].usb_button_sec_enable = true;
+    buttons[20].usb_button_sec = 25;
+    buttons[20].usb_button_sec_trigger_index = 0;
 
     // Set up elrs button layout
     buttons[0].elrs_channel = 7;
@@ -778,10 +786,10 @@ fn get_joystick_report(
     let mut rx: u16 = AXIS_CENTER;
     let mut ry: u16 = AXIS_CENTER;
     let rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value;
-    let (mut hat1, mut hat_button1) = format_hat_value(0);
-    let (mut hat2, mut hat_button2) = format_hat_value(0);
-    let (mut hat3, mut hat_button3) = format_hat_value(0);
-    let (mut hat4, mut hat_button4) = format_hat_value(0);
+    let (mut hat1, _hat_button1) = format_hat_value(0);
+    let (mut hat2, _hat_button2) = format_hat_value(0);
+    let (mut hat3, _hat_button3) = format_hat_value(0);
+    let (mut hat4, _hat_button4) = format_hat_value(0);
 
     // Right Alt mode active (bit 5)
     // Right gimbal control third joystick axis when right Fn mode is active
@@ -792,6 +800,7 @@ fn get_joystick_report(
         ry = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value;
     }
 
+    // Store hat bits
     let mut hat_left: u8 = 0;
     let mut hat_right: u8 = 0;
     for (index, key) in matrix_keys.iter_mut().enumerate() {
@@ -804,22 +813,52 @@ fn get_joystick_report(
     }
 
     // Convert hat switch data to HID code
-    if matrix_keys[7].pressed {
-        (hat3, hat_button3) = format_hat_value(hat_left);
-    } else {
-        (hat1, hat_button1) = format_hat_value(hat_left);
+    let (hat_l, hat_button_l) = format_hat_value(hat_left);
+    let (hat_r, hat_button_r) = format_hat_value(hat_right);
+
+    // Handle sec_button (Fn) for left hat switch
+    let mut sec_button_pressed: bool = false;
+    for (sec_index, sec_key) in matrix_keys.iter().enumerate() {
+        if matrix_keys[15].usb_button_sec_enable
+            && matrix_keys[15].usb_button_sec_trigger_index == sec_index
+            && sec_key.pressed
+        {
+            sec_button_pressed = true;
+            break;
+        }
     }
-    if matrix_keys[12].pressed {
-        (hat4, hat_button4) = format_hat_value(hat_right);
+    if matrix_keys[15].usb_changed {
+        matrix_keys[15].usb_button_sec_pressed = sec_button_pressed;
+    }
+    if matrix_keys[15].usb_button_sec != 0 && matrix_keys[15].usb_button_sec_pressed {
+        hat3 = hat_l;
     } else {
-        (hat2, hat_button2) = format_hat_value(hat_right);
+        hat1 = hat_l;
     }
 
-    // Update button state for joystick button 21-24 according to hat button 1-4
-    let mut buttons: u32 = (hat_button1 as u32) << 17
-        | ((hat_button2 as u32) << 18)
-        | ((hat_button3 as u32) << 19)
-        | ((hat_button4 as u32) << 24);
+    // Handle sec_button (Fn) for right hat switch
+    let mut sec_button_pressed: bool = false;
+    for (sec_index, sec_key) in matrix_keys.iter().enumerate() {
+        if matrix_keys[20].usb_button_sec_enable
+            && matrix_keys[20].usb_button_sec_trigger_index == sec_index
+            && sec_key.pressed
+        {
+            sec_button_pressed = true;
+            break;
+        }
+    }
+    if matrix_keys[20].usb_changed {
+        matrix_keys[20].usb_button_sec_pressed = sec_button_pressed;
+    }
+    if matrix_keys[20].usb_button_sec != 0 && matrix_keys[20].usb_button_sec_pressed {
+        hat4 = hat_r;
+    } else {
+        hat2 = hat_r;
+    }
+
+    // Fix button state for center hat press on hat
+    matrix_keys[15].pressed = hat_button_l != 0;
+    matrix_keys[20].pressed = hat_button_r != 0;
 
     // Update button array with Sec button trigger status
     // Using indexing instead of iterating to be able to iterate inside loop
@@ -834,10 +873,15 @@ fn get_joystick_report(
                 break;
             }
         }
-        matrix_keys[index].usb_button_sec_pressed = sec_button_pressed;
+        // Only update button when "root key" is pressed (ie. Do not change sec button as soon as Fn
+        // key is pressed
+        if matrix_keys[index].usb_changed {
+            matrix_keys[index].usb_button_sec_pressed = sec_button_pressed;
+        }
     }
 
     // Update button state for joystick buttons
+    let mut buttons: u32 = 0;
     for key in matrix_keys.iter_mut() {
         // Toggle mode button
         if key.usb_changed && key.usb_button_toggle_enable {