diff --git a/right/src/usb_interface_keyboard.c b/right/src/usb_interface_keyboard.c
index ee3b34b..4cb3fbe 100644
--- a/right/src/usb_interface_keyboard.c
+++ b/right/src/usb_interface_keyboard.c
@@ -70,41 +70,20 @@ static usb_status_t UsbKeyboardAction(void)
     UsbKeyboardReport.reserved = 0;
 
     KeyMatrix_Init(&keyMatrix);
-    //KeyMatrix_Scan(&keyMatrix);
+    KeyMatrix_Scan(&keyMatrix);
 
-    for (uint8_t scancode_idx=0; scancode_idx<USB_KEYBOARD_MAX_KEYS; scancode_idx++) {
-        UsbKeyboardReport.scancodes[scancode_idx] = 0;
-    }
-
-    uint8_t scancode_idx = 0;
-    for (uint8_t col=0; col<KEYBOARD_MATRIX_COLS_NUM; col++) {
-        GPIO_WritePinOutput(keyMatrix.cols[col].gpio, keyMatrix.cols[col].pin, 1);
-        for (uint8_t row=0; row<KEYBOARD_MATRIX_ROWS_NUM; row++) {
-            if (GPIO_ReadPinInput(keyMatrix.rows[row].gpio, keyMatrix.rows[row].pin)) {
-                GPIO_SetPinsOutput(TEST_LED_GPIO, 1 << TEST_LED_GPIO_PIN);
-                UsbKeyboardReport.scancodes[scancode_idx++] = HID_KEYBOARD_SC_A + row*KEYBOARD_MATRIX_COLS_NUM + col;
-            }
-        }
-        GPIO_WritePinOutput(keyMatrix.cols[col].gpio, keyMatrix.cols[col].pin, 0);
-        for (volatile uint32_t i=0; i<100; i++);
-    }
-
-/*
-    uint8_t keyId = 0;
-    for (uint8_t col=0; col<keyMatrix.colNum; col++) {
-        GPIO_WritePinOutput(keyMatrix.cols[col].gpio, keyMatrix.cols[col].pin, 1);
-        for (uint8_t row=0; row<keyMatrix.rowNum; row++) {
-            keystates[row+col*keyMatrix.rowNum] = GPIO_ReadPinInput(keyMatrix.rows[row].gpio, keyMatrix.rows[row].pin);
-        }
-        GPIO_WritePinOutput(keyMatrix.cols[col].gpio, keyMatrix.cols[col].pin, 0);
+    uint8_t scancodeIdx;
+    for (uint8_t scancodeIdx=0; scancodeIdx<USB_KEYBOARD_MAX_KEYS; scancodeIdx++) {
+        UsbKeyboardReport.scancodes[scancodeIdx] = 0;
     }
 
+    scancodeIdx = 0;
     for (uint8_t keyId=0; keyId<KEYBOARD_MATRIX_COLS_NUM*KEYBOARD_MATRIX_ROWS_NUM; keyId++) {
-        if (keystates[keyId] && scancode_idx<USB_KEYBOARD_MAX_KEYS) {
-            UsbKeyboardReport.scancodes[scancode_idx++] = keyId + HID_KEYBOARD_SC_A;
+        if (keyMatrix.keyStates[keyId] && scancodeIdx<USB_KEYBOARD_MAX_KEYS) {
+            UsbKeyboardReport.scancodes[scancodeIdx++] = keyId + HID_KEYBOARD_SC_A;
         }
     }
-*/
+
     return USB_DeviceHidSend(UsbCompositeDevice.keyboardHandle, USB_KEYBOARD_ENDPOINT_INDEX,
                              (uint8_t*)&UsbKeyboardReport, USB_KEYBOARD_REPORT_LENGTH);
 }
diff --git a/shared/key_matrix.c b/shared/key_matrix.c
index fab5d67..daf2ab8 100644
--- a/shared/key_matrix.c
+++ b/shared/key_matrix.c
@@ -22,12 +22,12 @@ void KeyMatrix_Init(key_matrix_t *keyMatrix)
 
 void KeyMatrix_Scan(key_matrix_t *keyMatrix)
 {
-    uint8_t keyId = 0;
     for (uint8_t col=0; col<keyMatrix->colNum; col++) {
         GPIO_WritePinOutput(keyMatrix->cols[col].gpio, keyMatrix->cols[col].pin, 1);
         for (uint8_t row=0; row<keyMatrix->rowNum; row++) {
-            keyMatrix->keyStates[keyId++] = GPIO_ReadPinInput(keyMatrix->rows[row].gpio, keyMatrix->rows[row].pin);
+            keyMatrix->keyStates[row*keyMatrix->colNum + col] = GPIO_ReadPinInput(keyMatrix->rows[row].gpio, keyMatrix->rows[row].pin);
         }
         GPIO_WritePinOutput(keyMatrix->cols[col].gpio, keyMatrix->cols[col].pin, 0);
+        for (volatile uint32_t i=0; i<100; i++); // Wait for the new port state to settle. This avoid bogus key state detection.
     }
 }