eive/eive-obsw
13
4
Fork 0
Code Issues4 Pull Requests1 Releases121 Wiki Activity

rewrite almost done

Browse Source
This commit is contained in:
Robin Müller
2023-11-13 16:32:40 +01:00
parent 8714948788
commit e41e2e62e0
3 changed files with 182 additions and 16 deletions

180
linux/payload/FreshSupvHandler.cpp

@ -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,12 +923,12 @@ 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) {
return true;
// 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;
}