completed impl

mission/devices/SolarArrayDeploymentHandler.cpp

@@ -153,9 +153,12 @@ bool SolarArrayDeploymentHandler::autonomousDeplForFile(const char* filename) {
   if (stateSwitch) {
     if (deplState == AutonomousDeplState::FIRST_BURN or
         deplState == AutonomousDeplState::SECOND_BURN) {
-      startFsm(true, true);
+      // TODO: Update to be channel specific
+      // startFsmOn(channel, config::SA_DEPL_BURN_TIME_SECS);
+      //startFsm(true, true);
     } else if (deplState == AutonomousDeplState::WAIT or deplState == AutonomousDeplState::DONE) {
-      startFsm(false, false);
+      startFsmOff();
+      //startFsm(false, false);
     }
   }
   if (stateSwitch or updateUptime) {
@@ -193,10 +196,7 @@ void SolarArrayDeploymentHandler::handleStateMachine() {
   }
   if (stateMachine == MAIN_POWER_OFF) {
     // These should never fail
-    deploymentTransistorsOff(DeploymentChannels::SA_1);
-    deploymentTransistorsOff(DeploymentChannels::SA_2);
-    mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
-    mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
+    allOff();
     stateMachine = WAIT_MAIN_POWER_OFF;
   }
   if (stateMachine == WAIT_MAIN_POWER_ON) {
@@ -211,8 +211,15 @@ void SolarArrayDeploymentHandler::handleStateMachine() {
   }
   if (stateMachine == SWITCH_DEPL_GPIOS) {
     // This should never fail
-    // deploymentTransistorsOn();
-    finishFsm(returnvalue::OK);
+    deploymentTransistorsOn(fsmInfo.channel);
+    burnCountdown.setTimeout(fsmInfo.burnCountdown);
+    stateMachine = CHANNEL_ON;
+  }
+  if(stateMachine == CHANNEL_ON) {
+    if(burnCountdown.hasTimedOut()) {
+      allOff();
+      stateMachine = WAIT_MAIN_POWER_OFF;
+    }
   }
 }
 
@@ -246,20 +253,39 @@ bool SolarArrayDeploymentHandler::checkMainPower(bool onOff) {
   return false;
 }
 
-bool SolarArrayDeploymentHandler::startFsm(std::optional<bool> sa1OnOff,
-                                           std::optional<bool> sa2OnOff) {
-  if ((stateMachine != StateMachine::IDLE) or (not sa1OnOff and not sa2OnOff)) {
+bool SolarArrayDeploymentHandler::startFsmOn(DeploymentChannels channel, uint32_t burnCountdown_) {
+  if (stateMachine != StateMachine::IDLE) {
     return false;
   }
+  if(burnCountdown_ > config::SA_DEPL_MAX_BURN_TIME) {
+    burnCountdown_ = config::SA_DEPL_MAX_BURN_TIME;
+  }
+  fsmInfo.onOff = true;
+  fsmInfo.channel = channel;
+  fsmInfo.burnCountdown = burnCountdown_;
   retryCounter = 0;
   return true;
 }
 
+void SolarArrayDeploymentHandler::startFsmOff() {
+  if(stateMachine != StateMachine::IDLE) {
+    // off commands override the state machine. Cancel any active action commands.
+    finishFsm(returnvalue::FAILED);
+  }
+  fsmInfo.onOff = false;
+  retryCounter = 0;
+  stateMachine = StateMachine::MAIN_POWER_ON;
+}
+
 void SolarArrayDeploymentHandler::finishFsm(ReturnValue_t resultForActionHelper) {
   retryCounter = 0;
   stateMachine = StateMachine::IDLE;
   if (actionActive) {
-    actionHelper.finish(true, rememberCommanderId, activeCmd, resultForActionHelper);
+    bool success = false;
+    if(resultForActionHelper == returnvalue::OK or resultForActionHelper == HasActionsIF::EXECUTION_FINISHED) {
+      success = true;
+    }
+    actionHelper.finish(success, rememberCommanderId, activeCmd, resultForActionHelper);
   }
 }
 
@@ -287,6 +313,13 @@ ReturnValue_t SolarArrayDeploymentHandler::deploymentTransistorsOn(DeploymentCha
   return result;
 }
 
+void SolarArrayDeploymentHandler::allOff() {
+  deploymentTransistorsOff(DeploymentChannels::SA_1);
+  deploymentTransistorsOff(DeploymentChannels::SA_2);
+  mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+  mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
+}
+
 ReturnValue_t SolarArrayDeploymentHandler::deploymentTransistorsOff(DeploymentChannels channel) {
   ReturnValue_t result = returnvalue::FAILED;
   if (channel == DeploymentChannels::SA_1) {
@@ -337,6 +370,19 @@ ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
     if (result != returnvalue::OK) {
       return result;
     }
+    uint32_t burnCountdown = cmd.getBurnTime();
+    DeploymentChannels channel;
+    result = cmd.getChannel(channel);
+    if(result != returnvalue::OK) {
+      return result;
+    }
+    if (not startFsmOn(channel, burnCountdown)) {
+      return HasActionsIF::IS_BUSY;
+    }
+    return result;
+  } else if(actionId == SWITCH_OFF_DEPLOYMENT) {
+     startFsmOff();
+     return result;
   } else {
     return HasActionsIF::INVALID_ACTION_ID;
   }