somethings wrong

linux/payload/FreshSupvHandler.cpp

@@ -31,6 +31,8 @@ FreshSupvHandler::FreshSupvHandler(DhbConfig cfg, CookieIF* comCookie, Gpio uart
       latchupStatusReport(this),
       countersReport(this),
       adcReport(this) {
+  spParams.buf = commandBuffer.data();
+  spParams.maxSize = commandBuffer.size();
   eventQueue = QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 5);
 }
 
@@ -83,7 +85,6 @@ void FreshSupvHandler::performDeviceOperation(uint8_t opCode) {
       }
     }
   } else if (opCode == OpCode::PARSE_TM) {
-    std::vector<ActionId_t> cmdsToRemove;
     for (auto& activeCmd : activeActionCmds) {
       if (activeCmd.second.cmdCountdown.hasTimedOut()) {
         if (activeCmd.second.commandedBy != MessageQueueIF::NO_QUEUE) {
@@ -452,6 +453,7 @@ void FreshSupvHandler::startTransition(Mode_t newMode, Submode_t newSubmode) {
 }
 
 void FreshSupvHandler::handleTransitionToOn() {
+  sif::debug << "psupv: trans to on" << std::endl;
   if (startupState == StartupState::IDLE) {
     bootTimeout.resetTimer();
     startupState = StartupState::POWER_SWITCHING;
@@ -473,24 +475,31 @@ void FreshSupvHandler::handleTransitionToOn() {
       return;
     }
   }
-  if (startupState == StartupState::BOOTING) {
-    if (bootTimeout.hasTimedOut()) {
-      uartIsolatorSwitch.pullHigh();
-      uartManager.start();
-      if (SET_TIME_DURING_BOOT) {
-        // TODO: Send command ,add command to command map.
-        startupState = StartupState::SET_TIME;
-      } else {
-        startupState = StartupState::ON;
-      }
+  if (startupState == StartupState::BOOTING and bootTimeout.hasTimedOut()) {
+    uartIsolatorSwitch.pullHigh();
+    uartManager.start();
+    if (SET_TIME_DURING_BOOT) {
+      startupState = StartupState::SET_TIME;
+    } else {
+      startupState = StartupState::ON;
     }
   }
+  if (startupState == StartupState::SET_TIME) {
+    ReturnValue_t result = prepareSetTimeRefCmd();
+    if (result != returnvalue::OK) {
+      sif::error << "FreshSupvHandler: Setting time command prepration failed" << std::endl;
+      startupState = StartupState::ON;
+    }
+    startupState = StartupState::WAIT_FOR_TIME_REPLY;
+  }
   if (startupState == StartupState::TIME_WAS_SET) {
     startupState = StartupState::ON;
   }
   if (startupState == StartupState::ON) {
     hkSet.setReportingEnabled(true);
     supv::SUPV_ON = true;
+    sif::debug << "psupv: going on" << std::endl;
+    transitionActive = false;
     setMode(targetMode);
   }
 }
@@ -506,7 +515,9 @@ void FreshSupvHandler::handleTransitionToOff() {
   }
   if (shutdownState == ShutdownState::POWER_SWITCHING) {
     if (switchIF.getSwitchState(switchId) == PowerSwitchIF::SWITCH_OFF or modeHelper.isTimedOut()) {
+      sif::debug << "psupv: going off" << std::endl;
       supv::SUPV_ON = false;
+      transitionActive = false;
       setMode(MODE_OFF);
     }
   }
@@ -1115,11 +1126,13 @@ ReturnValue_t FreshSupvHandler::handleAckReport(const uint8_t* data) {
     return returnvalue::OK;
   }
   info.ackRecv = true;
-  performCommandCompletionHandling(info);
+  performCommandCompletionHandling(static_cast<supv::Apid>((infoIter->first >> 16) & 0xffff),
+                                   infoIter->first & 0xff, info);
   return result;
 }
 
-void FreshSupvHandler::performCommandCompletionHandling(ActiveCmdInfo& info) {
+void FreshSupvHandler::performCommandCompletionHandling(supv::Apid apid, uint8_t serviceId,
+                                                        ActiveCmdInfo& info) {
   if (info.ackRecv and info.ackExeRecv and
       (not info.replyPacketExpected or info.replyPacketReceived)) {
     actionHelper.finish(true, info.commandedBy, info.commandId, returnvalue::OK);
@@ -1170,7 +1183,9 @@ ReturnValue_t FreshSupvHandler::handleExecutionReport(const uint8_t* data) {
     return returnvalue::OK;
   }
   info.ackExeRecv = true;
-  performCommandCompletionHandling(info);
+  performCommandCompletionHandling(static_cast<supv::Apid>((infoIter->first >> 16) & 0xffff),
+                                   infoIter->first & 0xff, info);
+
   return result;
 }
 
@@ -1197,9 +1212,6 @@ ReturnValue_t FreshSupvHandler::handleExecutionSuccessReport(ActiveCmdInfo& info
       break;
     }
     case supv::SET_TIME_REF: {
-      // We could only allow proper bootup when the time was set successfully, but
-      // this makes debugging difficult.
-
       if (startupState == StartupState::WAIT_FOR_TIME_REPLY) {
         startupState = StartupState::TIME_WAS_SET;
       }
@@ -1229,7 +1241,8 @@ void FreshSupvHandler::confirmReplyPacketReceived(supv::Apid apid, uint8_t servi
   if (infoIter != activeActionCmds.end()) {
     ActiveCmdInfo& info = infoIter->second;
     info.replyPacketReceived = true;
-    performCommandCompletionHandling(info);
+    performCommandCompletionHandling(static_cast<supv::Apid>((infoIter->first >> 16) & 0xffff),
+                                     infoIter->first & 0xff, info);
   }
 }