   }
   
   unsigned long now = millis();
+  
+  // Poll the switches 
+  if (now - debounce_sample_last > 5) {
+    // Debounce, then set the mode.
+    debounce_sample_last = now;
+    // Read all the switch pins at once.
+    uint8_t port_d = PIND;
+
+    // Shift the variable towards the most significant bit
+    // and set the least significant bit to the current switch value
+    for (uint8_t i = 0; i < 5; i++) {
+      uint8_t pin = i + 2; // PD2 -> PD7
+      debounce_switches[i] <<= 1;
+      bitWrite(debounce_switches[i], 0, bitRead(port_d, pin));
+      if (debounce_switches[i] == 0) {
+        bitWrite(switches, i, 1);
+      } else {
+        bitWrite(switches, i, 0);
+      }
+    }
+    
+  }
+  
   if ( now - ppsLast > 250 ) {
     pps = ppsCounter;
     ppsLost = ppsLostCounter;


void loop(void)
{
  
  
  tx_payload.throttle = joystickMapValues(analogRead(PIN_THROTTLE), 125, 515, 1000);
  tx_payload.yaw      = joystickMapValues(analogRead(PIN_YAW), 10, 521, 1000);

int ppsLost = 0;
int ppsLostCounter = 0;

uint8_t switches = 0; // bit flag

void resetPayload() 
{
