diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f680a2b..e46f6446 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -295,8 +295,10 @@ include(BuildType) set_build_type() set(FSFW_DEBUG_INFO 0) +set(Q7S_CHECK_FOR_ALREADY_RUNNING_IMG 0) if(RELEASE_BUILD MATCHES 0) set(FSFW_DEBUG_INFO 1) + set(Q7S_CHECK_FOR_ALREADY_RUNNING_IMG 1) endif() # Configuration files diff --git a/bsp_q7s/boardconfig/q7sConfig.h.in b/bsp_q7s/boardconfig/q7sConfig.h.in index ee9cd863..8fe0f658 100644 --- a/bsp_q7s/boardconfig/q7sConfig.h.in +++ b/bsp_q7s/boardconfig/q7sConfig.h.in @@ -17,7 +17,7 @@ /*******************************************************************/ // Probably better if this is disabled for mission code. Convenient for development -#define Q7S_CHECK_FOR_ALREADY_RUNNING_IMG 1 +#define Q7S_CHECK_FOR_ALREADY_RUNNING_IMG @Q7S_CHECK_FOR_ALREADY_RUNNING_IMG@ #define Q7S_SIMPLE_ADD_FILE_SYSTEM_TEST 0 diff --git a/bsp_q7s/core/WatchdogHandler.cpp b/bsp_q7s/core/WatchdogHandler.cpp index b5a9edc7..71569b65 100644 --- a/bsp_q7s/core/WatchdogHandler.cpp +++ b/bsp_q7s/core/WatchdogHandler.cpp @@ -68,7 +68,7 @@ ReturnValue_t WatchdogHandler::initialize(bool enableWatchdogFunction) { ReturnValue_t WatchdogHandler::performStartHandling() { char startBuf[2]; - size_t writeLen = 1; + ssize_t writeLen = 1; startBuf[0] = watchdog::first::START_CHAR; if (enableWatchFunction) { writeLen += 1; @@ -76,9 +76,11 @@ ReturnValue_t WatchdogHandler::performStartHandling() { } ssize_t writtenBytes = write(watchdogFifoFd, &startBuf, writeLen); if (writtenBytes < 0) { - sif::error << "Errors writing to watchdog FIFO, code " << errno << ": " << strerror(errno) - << std::endl; + sif::error << "WatchdogHandler: Errors writing to watchdog FIFO, code " << errno << ": " + << strerror(errno) << std::endl; return returnvalue::FAILED; + } else if (writtenBytes != writeLen) { + sif::warning << "WatchdogHandler: Not all bytes were written, possible error" << std::endl; } return returnvalue::OK; } diff --git a/bsp_q7s/main.cpp b/bsp_q7s/main.cpp index 56327005..d557cdb9 100644 --- a/bsp_q7s/main.cpp +++ b/bsp_q7s/main.cpp @@ -12,10 +12,10 @@ * @brief This is the main program for the target hardware. * @return */ -int main(void) { +int main(int argc, char* argv[]) { using namespace std; #if Q7S_SIMPLE_MODE == 0 - return obsw::obsw(); + return obsw::obsw(argc, argv); #else return simple::simple(); #endif diff --git a/bsp_q7s/obsw.cpp b/bsp_q7s/obsw.cpp index cd69bdf5..a0067574 100644 --- a/bsp_q7s/obsw.cpp +++ b/bsp_q7s/obsw.cpp @@ -19,7 +19,7 @@ #include "q7sConfig.h" #include "watchdog/definitions.h" -static int OBSW_ALREADY_RUNNING = -2; +static constexpr int OBSW_ALREADY_RUNNING = -2; #if OBSW_Q7S_EM == 0 static const char* DEV_STRING = "Xiphos Q7S FM"; #else @@ -28,7 +28,7 @@ static const char* DEV_STRING = "Xiphos Q7S EM"; WatchdogHandler WATCHDOG_HANDLER; -int obsw::obsw() { +int obsw::obsw(int argc, char* argv[]) { using namespace fsfw; std::cout << "-- EIVE OBSW --" << std::endl; std::cout << "-- Compiled for Linux (" << DEV_STRING << ") --" << std::endl; @@ -52,7 +52,8 @@ int obsw::obsw() { bootDelayHandling(); bool initWatchFunction = false; - if (std::filesystem::current_path() == "/usr/bin") { + std::string fullExecPath = argv[0]; + if (fullExecPath.find("/usr/bin") != std::string::npos) { initWatchFunction = true; } ReturnValue_t result = WATCHDOG_HANDLER.initialize(initWatchFunction); @@ -71,7 +72,7 @@ int obsw::obsw() { for (;;) { WATCHDOG_HANDLER.periodicOperation(); - TaskFactory::delayTask(1000); + TaskFactory::delayTask(2000); } return 0; } diff --git a/bsp_q7s/obsw.h b/bsp_q7s/obsw.h index 351925aa..1a6e4e05 100644 --- a/bsp_q7s/obsw.h +++ b/bsp_q7s/obsw.h @@ -3,7 +3,7 @@ namespace obsw { -int obsw(); +int obsw(int argc, char* argv[]); void bootDelayHandling(); void commandEiveSystemToSafe(); diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h index e3a05a56..2b1292b3 100644 --- a/common/config/eive/definitions.h +++ b/common/config/eive/definitions.h @@ -59,7 +59,7 @@ namespace spiSched { static constexpr uint32_t SCHED_BLOCK_1_SUS_READ_MS = 15; static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30; -static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42; +static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 43; static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45; static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 55; static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 105; diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp index 51618981..8a2f6b76 100644 --- a/linux/devices/ImtqPollingTask.cpp +++ b/linux/devices/ImtqPollingTask.cpp @@ -28,6 +28,8 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) { // Stopwatch watch; switch (currentRequest) { case imtq::RequestType::MEASURE_NO_ACTUATION: { + // Measured to take 24 ms for debug and release builds. + // Stopwatch watch; handleMeasureStep(); break; } @@ -47,6 +49,9 @@ void ImtqPollingTask::handleMeasureStep() { size_t replyLen = 0; uint8_t* replyPtr; ImtqRepliesDefault replies(replyBuf.data()); + // If some startup handling is added later, set configured after it was done once. + replies.setConfigured(); + // Can be used later to verify correct timing (e.g. all data has been read) clearReadFlagsDefault(replies); auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) { @@ -127,17 +132,12 @@ void ImtqPollingTask::handleMeasureStep() { if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { return; } + bool mgmMeasurementTooOld = false; // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably // have old data. Which can be really bad for ACS. And everything. if ((replyPtr[2] >> 7) == 0) { - sif::error << "IMTQ: MGM measurement too old" << std::endl; - TaskFactory::delayTask(2); - if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { - return; - } - if ((replyPtr[2] >> 7) == 0b1) { - replyPtr[0] = false; - } + replyPtr[0] = false; + mgmMeasurementTooOld = true; } cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA; @@ -149,7 +149,9 @@ void ImtqPollingTask::handleMeasureStep() { if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) { return; } - // sif::debug << "measure done" << std::endl; + if(mgmMeasurementTooOld) { + sif::error << "IMTQ: MGM measurement too old" << std::endl; + } return; } @@ -186,25 +188,22 @@ void ImtqPollingTask::handleActuateStep() { if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { return; } + bool measurementWasTooOld = false; // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably // have old data. Which can be really bad for ACS. And everything. if ((replyPtr[2] >> 7) == 0) { - sif::error << "IMTQ: MGM measurement too old" << std::endl; - TaskFactory::delayTask(2); - cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT; - if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) { - return; - } - if ((replyPtr[2] >> 7) == 0b1) { - replyPtr[0] = false; - } + measurementWasTooOld = true; + replyPtr[0] = false; } cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA; if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) { return; } - // sif::debug << "measure with torque done" << std::endl; + + if(measurementWasTooOld) { + sif::error << "IMTQ: MGM measurement too old" << std::endl; + } return; } @@ -223,7 +222,7 @@ ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) { ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { - const auto* imtqReq = reinterpret_cast(sendData); + const auto* imtqReq = reinterpret_cast(sendData); { MutexGuard mg(ipcLock); if (imtqReq->request == imtq::RequestType::ACTUATE) { diff --git a/linux/devices/ImtqPollingTask.h b/linux/devices/ImtqPollingTask.h index 5ae8f1f9..cb2d3882 100644 --- a/linux/devices/ImtqPollingTask.h +++ b/linux/devices/ImtqPollingTask.h @@ -33,13 +33,12 @@ class ImtqPollingTask : public SystemObject, address_t i2cAddr = 0; uint32_t currentIntegrationTimeMs = 10; // Required in addition to integration time, otherwise old data might be read. - static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 3; + static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 6; bool ignoreNextActuateRequest = false; imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; int16_t dipoles[3] = {}; uint16_t torqueDuration = 0; - // uint8_t startActuateRawBuf[3] = {}; std::array cmdBuf; std::array replyBuf; diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index b76f0575..30fd02e9 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -131,12 +131,21 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) { ImtqHandler::~ImtqHandler() = default; void ImtqHandler::doStartUp() { - updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE); - updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE); - if (goToNormalMode) { - setMode(MODE_NORMAL); - } else { - setMode(_MODE_TO_ON); + if (internalState != InternalState::STARTUP) { + commandExecuted = false; + updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE); + updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE); + internalState = InternalState::STARTUP; + } + if (internalState == InternalState::STARTUP) { + if (commandExecuted) { + if (goToNormalMode) { + setMode(MODE_NORMAL); + } else { + setMode(_MODE_TO_ON); + } + commandExecuted = false; + } } } @@ -145,6 +154,8 @@ void ImtqHandler::doShutDown() { updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE); specialRequestActive = false; firstReplyCycle = true; + internalState = InternalState::NONE; + commandExecuted = false; setMode(_MODE_POWER_DOWN); } @@ -162,8 +173,9 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { *id = imtq::cmdIds::REQUEST; request.request = imtq::RequestType::DO_NOTHING; request.specialRequest = imtq::SpecialRequest::NONE; + expectedReply = DeviceHandlerIF::NO_COMMAND_ID; rawPacket = reinterpret_cast(&request); - rawPacketLen = sizeof(ImtqRequest); + rawPacketLen = sizeof(imtq::Request); return returnvalue::OK; } } @@ -171,6 +183,10 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { } ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { + if (internalState == InternalState::STARTUP) { + *id = imtq::cmdIds::REQUEST; + return buildCommandFromCommand(*id, nullptr, 0); + } return NOTHING_TO_SEND; } @@ -183,7 +199,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma expectedReply = imtq::cmdIds::REPLY_NO_TORQUE; specialRequestActive = true; rawPacket = reinterpret_cast(&request); - rawPacketLen = sizeof(ImtqRequest); + rawPacketLen = sizeof(imtq::Request); }; switch (deviceCommand) { case (imtq::cmdIds::POS_X_SELF_TEST): { @@ -219,7 +235,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma request.specialRequest = imtq::SpecialRequest::NONE; expectedReply = imtq::cmdIds::REPLY_NO_TORQUE; rawPacket = reinterpret_cast(&request); - rawPacketLen = sizeof(ImtqRequest); + rawPacketLen = sizeof(imtq::Request); return returnvalue::OK; } case (imtq::cmdIds::START_ACTUATION_DIPOLE): { @@ -258,7 +274,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma torquer::TORQUEING = true; torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value); rawPacket = reinterpret_cast(&request); - rawPacketLen = sizeof(ImtqRequest); + rawPacketLen = sizeof(imtq::Request); return returnvalue::OK; } default: @@ -298,13 +314,20 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint ReturnValue_t result; ReturnValue_t status = returnvalue::OK; if (getMode() != MODE_NORMAL) { - // Ignore replies during transitions. + if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) { + ImtqRepliesDefault replies(packet); + if (replies.devWasConfigured() and internalState == InternalState::STARTUP) { + commandExecuted = true; + } + } return returnvalue::OK; } - // arrayprinter::print(packet, ImtqReplies::BASE_LEN); if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) { // sif::debug << "handle measure" << std::endl; ImtqRepliesDefault replies(packet); + if (replies.devWasConfigured() and internalState == InternalState::STARTUP) { + commandExecuted = true; + } if (specialRequestActive) { if (replies.wasSpecialRequestRead()) { uint8_t* specialRequest = replies.getSpecialRequest(); @@ -347,7 +370,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint } if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) { - sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; + sif::warning << "IMTQ: Possible timing issue, raw MGM measurement was not read" << std::endl; } uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement(); result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement); @@ -358,7 +381,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint } if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) { - sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl; + sif::warning << "IMTQ: Possible timing issue, calib MGM measurement was not read" + << std::endl; } uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement(); result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement); diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h index d579f7f3..e337f40c 100644 --- a/mission/devices/ImtqHandler.h +++ b/mission/devices/ImtqHandler.h @@ -83,6 +83,11 @@ class ImtqHandler : public DeviceHandlerBase { //! link between IMTQ and OBC. static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW); + enum class InternalState { NONE, STARTUP, SHUTDOWN } internalState = InternalState::NONE; + bool commandExecuted = false; + + imtq::Request request{}; + imtq::StatusDataset statusSet; imtq::DipoleActuationSet dipoleSet; imtq::RawMtmMeasurementNoTorque rawMtmNoTorque; @@ -123,8 +128,7 @@ class ImtqHandler : public DeviceHandlerBase { power::Switch_t switcher = power::NO_SWITCH; - ImtqRequest request{}; - DeviceCommandId_t expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE; + DeviceCommandId_t expectedReply = DeviceHandlerIF::NO_COMMAND_ID; bool goToNormalMode = false; bool debugMode = false; bool specialRequestActive = false; diff --git a/mission/devices/devicedefinitions/imtqHelpers.h b/mission/devices/devicedefinitions/imtqHelpers.h index a066216d..87b69f8b 100644 --- a/mission/devices/devicedefinitions/imtqHelpers.h +++ b/mission/devices/devicedefinitions/imtqHelpers.h @@ -10,18 +10,6 @@ class ImtqHandler; namespace imtq { -enum ComStep : uint8_t { - DHB_OP = 0, - START_MEASURE_SEND = 1, - START_MEASURE_GET = 2, - READ_MEASURE_SEND = 3, - READ_MEASURE_GET = 4, - START_ACTUATE_SEND = 5, - START_ACTUATE_GET = 6, - READ_ACTUATE_SEND = 7, - READ_ACTUATE_GET = 8, -}; - enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING }; enum class SpecialRequest : uint8_t { @@ -35,6 +23,26 @@ enum class SpecialRequest : uint8_t { GET_SELF_TEST_RESULT = 7 }; +struct Request { + imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION; + imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; + uint8_t integrationTimeSel = 3; + int16_t dipoles[3]{}; + uint16_t torqueDuration = 0; +}; + +enum ComStep : uint8_t { + DHB_OP = 0, + START_MEASURE_SEND = 1, + START_MEASURE_GET = 2, + READ_MEASURE_SEND = 3, + READ_MEASURE_GET = 4, + START_ACTUATE_SEND = 5, + START_ACTUATE_GET = 6, + READ_ACTUATE_SEND = 7, + READ_ACTUATE_GET = 8, +}; + static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER; static constexpr ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0); @@ -1124,25 +1132,21 @@ class NegZSelfTestSet : public StaticLocalDataSet { } // namespace imtq -struct ImtqRequest { - imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION; - imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE; - int16_t dipoles[3]{}; - uint16_t torqueDuration = 0; -}; - struct ImtqRepliesDefault { friend class ImtqPollingTask; public: static constexpr size_t BASE_LEN = - imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 + + 1 + imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 + +imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1 + imtq::replySize::ENG_HK_DATA_REPLY + 1 + imtq::replySize::CAL_MTM_MEASUREMENT + 1; ImtqRepliesDefault(const uint8_t* rawData) : rawData(const_cast(rawData)) { initPointers(); } + void setConfigured() { rawData[0] = true; } + bool devWasConfigured() const { return rawData[0]; } + uint8_t* getCalibMgmMeasurement() const { return calibMgmMeasurement + 1; } bool wasCalibMgmMeasurementRead() const { return calibMgmMeasurement[0]; }; @@ -1164,7 +1168,7 @@ struct ImtqRepliesDefault { private: void initPointers() { - swReset = rawData; + swReset = rawData + 1; systemState = swReset + imtq::replySize::DEFAULT_MIN_LEN + 1; startMtmMeasurement = systemState + imtq::replySize::SYSTEM_STATE + 1; rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1; @@ -1172,6 +1176,7 @@ struct ImtqRepliesDefault { calibMgmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1; specialRequestReply = calibMgmMeasurement + imtq::replySize::CAL_MTM_MEASUREMENT + 1; } + uint8_t* rawData; uint8_t* swReset; uint8_t* systemState; diff --git a/watchdog/Watchdog.cpp b/watchdog/Watchdog.cpp index eee4f25a..3518192e 100644 --- a/watchdog/Watchdog.cpp +++ b/watchdog/Watchdog.cpp @@ -44,11 +44,30 @@ int WatchdogTask::performOperation() { << strerror(errno) << std::endl; return -1; } + // Clear FIFO by reading until it is empty. + while (true) { + ssize_t readBytes = read(fd, buf.data(), buf.size()); + if (readBytes < 0) { + std::cerr << "Read error of FIFO: " << strerror(errno) << std::endl; + } else if (readBytes == 0) { + break; + } + } state = States::NOT_STARTED; + bool breakOuter = false; while (true) { - WatchdogTask::LoopResult loopResult = watchdogLoop(); - if (not stateMachine(loopResult)) { + watchdogLoop(); + while (not resultQueue.empty()) { + auto nextRequest = resultQueue.front(); + if (not stateMachine(nextRequest)) { + breakOuter = true; + resultQueue.pop(); + break; + } + resultQueue.pop(); + } + if (breakOuter) { break; } } @@ -60,7 +79,7 @@ int WatchdogTask::performOperation() { return 0; } -WatchdogTask::LoopResult WatchdogTask::watchdogLoop() { +void WatchdogTask::watchdogLoop() { using namespace std::chrono_literals; struct pollfd waiter = {}; waiter.fd = fd; @@ -69,10 +88,12 @@ WatchdogTask::LoopResult WatchdogTask::watchdogLoop() { // Only poll one file descriptor with timeout switch (poll(&waiter, 1, watchdog::TIMEOUT_MS)) { case (0): { - return LoopResult::TIMEOUT; + resultQueue.push(LoopResult::TIMEOUT); + return; } case (1): { - return pollEvent(waiter); + pollEvent(waiter); + return; } default: { std::cerr << "Unknown poll error at " << watchdog::FIFO_NAME << ", error " << errno << ": " @@ -80,50 +101,52 @@ WatchdogTask::LoopResult WatchdogTask::watchdogLoop() { break; } } - return LoopResult::OK; } -WatchdogTask::LoopResult WatchdogTask::pollEvent(struct pollfd& waiter) { +void WatchdogTask::pollEvent(struct pollfd& waiter) { if (waiter.revents & POLLIN) { ssize_t readLen = read(fd, buf.data(), buf.size()); +#if WATCHDOG_VERBOSE_LEVEL == 2 + std::cout << "Read " << readLen << " byte(s) on the pipe " << watchdog::FIFO_NAME << std::endl; +#endif if (readLen < 0) { std::cerr << "Read error on pipe " << watchdog::FIFO_NAME << ", error " << errno << ": " << strerror(errno) << std::endl; - return LoopResult::OK; - } -#if WATCHDOG_VERBOSE_LEVEL == 2 - std::cout << "Read " << readLen << " byte(s) on the pipe " << FIFO_NAME << std::endl; -#endif - else if (readLen >= 1) { - return parseCommand(readLen); + resultQueue.push(LoopResult::OK); + } else if (readLen >= 1) { + parseCommands(readLen); } } else if (waiter.revents & POLLERR) { std::cerr << "Poll error error on pipe " << watchdog::FIFO_NAME << std::endl; - return LoopResult::FAULT; + resultQueue.push(LoopResult::FAULT); } else if (waiter.revents & POLLHUP) { // Writer closed its end - return LoopResult::HUNG_UP; + resultQueue.push(LoopResult::HUNG_UP); } - return LoopResult::FAULT; } -WatchdogTask::LoopResult WatchdogTask::parseCommand(ssize_t readLen) { - char readChar = buf[0]; - // Cancel request - if (readChar == watchdog::first::CANCEL_CHAR) { - return LoopResult::CANCEL_REQ; - } else if (readChar == watchdog::first::SUSPEND_CHAR) { - // Suspend request - return LoopResult::SUSPEND_REQ; - } else if (readChar == watchdog::first::START_CHAR) { - if (readLen == 2 and static_cast(buf[1]) == watchdog::second::WATCH_FLAG) { - return LoopResult::START_WITH_WATCH_REQ; +void WatchdogTask::parseCommands(ssize_t readLen) { + for (ssize_t idx = 0; idx < readLen; idx++) { + char nextChar = buf[idx]; + // Cancel request + if (nextChar == watchdog::first::CANCEL_CHAR) { + resultQueue.push(LoopResult::CANCEL_REQ); + } else if (nextChar == watchdog::first::SUSPEND_CHAR) { + // Suspend request + resultQueue.push(LoopResult::SUSPEND_REQ); + } else if (nextChar == watchdog::first::START_CHAR) { + if (idx < readLen - 1 and static_cast(buf[idx + 1]) == watchdog::second::WATCH_FLAG) { + resultQueue.push(LoopResult::START_WITH_WATCH_REQ); + idx++; + continue; + } + resultQueue.push(LoopResult::START_REQ); + } else if (nextChar == watchdog::first::IDLE_CHAR) { + resultQueue.push(LoopResult::OK); } - return LoopResult::START_REQ; } // Everything else: All working as expected - return LoopResult::OK; } int WatchdogTask::performRunningOperation() { @@ -167,11 +190,12 @@ int WatchdogTask::performNotRunningOperation(LoopResult type) { } if (not notRunningStart.has_value()) { - notRunningStart = std::chrono::system_clock::now(); + notRunningStart = std::chrono::steady_clock::now(); } if (obswRunning) { #if WATCHDOG_CREATE_FILE_IF_RUNNING == 1 + std::cout << "Removing " << watchdog::RUNNING_FILE_NAME << std::endl; if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME)) { int result = std::remove(watchdog::RUNNING_FILE_NAME.c_str()); if (result != 0) { @@ -184,7 +208,7 @@ int WatchdogTask::performNotRunningOperation(LoopResult type) { } if (watchingObsw) { - auto timeNotRunning = std::chrono::system_clock::now() - notRunningStart.value(); + auto timeNotRunning = std::chrono::steady_clock::now() - notRunningStart.value(); if (std::chrono::duration_cast(timeNotRunning).count() > watchdog::MAX_NOT_RUNNING_MS) { std::cout << "Restarting OBSW with systemctl" << std::endl; @@ -269,7 +293,7 @@ bool WatchdogTask::stateMachine(LoopResult loopResult) { sleep = true; } if (sleep) { - std::this_thread::sleep_for(1000ms); + std::this_thread::sleep_for(500ms); } return true; } diff --git a/watchdog/Watchdog.h b/watchdog/Watchdog.h index 524675a9..340a9f9d 100644 --- a/watchdog/Watchdog.h +++ b/watchdog/Watchdog.h @@ -5,6 +5,7 @@ #include #include #include +#include #include class WatchdogTask { @@ -35,15 +36,17 @@ class WatchdogTask { bool watchingObsw = false; bool printNotRunningLatch = false; std::array buf; - std::optional> notRunningStart; + std::queue resultQueue; + + std::optional> notRunningStart; States state = States::NOT_STARTED; // Primary loop. Takes care of delaying, and reading from the communication pipe and translating // messages to loop results. - LoopResult watchdogLoop(); + void watchdogLoop(); bool stateMachine(LoopResult result); - LoopResult pollEvent(struct pollfd& waiter); - LoopResult parseCommand(ssize_t readLen); + void pollEvent(struct pollfd& waiter); + void parseCommands(ssize_t readLen); int performRunningOperation(); int performNotRunningOperation(LoopResult type); diff --git a/watchdog/main.cpp b/watchdog/main.cpp index e137d261..1ee6aea0 100644 --- a/watchdog/main.cpp +++ b/watchdog/main.cpp @@ -1,6 +1,10 @@ + +#include #include +#include #include "Watchdog.h" +#include "definitions.h" /** * @brief This watchdog application uses a FIFO to check whether the OBSW is still running. @@ -8,6 +12,13 @@ */ int main() { std::cout << "Starting OBSW watchdog" << std::endl; + if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME)) { + std::cout << "Removing " << watchdog::RUNNING_FILE_NAME << std::endl; + int result = std::remove(watchdog::RUNNING_FILE_NAME.c_str()); + if (result != 0) { + std::cerr << "file removal failure" << std::endl; + } + } try { WatchdogTask watchdogTask; int result = watchdogTask.performOperation();