Important bugfix and improvement for PCDU Handler

- Fixed out of bounds error
- Handle multiple messages in queue handler
- PCDU Handler now only called once in PST

mission/devices/PCDUHandler.cpp

@@ -81,27 +81,34 @@ ReturnValue_t PCDUHandler::initialize() {
 
 void PCDUHandler::initializeSwitchStates() {
   using namespace pcdu;
-  for (uint8_t idx = 0; idx < NUMBER_OF_SWITCHES; idx++) {
-    if (idx < PDU::CHANNELS_LEN) {
-      switchStates[idx] = INIT_SWITCHES_PDU1[idx];
-    } else {
-      switchStates[idx] = INIT_SWITCHES_PDU2[idx];
+  try {
+    for (uint8_t idx = 0; idx < NUMBER_OF_SWITCHES; idx++) {
+      if (idx < PDU::CHANNELS_LEN) {
+        switchStates[idx] = INIT_SWITCHES_PDU1.at(idx);
+      } else {
+        switchStates[idx] = INIT_SWITCHES_PDU2.at(idx - PDU::CHANNELS_LEN);
+      }
     }
+  } catch (const std::out_of_range& err) {
+    sif::error << "PCDUHandler::initializeSwitchStates: " << err.what() << std::endl;
   }
 }
 
 void PCDUHandler::readCommandQueue() {
-  ReturnValue_t result;
+  ReturnValue_t result = RETURN_OK;
   CommandMessage command;
 
-  result = commandQueue->receiveMessage(&command);
-  if (result != RETURN_OK) {
-    return;
-  }
+  for (result = commandQueue->receiveMessage(&command); result == RETURN_OK;
+       result = commandQueue->receiveMessage(&command)) {
+    result = commandQueue->receiveMessage(&command);
+    if (result != RETURN_OK) {
+      return;
+    }
 
-  result = poolManager.handleHousekeepingMessage(&command);
-  if (result == RETURN_OK) {
-    return;
+    result = poolManager.handleHousekeepingMessage(&command);
+    if (result == RETURN_OK) {
+      return;
+    }
   }
 }