|
|
|
@ -505,11 +505,27 @@ void FreshSupvHandler::handleTransitionToOff() {
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t FreshSupvHandler::sendCommand(TcBase& tc) {
|
|
|
|
|
ReturnValue_t FreshSupvHandler::sendCommand(TcBase& tc, uint32_t cmdCountdownMs) {
|
|
|
|
|
if (DEBUG_PLOC_SUPV) {
|
|
|
|
|
sif::debug << "PLOC SUPV: SEND PACKET Size " << tc.getFullPacketLen() << " Module APID "
|
|
|
|
|
<< (int)tc.getModuleApid() << " Service ID " << (int)tc.getServiceId() << std::endl;
|
|
|
|
|
}
|
|
|
|
|
ActiveCmdInfo info(cmdCountdownMs);
|
|
|
|
|
auto activeCmdIter =
|
|
|
|
|
activeActionCmds.find(buildActiveCmdKey(tc.getModuleApid(), tc.getServiceId()));
|
|
|
|
|
if (activeCmdIter == activeActionCmds.end()) {
|
|
|
|
|
activeActionCmds.emplace(buildActiveCmdKey(tc.getModuleApid(), tc.getServiceId()), info);
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
if (activeCmdIter->second.isPending) {
|
|
|
|
|
return HasActionsIF::IS_BUSY;
|
|
|
|
|
}
|
|
|
|
|
activeCmdIter->second.isPending = true;
|
|
|
|
|
activeCmdIter->second.ackRecv = false;
|
|
|
|
|
activeCmdIter->second.ackExeRecv = false;
|
|
|
|
|
activeCmdIter->second.cmdCountdown.setTimeout(cmdCountdownMs);
|
|
|
|
|
activeCmdIter->second.cmdCountdown.resetTimer();
|
|
|
|
|
}
|
|
|
|
|
return uartManager.sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen());
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
@ -768,10 +784,9 @@ ReturnValue_t FreshSupvHandler::prepareWipeMramCmd(const uint8_t* commandData, s
|
|
|
|
|
ReturnValue_t FreshSupvHandler::parseTmPackets() {
|
|
|
|
|
uint8_t* receivedData = nullptr;
|
|
|
|
|
size_t receivedSize = 0;
|
|
|
|
|
ReturnValue_t result;
|
|
|
|
|
while (true) {
|
|
|
|
|
result = uartManager.readReceivedMessage(comCookie, &receivedData, &receivedSize);
|
|
|
|
|
if (receivedSize == 0) {
|
|
|
|
|
ReturnValue_t result = uartManager.readReceivedMessage(comCookie, &receivedData, &receivedSize);
|
|
|
|
|
if (result != returnvalue::OK or receivedSize == 0) {
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
tmReader.setData(receivedData, receivedSize);
|
|
|
|
@ -784,13 +799,13 @@ ReturnValue_t FreshSupvHandler::parseTmPackets() {
|
|
|
|
|
switch (tmReader.getServiceId()) {
|
|
|
|
|
case (static_cast<uint8_t>(supv::tm::TmtcId::ACK)):
|
|
|
|
|
case (static_cast<uint8_t>(supv::tm::TmtcId::NAK)): {
|
|
|
|
|
// TODO: Handle ACK report.
|
|
|
|
|
return OK;
|
|
|
|
|
handleAckReport(receivedData);
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
case (static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK)):
|
|
|
|
|
case (static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)): {
|
|
|
|
|
// TODO: Hnadle Exe report.
|
|
|
|
|
return OK;
|
|
|
|
|
handleExecutionReport(receivedData);
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
@ -908,13 +923,13 @@ void FreshSupvHandler::handlePacketPrint() {
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool FreshSupvHandler::isCommandAlreadyActive(ActionId_t actionId) const {
|
|
|
|
|
auto iter = activeActionCmds.find(actionId);
|
|
|
|
|
if (iter == activeActionCmds.end()) {
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
if (iter->second.isPending) {
|
|
|
|
|
// Not the most efficient implementation but who cares. It's not like this map is going
|
|
|
|
|
// to be huge in the nominal case..
|
|
|
|
|
for (const auto& info : activeActionCmds) {
|
|
|
|
|
if (info.second.commandId == actionId and info.second.isPending) {
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
@ -1053,3 +1068,140 @@ ReturnValue_t FreshSupvHandler::eventSubscription() {
|
|
|
|
|
}
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t FreshSupvHandler::handleAckReport(const uint8_t* data) {
|
|
|
|
|
using namespace supv;
|
|
|
|
|
ReturnValue_t result = returnvalue::OK;
|
|
|
|
|
if (not tmReader.verifyCrc()) {
|
|
|
|
|
sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl;
|
|
|
|
|
triggerEvent(SUPV_CRC_FAILURE_EVENT);
|
|
|
|
|
return returnvalue::FAILED;
|
|
|
|
|
}
|
|
|
|
|
AcknowledgmentReport ack(tmReader);
|
|
|
|
|
result = ack.parse();
|
|
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
auto infoIter =
|
|
|
|
|
activeActionCmds.find(buildActiveCmdKey(ack.getRefModuleApid(), ack.getRefServiceId()));
|
|
|
|
|
if (infoIter == activeActionCmds.end()) {
|
|
|
|
|
triggerEvent(SUPV_ACK_UNKNOWN_COMMAND, ack.getRefModuleApid(), ack.getRefServiceId());
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
ActiveCmdInfo& info = infoIter->second;
|
|
|
|
|
if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::NAK)) {
|
|
|
|
|
triggerEvent(SUPV_ACK_FAILURE, info.commandId, static_cast<uint32_t>(ack.getStatusCode()));
|
|
|
|
|
ack.printStatusInformation();
|
|
|
|
|
printAckFailureInfo(ack.getStatusCode(), info.commandId);
|
|
|
|
|
if (info.commandedBy != MessageQueueIF::NO_QUEUE) {
|
|
|
|
|
actionHelper.finish(false, info.commandedBy, info.commandId, result::RECEIVED_ACK_FAILURE);
|
|
|
|
|
}
|
|
|
|
|
info.isPending = false;
|
|
|
|
|
return returnvalue::OK;
|
|
|
|
|
}
|
|
|
|
|
info.ackRecv = true;
|
|
|
|
|
if (info.ackRecv and info.ackExeRecv) {
|
|
|
|
|
actionHelper.finish(true, info.commandedBy, info.commandId, returnvalue::OK);
|
|
|
|
|
info.isPending = false;
|
|
|
|
|
}
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void FreshSupvHandler::printAckFailureInfo(uint16_t statusCode, DeviceCommandId_t commandId) {
|
|
|
|
|
switch (commandId) {
|
|
|
|
|
case (supv::SET_TIME_REF): {
|
|
|
|
|
sif::warning
|
|
|
|
|
<< "PlocSupervisoHandler: Setting time failed. Make sure the OBC has a valid time"
|
|
|
|
|
<< std::endl;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
uint32_t FreshSupvHandler::buildActiveCmdKey(uint16_t moduleApid, uint8_t serviceId) {
|
|
|
|
|
return (moduleApid << 16) | serviceId;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t FreshSupvHandler::handleExecutionReport(const uint8_t* data) {
|
|
|
|
|
using namespace supv;
|
|
|
|
|
ReturnValue_t result = returnvalue::OK;
|
|
|
|
|
|
|
|
|
|
if (not tmReader.verifyCrc()) {
|
|
|
|
|
return result::CRC_FAILURE;
|
|
|
|
|
}
|
|
|
|
|
ExecutionReport exe(tmReader);
|
|
|
|
|
result = exe.parse();
|
|
|
|
|
if (result != OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
auto infoIter =
|
|
|
|
|
activeActionCmds.find(buildActiveCmdKey(exe.getRefModuleApid(), exe.getRefServiceId()));
|
|
|
|
|
if (infoIter == activeActionCmds.end()) {
|
|
|
|
|
triggerEvent(SUPV_EXE_ACK_UNKNOWN_COMMAND, exe.getRefModuleApid(), exe.getRefServiceId());
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
ActiveCmdInfo& info = infoIter->second;
|
|
|
|
|
if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK)) {
|
|
|
|
|
result = handleExecutionSuccessReport(info, exe);
|
|
|
|
|
} else if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)) {
|
|
|
|
|
handleExecutionFailureReport(info, exe);
|
|
|
|
|
return returnvalue::OK;
|
|
|
|
|
}
|
|
|
|
|
info.ackExeRecv = true;
|
|
|
|
|
if (info.ackRecv and info.ackExeRecv) {
|
|
|
|
|
actionHelper.finish(true, info.commandedBy, info.commandId, returnvalue::OK);
|
|
|
|
|
info.isPending = false;
|
|
|
|
|
}
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t FreshSupvHandler::handleExecutionSuccessReport(ActiveCmdInfo& info,
|
|
|
|
|
ExecutionReport& report) {
|
|
|
|
|
switch (info.commandId) {
|
|
|
|
|
case supv::READ_GPIO: {
|
|
|
|
|
// TODO: Fix
|
|
|
|
|
uint16_t gpioState = report.getStatusCode();
|
|
|
|
|
#if OBSW_DEBUG_PLOC_SUPERVISOR == 1
|
|
|
|
|
sif::info << "PlocSupervisorHandler: Read GPIO TM, State: " << gpioState << std::endl;
|
|
|
|
|
#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */
|
|
|
|
|
uint8_t data[sizeof(gpioState)];
|
|
|
|
|
size_t size = 0;
|
|
|
|
|
ReturnValue_t result = SerializeAdapter::serialize(&gpioState, data, &size, sizeof(gpioState),
|
|
|
|
|
SerializeIF::Endianness::BIG);
|
|
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
|
sif::debug << "PlocSupervisorHandler: Failed to deserialize GPIO state" << std::endl;
|
|
|
|
|
}
|
|
|
|
|
result = actionHelper.reportData(info.commandedBy, info.commandId, data, sizeof(data));
|
|
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
|
sif::warning << "PlocSupervisorHandler: Read GPIO, failed to report data" << std::endl;
|
|
|
|
|
}
|
|
|
|
|
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;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
return returnvalue::OK;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void FreshSupvHandler::handleExecutionFailureReport(ActiveCmdInfo& info, ExecutionReport& report) {
|
|
|
|
|
using namespace supv;
|
|
|
|
|
report.printStatusInformation();
|
|
|
|
|
if (info.commandId != DeviceHandlerIF::NO_COMMAND_ID) {
|
|
|
|
|
triggerEvent(SUPV_EXE_FAILURE, info.commandId, static_cast<uint32_t>(report.getStatusCode()));
|
|
|
|
|
}
|
|
|
|
|
if (info.commandedBy) {
|
|
|
|
|
actionHelper.finish(false, info.commandedBy, info.commandId, report.getStatusCode());
|
|
|
|
|
}
|
|
|
|
|
info.isPending = false;
|
|
|
|
|
}
|
|
|
|
|