diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs
index 1f1e7a5..ea8817b 100644
--- a/rp2040/src/main.rs
+++ b/rp2040/src/main.rs
@@ -26,6 +26,8 @@ use waveshare_rp2040_zero::{
 };
 use ws2812_pio::Ws2812;
 
+pub const KEY_ROWS: usize = 4;
+pub const KEY_COLS: usize = 12;
 pub const NUMBER_OF_KEYS: usize = 42;
 
 #[derive(Copy, Clone)]
@@ -148,7 +150,8 @@ fn main() -> ! {
     // RIGHT_ALT = AltGr
 
     let layout: [[Keyboard; NUMBER_OF_KEYS]; 3] = [
-        [   // Function layer 0
+        [
+            // Function layer 0
             Keyboard::Tab,
             Keyboard::Q,
             Keyboard::W,
@@ -192,7 +195,8 @@ fn main() -> ! {
             Keyboard::NoEventIndicated,
             Keyboard::RightAlt,
         ],
-        [   // Function layer 1
+        [
+            // Function layer 1
             Keyboard::Escape,
             Keyboard::F1,
             Keyboard::F2,
@@ -236,7 +240,8 @@ fn main() -> ! {
             Keyboard::NoEventIndicated,
             Keyboard::RightAlt,
         ],
-        [   // Function layer 2
+        [
+            // Function layer 2
             Keyboard::F11,
             Keyboard::F12,
             Keyboard::F13,
@@ -288,9 +293,15 @@ fn main() -> ! {
     let mut tick_count_down = timer.count_down();
     tick_count_down.start(1.millis());
 
-    let color_purple: RGB8 = (0, 10, 10).into();
-    let color_red: RGB8 = (0, 10, 0).into();
-    let color_none: RGB8 = (0, 0, 0).into();
+    let status_led_color: [RGB8; 5] = [
+        (0, 0, 0).into(),   // Off
+        (10, 7, 0).into(),  // Green
+        (10, 4, 10).into(), // Blue
+        (5, 10, 0).into(),  // Orange
+        (2, 20, 0).into(),  // Red
+    ];
+
+    let mut caps_lock_active = false;
 
     init_keyboard_matrix_pins(matrix_cols, &mut delay);
 
@@ -303,21 +314,31 @@ fn main() -> ! {
             // Get current function layer
             let fn_mode = get_fn_mode(pressed_keys);
 
+            // Set status LED colour based on function layer and capslock
+            // 0 = green, 1 = blue, 2 = orange, capslock active = red.
+            if caps_lock_active == true {
+                status_led
+                    .write([status_led_color[4]].iter().copied())
+                    .unwrap();
+            } else {
+                status_led
+                    .write([status_led_color[usize::from(fn_mode) + 1]].iter().copied())
+                    .unwrap();
+            }
+
             // Copy result from scanned keys to matrix struct
             for (index, key) in pressed_keys.iter().enumerate() {
                 matrix_keys[index].current_state = *key;
             }
 
             // Generate keyboard report
-            let keyboard_report =
-                get_keyboard_report(matrix_keys, layout, fn_mode);
+            let keyboard_report = get_keyboard_report(matrix_keys, layout, fn_mode);
 
             match keyboard.device().write_report(keyboard_report) {
                 Err(UsbHidError::WouldBlock) => {}
                 Err(UsbHidError::Duplicate) => {}
                 Ok(_) => {}
                 Err(e) => {
-                    status_led.write([color_red].iter().copied()).unwrap();
                     core::panic!("Failed to write keyboard report: {:?}", e)
                 }
             };
@@ -329,7 +350,6 @@ fn main() -> ! {
                 Err(UsbHidError::WouldBlock) => {}
                 Ok(_) => {}
                 Err(e) => {
-                    status_led.write([color_red].iter().copied()).unwrap();
                     core::panic!("Failed to process keyboard tick: {:?}", e)
                 }
             };
@@ -341,14 +361,13 @@ fn main() -> ! {
                     //do nothing
                 }
                 Err(e) => {
-                    status_led.write([color_red].iter().copied()).unwrap();
                     core::panic!("Failed to read keyboard report: {:?}", e)
                 }
                 Ok(leds) => {
                     if leds.caps_lock == true {
-                        status_led.write([color_purple].iter().copied()).unwrap();
+                        caps_lock_active = true;
                     } else {
-                        status_led.write([color_none].iter().copied()).unwrap();
+                        caps_lock_active = false;
                     }
                 }
             }
@@ -377,25 +396,29 @@ fn get_pressed_keys(
 ) -> [bool; NUMBER_OF_KEYS] {
     // Scan keyboard matrix for pressed keys
     let mut pressed_keys: [bool; NUMBER_OF_KEYS] = [false; NUMBER_OF_KEYS];
-    let mut key_index: usize = 0;
     for (col_index, col) in cols.iter_mut().enumerate() {
+        // Activate column
         col.set_low().unwrap();
         delay.delay_us(10);
+
+        // Read rows
         for (row_index, row) in rows.iter().enumerate() {
-            if col_index == 3
+            // Do not check unconnected keys in the matrix
+            if row_index == 3
                 && (row_index == 0
-                    || row_index == 1
-                    || row_index == 2
-                    || row_index == 9
-                    || row_index == 10
-                    || row_index == 11)
+                    || col_index == 1
+                    || col_index == 2
+                    || col_index == 9
+                    || col_index == 10
+                    || col_index == 11)
             {
                 continue;
             }
-            if row.is_low().unwrap() {
-                pressed_keys[key_index] = true;
+            if row_index < 3 && row.is_low().unwrap() {
+                pressed_keys[col_index + (row_index * KEY_COLS)] = true;
+            } else if row.is_low().unwrap() {
+                pressed_keys[col_index + (row_index * KEY_COLS) - 3] = true;
             }
-            key_index += 1;
         }
         col.set_high().unwrap();
         delay.delay_us(10);
@@ -431,7 +454,7 @@ fn get_keyboard_report(
             key.fn_mode = fn_mode;
         }
         if key.current_state == true {
-            keyboard_report[index] = layout[index][usize::from(key.fn_mode)];
+            keyboard_report[index] = layout[usize::from(key.fn_mode)][index];
         }
     }
     // Return report